#include "rasterize.hh"
#include "math.hh"
#include <array>
#include <vector>
#include <map>
#include <cmath>
#include <cstdint>

class Slope
{
    float begin, step;
public:
    Slope() {}
    Slope(float from, float to, int num_steps)
    {
        float inv_step = 1.f / num_steps;
        begin = from;                   // Begin here
        step  = (to - from) * inv_step; // Stepsize = (end-begin) / num_steps
    }
    float get() const { return begin; }
    void advance()    { begin += step; }
};

template<typename PlotFunc, typename T, std::size_t Size = std::tuple_size_v<T>>
void DrawTexturedPolygon(
    T p0,
    T p1,
    T p2,
    PlotFunc&& Plot)
{
    using SlopeData = std::array<Slope, Size-1>; // All input units except y

    RasterizeTriangle(
        &p0, &p1, &p2,
        // GetXY: Retrieve std::tuple<int,int> or std::array<int,2> from a PointType
        [&](const auto& p) { return std::tuple{ int(std::get<0>(p)), int(std::get<1>(p)) }; },
        // Slope generator
        [&](const T* from, const T* to, int num_steps)
        {
            SlopeData result;
            // Number of steps = number of scanlines
            // Retrieve X coordinates for begin and end
            result[0] = Slope( std::get<0>(*from), std::get<0>(*to), num_steps );
            // Take the rest of the props
            [&]<std::size_t...p>(std::index_sequence<p...>)
            {
                // Retrieve begin to end for each prop. The +2 skips x and y.
                ((result[p+1] = Slope( std::get<p+2>(*from), std::get<p+2>(*to), num_steps )), ...);
            }
            (std::make_index_sequence<Size-2>{});
            return result;
        },
        // Scanline function
        [&](int y, SlopeData& left, SlopeData& right)
        {
            int x = left[0].get(), endx = right[0].get();

            // Number of steps = number of pixels on this scanline = endx-x
            std::array<Slope, Size-2> props;
            for(unsigned p=0; p<Size-2; ++p)
            {
                props[p] = Slope( left[p+1].get(), right[p+1].get(), endx-x );
            }

            for(; x < endx; ++x) // Render each pixel
            {
                std::apply([&](auto... args) { Plot(x,y, args.get()...); }, props);

                // After each pixel, update the props by their step-sizes
                for(auto& prop: props) prop.advance();
            }

            // After the scanline is drawn, update the X coordinate and props on both sides
            for(auto& slope: left) slope.advance();
            for(auto& slope: right) slope.advance();
        });
}

#include <SDL.h>

