extern TFT_eSPI tft;
extern WiFiUDP udp;

const float display_width = 320;
const float display_height = 480;
const int frame_budget_ms = 32; // target 30 fps
const int pi = 3.14159;

struct vector3 {
    float x, y, z;
};

struct triangle {
    vector3 points[3];
};

struct mesh {
    vector<triangle> tris = {};
};

struct matrix4x4 {
    float m[4][4] = { { 0 } };
};

class renderer {

    private:
        mesh mesh_cube;
        matrix4x4 matrix_projection;
        String current_mesh_name = "Default";
        vector<triangle> last_drawn_tris = {};
        vector3 camera;

        unsigned long next_signal_send = millis() + 1000;
        float f_near = 0.1;
        float f_far = 1000;
        float fov = 90;
        float aspect_ratio = display_height / display_width;
        float f_fov_rad = 1 / tanf(fov * 0.5 / 180 * pi);
        float f_theta = 0;

        void multiply_matrix_vector(vector3 &i, vector3 &o, matrix4x4 &m) {
            o.x = i.x * m.m[0][0] + i.y * m.m[1][0] + i.z * m.m[2][0] + m.m[3][0];
            o.y = i.x * m.m[0][1] + i.y * m.m[1][1] + i.z * m.m[2][1] + m.m[3][1];
            o.z = i.x * m.m[0][2] + i.y * m.m[1][2] + i.z * m.m[2][2] + m.m[3][2];
            float w = i.x * m.m[0][3] + i.y * m.m[1][3] + i.z * m.m[2][3] + m.m[3][3];

            if (w != 0.0f)
            {
                o.x /= w;
                o.y /= w;
                o.z /= w;
            }
        }

        String split_string(String data, char separator, int index)
        {
            int found = 0;
            int strIndex[] = {0, -1};
            int maxIndex = data.length()-1;

            for(int i=0; i<=maxIndex && found<=index; i++){
                if(data.charAt(i)==separator || i==maxIndex){
                    found++;
                    strIndex[0] = strIndex[1]+1;
                    strIndex[1] = (i == maxIndex) ? i+1 : i;
                }
            }

            return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
        }

    public:
        void setup() {

            mesh_cube.tris = {

                // SOUTH
                { 0, 0, 0,    0, 1, 0,    1, 1, 0 },
                { 0, 0, 0,    1, 1, 0,    1, 0, 0 },

                // EAST
                { 1, 0, 0,    1, 1, 0,    1, 1, 1 },
                { 1, 0, 0,    1, 1, 1,    1, 0, 1 },

                // NORTH
                { 1, 0, 1,    1, 1, 1,    0, 1, 1 },
                { 1, 0, 1,    0, 1, 1,    0, 0, 1 },

                // WEST
                { 0, 0, 1,    0, 1, 1,    0, 1, 0 },
                { 0, 0, 1,    0, 1, 0,    0, 0, 0 },

                // TOP
                { 0, 1, 0,    0, 1, 1,    1, 1, 1 },
                { 0, 1, 0,    1, 1, 1,    1, 1, 0 },

                // BOTTOM
                { 1, 0, 1,    0, 0, 1,    0, 0, 0 },
                { 1, 0, 1,    0, 0, 0,    1, 0, 0 },

            };

            matrix_projection.m[0][0] = aspect_ratio * f_fov_rad;
            matrix_projection.m[1][1] = f_fov_rad;
            matrix_projection.m[2][2] = f_far / (f_far - f_near);
            matrix_projection.m[3][2] = (-f_far * f_near) / (f_far - f_near);
            matrix_projection.m[2][3] = 1;
            matrix_projection.m[3][3] = 0;

            tft.fillScreen(TFT_BLACK);
        }

        void upload(String filename) {

            // o = filename
            // p = eof
            // f, v = vertex data

            tft.fillScreen(TFT_BLACK);
            tft.setTextSize(2);
            tft.setCursor(40, 40);
            tft.print("uploading file");

            tft.setCursor(40, 60);
            tft.setTextSize(1);
            tft.print(filename);

            // fresh data
            current_mesh_name = filename;
            vector<vector3> vertices = {};
            mesh_cube.tris = {};

            bool finished = false;
            String last_uploaded;
            while (!finished) {

                // read data
                String data = Serial.readStringUntil('\n');
                String command = split_string(data, ' ', 0);
                String arg1 = split_string(data, ' ', 1);
                String arg2 = split_string(data, ' ', 2);
                String arg3 = split_string(data, ' ', 3);

                // print data
                if (last_uploaded != command) {
                    tft.setCursor(40, tft.cursor_y);
                    tft.println("uploading data with sign " + command);
                    last_uploaded = command;
                }

                // reset display if it overflows
                if (tft.cursor_y > display_height-40) {
                    tft.fillRect(40, 60, display_width, display_height-40, TFT_BLACK);
                    tft.setCursor(40, 60);
                }

                if (command == "v") {
                    vector3 new_vector;
                    new_vector.x = arg1.toFloat();
                    new_vector.y = arg2.toFloat();
                    new_vector.z = arg3.toFloat();
                    vertices.push_back(new_vector);
                }

                if (command == "f") {
                    int f[3];
                    f[0] = arg1.toInt();
                    f[1] = arg2.toInt();
                    f[2] = arg3.toInt();
                    mesh_cube.tris.push_back({ vertices[f[0] - 1], vertices[f[1] - 1], vertices[f[2] - 1] });
                }

                // finish sign
                if (command == "p") {
                    tft.setCursor(40, tft.cursor_y);
                    tft.println("finished");
                    finished = true;
                    delay(1000);
                }

                // write some data to tell the python program something is happening
                Serial.write("2");
            }

            vertices.clear();
        }

