#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>

#define GRID_SIZE 22
#define TOTAL_FACETS ((GRID_SIZE * 2 - 1) * (GRID_SIZE * 2 - 1))

typedef struct {
    float depth;
    char color_idx[8]; 
    int x[4];
    int y[4];
} Facet;

float SUN_VECTOR[3] = {0.6f, -0.6f, 0.5f};

// Base function to compute the wave height with edge decay
float calculate_z(int x, int y) {
    float d = sqrtf((float)(x * x + y * y));
    float max_d = sqrtf(2.0f * (GRID_SIZE * GRID_SIZE));
    
    // Linear decay envelope factor
    float decay = 1.0f - (d / max_d);
    if (decay < 0.0f) decay = 0.0f;
    
    return cosf(d * 0.4f) * 9.0f * decay;
}

void raw_isometric_projection(float x, float y, float z, float *iso_x, float *iso_y) {
    *iso_x = (x - y) * 0.866f;
    *iso_y = (x + y) * 0.500f - z;
}

int main() {
    float s_len = sqrtf(SUN_VECTOR[0]*SUN_VECTOR[0] + SUN_VECTOR[1]*SUN_VECTOR[1] + SUN_VECTOR[2]*SUN_VECTOR[2]);
    SUN_VECTOR[0] /= s_len; SUN_VECTOR[1] /= s_len; SUN_VECTOR[2] /= s_len;

    printf("Connecting directly to POSIX terminal link /tmp/vtty1...\n");
    int serial_fd = open("/tmp/vtty1", O_WRONLY | O_NOCTTY);
    if (serial_fd < 0) {
        perror("Error opening /tmp/vtty1");
        return 1;
    }

    struct termios tty;
    memset(&tty, 0, sizeof(tty));
    cfsetospeed(&tty, B115200);
    tty.c_cflag |= (CLOCAL | CREAD | CS8);
    tcsetattr(serial_fd, TCSANOW, &tty);

    // Erase background buffer
    if (write(serial_fd, "!GRA\n", 5) < 0) perror("Write failed");

    Facet *facets = malloc(sizeof(Facet) * TOTAL_FACETS);
    int facet_count = 0;

    float min_x = 1e9f, max_x = -1e9f;
    float min_y = 1e9f, max_y = -1e9f;

    printf("Step 1: Calculating geometry coordinates...\n");
    for (int x = -GRID_SIZE; x < GRID_SIZE - 1; x++) {
        for (int y = -GRID_SIZE; y < GRID_SIZE - 1; y++) {
            float z00 = calculate_z(x, y);
            float z10 = calculate_z(x + 1, y);
            float z01 = calculate_z(x, y + 1);
            float z11 = calculate_z(x + 1, y + 1);

            float nx = z00 - z10;
            float ny = z00 - z01;
            float nz = 1.0f;

            float n_len = sqrtf(nx*nx + ny*ny + nz*nz);
            if (n_len > 0.0f) { nx /= n_len; ny /= n_len; nz /= n_len; }

            float dot_product = nx * SUN_VECTOR[0] + ny * SUN_VECTOR[1] + nz * SUN_VECTOR[2];
            float intensity = (dot_product < 0.0f) ? 0.0f : dot_product;

            float shaded_brightness = 0.30f + (0.70f * intensity);
            unsigned char vga_color = 16 + (unsigned char)(shaded_brightness * 15.99f);


            float rx[4], ry[4];
            raw_isometric_projection((float)x,     (float)y,     z00, &rx[0], &ry[0]);
            raw_isometric_projection((float)(x+1), (float)y,     z10, &rx[1], &ry[1]);
            raw_isometric_projection((float)(x+1), (float)(y+1), z11, &rx[2], &ry[2]);
            raw_isometric_projection((float)x,     (float)(y+1), z01, &rx[3], &ry[3]);

            for (int i = 0; i < 4; i++) {
                if (rx[i] < min_x) min_x = rx[i]; if (rx[i] > max_x) max_x = rx[i];
                if (ry[i] < min_y) min_y = ry[i]; if (ry[i] > max_y) max_y = ry[i];
            }

            Facet f;
            f.depth = (float)(x + y);
            snprintf(f.color_idx, sizeof(f.color_idx), "%u", vga_color);
            
            // Store absolute vertex coordinate offsets using the loop index 'i'
            for(int i = 0; i < 4; i++) { 
                f.x[i] = (i == 1 || i == 2) ? x + 1 : x; 
                f.y[i] = (i == 2 || i == 3) ? y + 1 : y;
            } 
            facets[facet_count++] = f;
        }
    }

    float raw_width = max_x - min_x;
    float raw_height = max_y - min_y;
    float scale_x_factor = (1024.0f - 80.0f) / raw_width;
    float scale_y_factor = (768.0f - 80.0f) / raw_height;
    float scale_factor = (scale_x_factor < scale_y_factor) ? scale_x_factor : scale_y_factor;

    float offset_x = 512.0f - (raw_width * scale_factor) / 2.0f - (min_x * scale_factor);
    float offset_y = 384.0f - (raw_height * scale_factor) / 2.0f - (min_y * scale_factor);

    printf("Step 2: Sorting panels by depth...\n");
    for (int i = 0; i < facet_count - 1; i++) {
        for (int j = 0; j < facet_count - i - 1; j++) {
            if (facets[j].depth > facets[j + 1].depth) {
                Facet temp = facets[j];
                facets[j] = facets[j + 1];
                facets[j + 1] = temp;
            }
        }
    }

    printf("Step 3: Streaming coordinates to independent terminal...\n");
    char output_buffer[256];

    for (int i = 0; i < facet_count; i++) {
        int sx[4], sy[4];

        for (int p = 0; p < 4; p++) {
            float rx, ry;
            float z = calculate_z(facets[i].x[p], facets[i].y[p]);
            raw_isometric_projection((float)facets[i].x[p], (float)facets[i].y[p], z, &rx, &ry);

            sx[p] = (int)(rx * scale_factor + offset_x);
            sy[p] = (int)(ry * scale_factor + offset_y);
        }

        int len = snprintf(output_buffer, sizeof(output_buffer), 
                           "!POL %s %d %d %d %d %d %d %d %d\n",
                           facets[i].color_idx, sx[0], sy[0], sx[1], sy[1], sx[2], sy[2], sx[3], sy[3]);

        if (write(serial_fd, output_buffer, len) < 0) perror("Write failed");
    }

    // STEP 4: Send the dynamic frame confirmation flip token!
    printf("Step 4: Triggering explicit frame flip flag...\n");
    if (write(serial_fd, "!FLI\n", 5) < 0) perror("Flip token send failed");
    tcdrain(serial_fd);

    printf("Render Complete!\n");
    free(facets);
    close(serial_fd);
    return 0;
}