int main()
{
    const int W = 424, H = 240;
    // Create a screen.
    SDL_Window* window = SDL_CreateWindow("Chip8",
        SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, W*4,H*4, SDL_WINDOW_RESIZABLE);
    SDL_Renderer* renderer = SDL_CreateRenderer(window, -1, 0);
    SDL_Texture* texture = SDL_CreateTexture(renderer,
        SDL_PIXELFORMAT_ARGB8888, SDL_TEXTUREACCESS_STREAMING, W,H);

    const int txW = 256, txH = 256;
    unsigned bitmap[txW*txH];

    for(unsigned y=0; y<txH; ++y)
        for(unsigned x=0; x<txW; ++x)
        {
            int l = (0x1FF >> std::min({x,y,txW-1-x,txH-1-y,31u}));
            int d = std::min(50,std::max<int>(0,255-50*std::pow(std::hypot(x/float(txW/2)-1.f, y/float(txH/2)-1.f)*4,2.f)));
            int r = (~x & ~y)&255, g = (x & ~y)&255, b = (~x & y)&255;
            bitmap[y*txW+x] = std::min(std::max(r-d,l),255)*65536 + std::min(std::max(g-d,l),255)*256 + std::min(std::max(b-d,l),255);
        }

    // Create a box with five walls using two triangles each.
    // This function is copied verbatim from my 75000 subscribers demo.
    std::vector<float> tri;
    auto addcuboid = [&](unsigned mask,
                         std::array<float,2> x, std::array<float,2> z, std::array<float,2> y,
                         std::array<float,3> c, std::array<float,3> u, std::array<float,3> v)
    {
        auto ext = [](auto m,unsigned n,unsigned b=1) { return (m >> (n*b)) & ~(~0u << b); }; // extracts bits
        // Generates: For six vertices, color(rgb), coordinate(xyz) and texture coord(uv).
        std::array p{&c[0],&c[0],&c[0], &x[0],&y[0],&z[0], &u[0],&v[0]};
        // capflag(1 bit), mask(3 bits), X(4 bits), Y(4 bits), Z(4 bits), U(4 bits), V(4 bits)
        for(unsigned m: std::array{0x960339,0xA9F339,0x436039,0x4C6F39,0x406C39,0x4F6339}) // bottom, top, four sides
            if(std::uint64_t s = (m>>23) * 0b11'000'111 * (~0llu/255); mask & m)
                for(unsigned n = 0; n < 6*8; ++n)
                    tri.push_back( p[n%8][ext(m, ext(012345444u, n%8, 3)*4 - ext(0123341u, n/8, 3)) << ext(s,n)] );
        // 123341 = order of vertices in two triangles; 12345444 = nibble indexes in "m" for each of 8 values
    };
    addcuboid(7<<20, {-10,10}, {-10,10}, {-10,10}, { 1, 1, 1}, {0,1,1},  {0,1,1});
    tri.erase(tri.begin() + 2*6*8, tri.begin() + 3*6*8); // Erase the third wall to make the box open

    std::tuple r{0.f, 0.f, .2f};             // Rotation momentum vector (nonzero indicates view is still rotating)
    std::tuple m{0.f, 0.f, 0.f};             // Movement momentum vector (nonzero indicates player is still moving)
    std::tuple l{6.9f, 0.146f, -21.f};       // Camera location (X,Y,Z) coordinate
    float aa=0.094,ab=-0.11,ac=-0.021,ad=1;  // View rotation quaternion
    float tform[12]{};                       // View rotation matrix (calculated from the quaternion)
    // Generate the perspective projection and unprojection functions
    auto [PerspectiveProject, PerspectiveUnproject] = [](int W,int H, float fov)
    {
        std::tuple center{ W*.5f, H*.5f }, size = center, aspect{ 1.f, W*1.f/H };
        auto scale = Mul(size, aspect, 1.f / std::tan(fov/2.f * (std::atan(1.f)/45.f)));
        // Converting 3D x,y,z into 2D X & Y follows this formula (rectilinear projection):
        //    X = xcenter + x * hscale / z
        //    Y = ycenter + y * vscale / z
        return std::pair{ [=](const auto& point) // PerspectiveProject function
        {
            return Mul<std::plus<void>>(Mul(scale, point, 1 / std::get<2>(point)), center);
        },
        // Doing the same in reverse, getting 3D x,y,z from 2D X & Y,
        // can be done as follows, but requires that we already know z:
        //    x = (X - xcenter) * z / hscale
        //    y = (Y - ycenter) * z / vscale
        [=](const auto& point, float z) // PerspectiveUnproject function
        {
            return std::tuple_cat(Mul<std::divides<void>>(Mul(Mul<std::minus<void>>(point, center), z), scale), std::tuple{z});
        } };
    }(W,H, 120.f /* degrees */);
    // Main loop
    for(std::map<int,bool> keys; !keys[SDLK_ESCAPE]; )
    {
        // Process events.
        SDL_Event ev;
        while(SDL_PollEvent(&ev))
            switch(ev.type)
            {
                case SDL_QUIT: keys[SDLK_ESCAPE] = true; break;
                case SDL_KEYDOWN: keys[ev.key.keysym.sym] = true; break;
                case SDL_KEYUP:   keys[ev.key.keysym.sym] = false; break;
            }
        // The input scheme is the same as in Descent, the game by Parallax Interactive.
        // Mouse input is not handled for now.
        bool up    = keys[SDLK_UP]   || keys[SDLK_KP_8];
        bool down  = keys[SDLK_DOWN] || keys[SDLK_KP_2],     alt   = keys[SDLK_LALT]|| keys[SDLK_RALT];
        bool left  = keys[SDLK_LEFT] || keys[SDLK_KP_4],     rleft = keys[SDLK_q]   || keys[SDLK_KP_7];
        bool right = keys[SDLK_RIGHT]|| keys[SDLK_KP_6],     rright= keys[SDLK_e]   || keys[SDLK_KP_9];
        bool fwd   = keys[SDLK_a], sup   = keys[SDLK_KP_MINUS], sleft = keys[SDLK_KP_1];
        bool back  = keys[SDLK_z], sdown = keys[SDLK_KP_PLUS],  sright= keys[SDLK_KP_3];
        // Change the rotation momentum vector (r) with hysteresis: newvalue = oldvalue*(1-eagerness) + input*eagerness
        r = Mul<std::plus<void>>(Mul(r, .9f), Mul({0.f+(up     - down) * !alt,
                                                   0.f+(right  - left) * !alt,
                                                   0.f+(rright - rleft)}, .1f));
        if(float rlen = Length(r); rlen > 1e-3f) // Still rotating?
        {
            // Create rotation change quaternion (q) relative to the direction that the camera looks
            // by multiplying the rotation momentum vector (r) with the current rotation matrix.
            float theta = rlen*.03f, c = std::cos(theta*.5f), s = std::sin(theta*.5f)/rlen;
            std::tuple q{ c,
                          s * Dot(r, {tform[0],tform[4],tform[8]}),
                          s * Dot(r, {tform[1],tform[5],tform[9]}),
                          s * Dot(r, {tform[2],tform[6],tform[10]}) };
            // Update the rotation quaternion (a) by multiplying it by the rotation change quaternion (q):
            std::tie(aa,ab,ac,ad) = Normalized(std::tuple{ Dot(q, {aa,-ab,-ac,-ad}),
                                                           Dot(q, {ab, aa,-ad, ac}),
                                                           Dot(q, {ac, ad, aa,-ab}),
                                                           Dot(q, {ad,-ac, ab, aa})});
            // Convert the rotation quaternion (a) into rotation matrix using formula from Wikipedia:
            //auto a = Mul(std::array{aa,aa,aa,ab,ab,ab,ac,ac,ad}, {ab,ac,ad,ab,ac,ad,ac,ac,ad});
            //auto b = Mul({a[0],a[1],a[2]},-1);
            //std::array slices[]{-aa*ab,-aa*ac,-aa*ad, aa*ab, aa*ac, aa*ad, ab*ab, ab*ac, ab*ad, ac*ac, ac*ac, ad*ad };
            //                    0      1      2       3      4      5      6      7      8      9      10     11
            tform[0] = 1-2*(ac*ac+ad*ad); tform[4] =   2*(ab*ac+aa*ad); tform[8] =   2*(ab*ad-aa*ac);
            tform[1] =   2*(ab*ac-aa*ad); tform[5] = 1-2*(ab*ab+ad*ad); tform[9] =   2*(ac*ad+aa*ab);
            tform[2] =   2*(ab*ad+aa*ac); tform[6] =   2*(ac*ad-aa*ab); tform[10]= 1-2*(ab*ab+ac*ac);
        }
        // Camera movement vector
        std::array M{ 0.f+((sleft || (alt && left)) - (sright || (alt && right))),
                      0.f+((sdown || (alt && down)) - (sup    || (alt && up))),
                      0.f+(fwd - back) };
        float mlen = 2*Length(M); if(mlen < 1e-3f) mlen = 1;
        // Multiply with rotation matrix (tform) and apply with hysteresis to movement momentum vector (m).
        m = Mul<std::plus<void>>(Mul(m, .9f), Mul({Dot(M, {tform[0],tform[4],tform[8]}),
                                                   Dot(M, {tform[1],tform[5],tform[9]}),
                                                   Dot(M, {tform[2],tform[6],tform[10]})}, .1f/mlen));
        // Add the movement momentum vector (m) to the camera position (l), thereby moving the camera
        l = Mul<std::plus<void>>(l, m);

        // Render graphics
        Uint32 pixels[W*H]={};
        for(unsigned p=0; p<tri.size(); p+=24)
        {
            // Translate, rotate, and perspective project the triangles
            auto f = [&](unsigned p)
            {
                auto xyz = Mul<std::minus<void>>({tri[p+3],tri[p+4],tri[p+5]}, l);
                std::array r{-Dot(xyz, {tform[0],tform[1],tform[2]}),
                              Dot(xyz, {tform[4],tform[5],tform[6]}),
                              Dot(xyz, {tform[8],tform[9],tform[10]}) };
                auto [vx,vy] = PerspectiveProject(r);
                return std::tuple{ vx,vy, r[2], tri[p+6]*txW,tri[p+7]*txH };
            };
            DrawTexturedPolygon(
                f(p+0), f(p+8), f(p+16),
                [&](int x,int y, float, int u,int v)
                {
                    if(y>=0 && x>=0 && y<H && x<W)
                    {
                        pixels[y*W+x] = bitmap[(u%txW)*txW + v%txH];
                    }
                });
        }
        SDL_UpdateTexture(texture, nullptr, pixels, 4*W);
        SDL_RenderCopy(renderer, texture, nullptr, nullptr);
        SDL_RenderPresent(renderer);

        SDL_Delay(1000/60);
    }
}