        void loop() {

            // start time
            unsigned long frame_start_time = millis();
            f_theta += 0.01;

            // assume incoming mesh information
            // .obj is encoded with regular text
            // also a periodic signal to tell the python program that no upload is taking place
            if (Serial.available()) {
                String data = Serial.readStringUntil('\n');
                if (data[0] == 'o') {
                    upload(data.substring(2, -1));
                    return;
                }
            } else if (millis() > next_signal_send) {
                Serial.write("1");
                next_signal_send = millis() + 1000;
            }

            // clearing the screen
            delay(10); // wait for 25ms longer to make the displayed image last longer
            for (auto tri : last_drawn_tris) {
                 tft.drawTriangle(
                    tri.points[0].x, tri.points[0].y,
                    tri.points[1].x, tri.points[1].y,
                    tri.points[2].x, tri.points[2].y,
                    TFT_BLACK
                 );
            }

            last_drawn_tris.clear();
            matrix4x4 matrix_rotationZ;
            matrix4x4 matrix_rotationX;

            // Rotation Z
            matrix_rotationZ.m[0][0] = cosf(f_theta);
            matrix_rotationZ.m[0][1] = sinf(f_theta);
            matrix_rotationZ.m[1][0] = -sinf(f_theta);
            matrix_rotationZ.m[1][1] = cosf(f_theta);
            matrix_rotationZ.m[2][2] = 1;
            matrix_rotationZ.m[3][3] = 1;

            // Rotation X
            matrix_rotationX.m[0][0] = 1;
            matrix_rotationX.m[1][1] = cosf(f_theta * 0.5f);
            matrix_rotationX.m[1][2] = sinf(f_theta * 0.5f);
            matrix_rotationX.m[2][1] = -sinf(f_theta * 0.5f);
            matrix_rotationX.m[2][2] = cosf(f_theta * 0.5f);
            matrix_rotationX.m[3][3] = 1;


            for (auto tri : mesh_cube.tris) {

                triangle projected, translated, rotated_z, rotated_zx;

                // Rotate in Z-Axis
                multiply_matrix_vector(tri.points[0], rotated_z.points[0], matrix_rotationZ);
                multiply_matrix_vector(tri.points[1], rotated_z.points[1], matrix_rotationZ);
                multiply_matrix_vector(tri.points[2], rotated_z.points[2], matrix_rotationZ);

                // Rotate in X-Axis
                multiply_matrix_vector(rotated_z.points[0], rotated_zx.points[0], matrix_rotationX);
                multiply_matrix_vector(rotated_z.points[1], rotated_zx.points[1], matrix_rotationX);
                multiply_matrix_vector(rotated_z.points[2], rotated_zx.points[2], matrix_rotationX);

                // Offset into the screen
                translated = rotated_zx;
                translated.points[0].z = rotated_zx.points[0].z + 6;
                translated.points[1].z = rotated_zx.points[1].z + 6;
                translated.points[2].z = rotated_zx.points[2].z + 6;



                vector3 normal, line1, line2;
                line1.x = translated.points[1].x - translated.points[0].x;
                line1.y = translated.points[1].y - translated.points[0].x;
                line1.z = translated.points[1].z - translated.points[0].x;

                line2.x = translated.points[2].x - translated.points[0].x;
                line2.y = translated.points[2].y - translated.points[0].x;
                line2.z = translated.points[2].z - translated.points[0].x;

                normal.x = line1.y * line2.z - line1.z * line2.y;
                normal.y = line1.z * line2.x - line1.x * line2.z;
                normal.z = line1.x * line2.y - line1.y * line2.x;

                float l = sqrtf(normal.x*normal.x + normal.y*normal.y + normal.z*normal.z);
                normal.x /= l; normal.y /= l; normal.z /= l;

                if (normal.x * (translated.points[0].x - camera.x) + normal.y * (translated.points[0].y - camera.y) + normal.z * (translated.points[0].z - camera.z) < 0) {

                    multiply_matrix_vector(translated.points[0], projected.points[0], matrix_projection);
                    multiply_matrix_vector(translated.points[1], projected.points[1], matrix_projection);
                    multiply_matrix_vector(translated.points[2], projected.points[2], matrix_projection);

                    projected.points[0].x += 1; projected.points[0].y += 1;
                    projected.points[1].x += 1; projected.points[1].y += 1;
                    projected.points[2].x += 1; projected.points[2].y += 1;

                    projected.points[0].x *= 0.5 * display_width;
                    projected.points[0].y *= 0.5 * display_height;
                    projected.points[1].x *= 0.5 * display_width;
                    projected.points[1].y *= 0.5 * display_height;
                    projected.points[2].x *= 0.5 * display_width;
                    projected.points[2].y *= 0.5 * display_height;

                    last_drawn_tris.push_back(projected);
                    tft.drawTriangle(
                        projected.points[0].x, projected.points[0].y,
                        projected.points[1].x, projected.points[1].y,
                        projected.points[2].x, projected.points[2].y,
                        TFT_ORANGE
                    );

                    /*
                        tft.setCursor(projected.points[0].x, projected.points[0].y);
                        tft.print((normal.x * (translated.points[0].x - camera.x) +
                                   normal.y * (translated.points[0].y - camera.y) +
                                    normal.z * (translated.points[0].z - camera.z)));
                    */

                }
            };

            // profiling
            unsigned long frame_end_time = millis();
            int wait_time = frame_end_time - frame_start_time;

            tft.setCursor(0, 0);
            tft.println("object: " + current_mesh_name + "");

            tft.print(int(wait_time));
            tft.println("ms");

            tft.print(int(float(1000/wait_time)));
            tft.println("fps");

            // if the frame budget is high enough, we can wait
            delay(max(frame_budget_ms-wait_time, 0));
        };
};