#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) requires requires { typename std::tuple_size<T>; }
{
    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 );

            // For the Z coordinate, use the inverted value.
            float zbegin = 1.f / std::get<2>(*from), zend = 1.f / std::get<2>(*to);
            result[1] = Slope( zbegin, zend, num_steps );

            // Take the rest of the props, and use the inverted Z coordinate to iterate them through.
            [&]<std::size_t...p>(std::index_sequence<p...>)
            {
                // Retrieve begin to end for each prop. The +3 skips x, y, and z.
                ((result[p+2] = Slope( std::get<p+3>(*from) * zbegin, std::get<p+3>(*to) * zend, num_steps )), ...);
            }
            (std::make_index_sequence<Size-3>{});
            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
            {
                float z = 1.f / props[0].get(); // Invert the inverted z-coordinate, producing real z coordinate
                std::apply([&](auto... args) { Plot(x,y, z, (args.get() * z) ...); }, 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

    float rx=0,ry=0,rz=.2; // Rotation delta vector (nonzero indicates view is still rotating)
    float mx=0,my=0,mz=0;  // Movement delta vector (nonzero indicates player is still moving)
    float lx=6.89334,ly=0.146147,lz=-21.0085;                 // Player location (X,Y,Z) coordinate
    float aa=0.093881,ab=-0.112375,ac=-0.0209176,ad=0.989;    // View rotation quaternion
    float tform[16]{1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};      // View rotation matrix
    // Field of vision scalar, and the perspective projection constants derived from it and from the screen resolution
    float fov = 120, cent[2]{ W/2, H/2 }, asp[2] = { 1, W*1./H }, sf = 1.f / std::tan(fov/2*3.14159/180), scale[2]{ cent[0]*sf*asp[0], cent[1]*sf*asp[1] };
    // Converting 3D x,y,z into 2D X & Y follows this formula:
    //    X = xcenter + x * hscale / z
    //    Y = ycenter + y * vscale / z
    auto PerspectiveProject = [&](const auto& point)
    {
        return std::array{ cent[0] + std::get<0>(point) * scale[0] / std::get<2>(point),
                           cent[1] + std::get<1>(point) * scale[1] / std::get<2>(point) };
    };
    // 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];
        // Apply rotation delta with hysteresis: newvalue = input*eagerness + oldvalue*(1-eagerness)
        rx = rx*.9f + .1f*(up     - down) * !alt;
        ry = ry*.9f + .1f*(right  - left) * !alt;
        rz = rz*.9f + .1f*(rright - rleft);
        if(float rlen = std::sqrt(rx*rx + ry*ry + rz*rz); rlen > 1e-3f) // Still rotating?
        {
            // Create rotation quaternion (q), relative to the current angle that the player is looking towards.
            float theta = rlen*.03f, c = std::cos(theta*.5f), s = std::sin(theta*.5f)/rlen;
            auto [qa,qb,qc,qd] = std::tuple{ c,
                                             s*(tform[0]*rx + tform[1]*ry + tform[2]*rz),
                                             s*(tform[4]*rx + tform[5]*ry + tform[6]*rz),
                                             s*(tform[8]*rx + tform[9]*ry + tform[10]*rz) };
            // Update player angle (a) by multiplying it by the rotation quaternion (r)
            std::tie(aa,ab,ac,ad) = Normalized(std::tuple{ qa*aa - qb*ab - qc*ac - qd*ad,
                                                           qb*aa + qa*ab + qd*ac - qc*ad,
                                                           qc*aa - qd*ab + qa*ac + qb*ad,
                                                           qd*aa + qc*ab - qb*ac + qa*ad });
            // Recalculate the rotation matrix from the new player angle (a).
            tform[0] = 1-2*(ac*ac+ad*ad); tform[1] =   2*(ab*ac+aa*ad); tform[2] =   2*(ab*ad-aa*ac);
            tform[4] =   2*(ab*ac-aa*ad); tform[5] = 1-2*(ab*ab+ad*ad); tform[6] =   2*(ac*ad+aa*ab);
            tform[8] =   2*(ab*ad+aa*ac); tform[9] =   2*(ac*ad-aa*ab); tform[10]= 1-2*(ab*ab+ac*ac);
        }
        // Apply player movement delta with hysteresis
        float Mx = (sleft || (alt && left)) - (sright || (alt && right));
        float My = (sdown || (alt && down)) - (sup    || (alt && up));
        float Mz = fwd - back;
        float mlen = std::sqrt(Mx*Mx + My*My + Mz*Mz)/0.5; if(mlen < 1e-3f) mlen = 1;
        // The new movement is relative to the angle that player is looking towards.
        mx = mx*.9f + .1f*(tform[0]*Mx + tform[1]*My + tform[2]*Mz)/mlen;
        my = my*.9f + .1f*(tform[4]*Mx + tform[5]*My + tform[6]*Mz)/mlen;
        mz = mz*.9f + .1f*(tform[8]*Mx + tform[9]*My + tform[10]*Mz)/mlen;
        // Update player position (l) by the movement vector (m)
        lx += mx; ly += my; lz += mz;
/*
        printf(R"(
    float lx=%g,ly=%g,lz=%g;                // Player location (X,Y,Z) coordinate
    float aa=%g,ab=%g,ac=%g,ad=%g; // View rotation quaternion
)", lx*1.,ly*1.,lz*1.,aa*1.,ab*1.,ac*1.,ad*1.);*/

        // Render graphics
        Uint32 pixels[W*H]={};
        float  zbuffer[W*H];
        for(auto& z: zbuffer) z = 1e38f;
        for(unsigned p=0; p<tri.size(); p+=24)
        {
            // Translate, rotate, and perspective project the triangles
            auto f = [&](unsigned p)
            {
                float x = tri[p+3] - lx, y = tri[p+4] - ly, z = tri[p+5] - lz;
                float rx = x * tform[0] + y*tform[4] + z*tform[8];
                float ry = x * tform[1] + y*tform[5] + z*tform[9];
                float rz = x * tform[2] + y*tform[6] + z*tform[10];
                auto [vx,vy] = PerspectiveProject( std::tuple{-rx,ry,rz} );
                return std::tuple{ vx,vy, rz, tri[p+6]*txW,tri[p+7]*txH };
            };
            DrawTexturedPolygon(
                f(p+0), f(p+8), f(p+16),
                [&](int x,int y,float z, float, float u,float v)
                {
                    if(y>=0 && x>=0 && y<H && x<W && z<zbuffer[y*W+x])
                    {
                        zbuffer[y*W+x] = z;
                        auto decompose = [](unsigned rgb)
                        {
                            return std::array{ int(rgb>>16)&0xFF, int(rgb>>8)&0xFF, int(rgb)&0xFF };
                        };
                        auto mix = [](float f, auto a, auto b)
                        {
                            return a + (b-a)*f;
                            //return std::array{ a[0] + f*(b[0]-a[0]),  a[1] + f*(b[1]-a[1]),  a[2] + f*(b[2]-a[2]) };
                        };
                        auto compose = [](std::array<float,3> rgb)
                        {
                            return Sum(std::array{65536,256,1} * std::array{(int)rgb[0],(int)rgb[1],(int)rgb[2]});
                            //return (unsigned(rgb[0])<<16) + (unsigned(rgb[1])<<8) + unsigned(rgb[2]);
                        };
                        unsigned u0 = u, u1 = u0+1; float uf = u-u0;
                        unsigned v0 = v, v1 = v0+1; float vf = v-v0;
#if 0
                        auto u0v0 = decompose(bitmap[(u0%txW)*txW + v0%txH]);
                        auto u1v0 = decompose(bitmap[(u1%txW)*txW + v0%txH]);
                        auto u0v1 = decompose(bitmap[(u0%txW)*txW + v1%txH]);
                        auto u1v1 = decompose(bitmap[(u1%txW)*txW + v1%txH]);
                        auto mix0 = mix(u0v0, u0v1, vf);
                        auto mix1 = mix(u1v0, u1v1, vf);
                        pixels[y*W+x] = compose(mix(mix0, mix1, uf));
#else
                        pixels[y*W+x] = compose(mix(uf, mix(vf, decompose(bitmap[(u0%txW)*txW + v0%txH]),
                                                                decompose(bitmap[(u0%txW)*txW + v1%txH])),
                                                        mix(vf, decompose(bitmap[(u1%txW)*txW + v0%txH]),
                                                                decompose(bitmap[(u1%txW)*txW + v1%txH]))));
#endif
                    }
                });
        }
        SDL_UpdateTexture(texture, nullptr, pixels, 4*W);
        SDL_RenderCopy(renderer, texture, nullptr, nullptr);
        SDL_RenderPresent(renderer);

        SDL_Delay(1000/60);
    }
}