#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdint-gcc.h>
#include <ncurses.h>
#include <time.h>
#include "packets.h"
#include "base.h"

#define MAG_SCALE (4912.0f / 32760.0f)

void printTelemetryPacketNcurses(TelemetryPacket *packet, DownBoundPacket *down) {
    clear();
    int row = 0;


    // Time
    attroff(COLOR_PAIR(6));
    mvprintw(row++, 0, "Timestamp: %u", down->missionTimer);

    // MPU9250
    if (packet->presentDevices & MPU9250_PRESENT_BIT) {
        attron(COLOR_PAIR(1));
        mvprintw(row++, 0, "-- MPU9250 --");
    } else {
        attron(COLOR_PAIR(4));
        mvprintw(row++, 0, "-- MPU9250 (not present) --");
    }
    float ax = packet->accelerationX / 16384.0f;
    float ay = packet->accelerationY / 16384.0f;
    float az = packet->accelerationZ / 16384.0f;
    float gx = packet->gyroX / 131.0f;
    float gy = packet->gyroY / 131.0f;
    float gz = packet->gyroZ / 131.0f;
    float mx = packet->magnetX * MAG_SCALE;
    float my = packet->magnetY * MAG_SCALE;
    float mz = packet->magnetZ * MAG_SCALE;
    float temp = (packet->accelerometer_temperature / 333.87f) + 21.0f;
    mvprintw(row++, 2, "Accel      [g]: X=%.2f Y=%.2f Z=%.2f", ax, ay, az);
    mvprintw(row++, 2, "Gyro  [deg/s]: X=%.2f Y=%.2f Z=%.2f", gx, gy, gz);
    mvprintw(row++, 2, "Magnet [uT]: X=%.2f Y=%.2f Z=%.2f", mx, my, mz);
    mvprintw(row++, 2, "Temp [°C]: %.2f", temp);
    attroff(COLOR_PAIR(1));
    attroff(COLOR_PAIR(4));

    // CCS811
    if (packet->presentDevices & CCS811_PRESENT_BIT) {
        attron(COLOR_PAIR(2));
        mvprintw(row++, 0, "-- CCS811 --");
    } else {
        attron(COLOR_PAIR(4));
        mvprintw(row++, 0, "-- CCS811 (not present) --");
    }
    mvprintw(row++, 2, "eCO2: %u ppm", packet->eCO2);
    mvprintw(row++, 2, "TVOC: %u ppb", packet->tvoc);
    mvprintw(row++, 2, "Current: %u", packet->currentCCS);
    mvprintw(row++, 2, "Raw CCS: %u", packet->rawCCSData);
    attroff(COLOR_PAIR(2));
    attroff(COLOR_PAIR(4));

    // INA260
    if (packet->presentDevices & INA260_PRESENT_BIT) {
        attron(COLOR_PAIR(3));
        mvprintw(row++, 0, "-- INA260 --");
    } else {
        attron(COLOR_PAIR(4));
        mvprintw(row++, 0, "-- INA260 (not present) --");
    }
    mvprintw(row++, 2, "Voltage [V]: %.4f", packet->volts * 0.00125);
    mvprintw(row++, 2, "Current [A]: %.4f", packet->current * 0.00125);
    mvprintw(row++, 2, "Power   [W]: %.4f", packet->power * 0.01);
    attroff(COLOR_PAIR(3));
    attroff(COLOR_PAIR(4));

    // BME680
    if (packet->presentDevices & BME680_PRESENT_BIT) {
        attron(COLOR_PAIR(5));
        mvprintw(row++, 0, "-- BME680 --");
    } else {
        attron(COLOR_PAIR(4));
        mvprintw(row++, 0, "-- BME680 (not present) --");
    }
    mvprintw(row++, 2, "Temp: %d - %.2f °C", packet->temperature, packet->air_temperature / 100.0f);
    mvprintw(row++, 2, "Humidity: %d - %.2f %%", packet->humidity, packet->relative_humidity / 100.0f);
    mvprintw(row++, 2, "Pressure: %d - %.2f hPa", packet->pressure, packet->barometric_pressure / 100.0f);
    mvprintw(row++, 2, "Gas Resistance: %d - %.2f Ohm", packet->gas, packet->gas_resistance / 1000.0f);
    mvprintw(row++, 2, "Gas Valid: %s", packet->gas_valid ? "Yes" : "No");
    mvprintw(row++, 2, "Heater Stable: %s", packet->heater_stable ? "Yes" : "No");

    mvprintw(row++, 2, "IAQ: %u", packet->iaq_score);
    mvprintw(row++, 2, "TempScore: %.2f", packet->temperature_score);
    mvprintw(row++, 2, "HumidityScore: %.2f", packet->humidity_score);
    mvprintw(row++, 2, "GasScore: %.2f", packet->gas_score);
    attroff(COLOR_PAIR(5));
    attroff(COLOR_PAIR(4));

    // GPS
    attron(COLOR_PAIR(6));
    mvprintw(row++, 0, "-- GPS --");
    time_t rawtime = packet->time_seconds;
    struct tm *ptm = gmtime(&rawtime);

    // Parse custom date format YYDDMM -> YYYY-MM-DD
    int date = packet->date_yyddmm;
    int yy = date / 10000;
    int dd = (date / 100) % 100;
    int mm = date % 100;
    int year = 2000 + yy;

    mvprintw(row++, 2, "Time: %02d:%02d:%02d", ptm->tm_hour, ptm->tm_min, ptm->tm_sec);
    mvprintw(row++, 2, "Date: %04d-%02d-%02d", year, mm, dd);
    mvprintw(row++, 2, "Lat: %.4f°", packet->latitude_centi_degrees / 10000.0f);
    mvprintw(row++, 2, "Long: %.4f°", packet->longitude_centi_degrees / 10000.0f);
    mvprintw(row++, 2, "Alt: %.2f m", packet->altitude_centi_meters / 100.0f);
    mvprintw(row++, 2, "Speed: %.2f knots", packet->speed_centi_knots / 100.0f);
    mvprintw(row++, 2, "Satellites: %u", packet->num_satellites);
    mvprintw(row++, 2, "Fix Quality: %u", packet->fix_quality);

    // GPS Prediction
    mvprintw(row++, 2, "Predicted Lat: %.4f°", packet->predicted_latitude_centi_degrees / 10000.0f);
    mvprintw(row++, 2, "Predicted Long: %.4f°", packet->predicted_longitude_centi_degrees / 10000.0f);
    mvprintw(row++, 2, "Predicted Alt: %.2f m", packet->predicted_altitude_centi_meters / 100.0f);

    // MCP23018 ADC
    if (packet->presentDevices & MCP23018_PRESENT_BIT) {
        attron(COLOR_PAIR(3));
        mvprintw(row++, 0, "-- ADC sensors --");
    } else {
        attron(COLOR_PAIR(4));
        mvprintw(row++, 0, "-- ADC sensors (not present) --");
    }
    mvprintw(row++, 2, "NH3: %d", packet->NH3);
    mvprintw(row++, 2, "CO: %d", packet->CO);
    mvprintw(row++, 2, "NO2: %d", packet->NO2);
    mvprintw(row++, 2, "UVC: %d", packet->UVC);

    // Servo
    attron(COLOR_PAIR(5));
    mvprintw(row++, 0, "-- Servos --");
    mvprintw(row++, 2, "Servo A: Current=%d Target=%d", packet->currentServoA, packet->targetServoA);
    mvprintw(row++, 2, "Servo B: Current=%d Target=%d", packet->currentServoB, packet->targetServoB);

    // Index
    mvprintw(row++, 0, "Telemetry Index: %d", packet->telemetryIndex);


    refresh();
}


TelemetryPacket telemetryPacket;
DownBoundPacket down;

void handle_downlink_packet(uint8_t *buf, uint8_t rxLen) {
    if (rxLen < sizeof(DownBoundPacket)) {
        printf("Received packet too small to be valid.");
        return;
    }

    memcpy(&down, buf, sizeof(DownBoundPacket));

    printf("Downlink packet index: %u, type: %u", down.packetIndex, down.packetType);

    // Verify sync phrase
    if (strncmp(down.syncPhrase, DownlinkSync, strlen(DownlinkSync)) != 0) {
        printf("Invalid sync phrase, ignoring packet.");
        return;
    }

    uint8_t *payload = buf + sizeof(DownBoundPacket);
    uint32_t payloadSize = rxLen - sizeof(DownBoundPacket);

    uint32_t crcCheck = crc32_le(0, payload, payloadSize);

    if (crcCheck != down.CRCCheck) {
        printf("Received BAD CRC for packet %u, crc is %u, should be %u", down.packetIndex, crcCheck, down.CRCCheck);
        return;
    }

    switch (down.packetType) {
        case DownlinkPacketType_Telemetry:
            if (payloadSize >= sizeof(TelemetryPacket)) {
                memcpy(&telemetryPacket, payload, sizeof(TelemetryPacket));
                printf("Telemetry packet received!");

                printTelemetryPacketNcurses(&telemetryPacket, &down);
                // Here you would actually do something with the telemetry, like save it or display it
            } else {
                printf("Telemetry packet too small (%u bytes)", payloadSize);
            }
            break;
        case DownlinkPacketType_Ping:
            printf("Ping packet received!");
            // Optionally respond with pong here
            break;
        case DownlinkPacketType_ACK:
            printf("ACK packet received!");
            break;
        default:
            printf("Unknown packet type: %u", down.packetType);
            break;
    }
}


int open_serial(const char *device) {
    int fd = open(device, O_RDWR | O_NOCTTY | O_SYNC);
    if (fd < 0) {
        perror("open");
        return -1;
    }

    struct termios tty;
    memset(&tty, 0, sizeof tty);
    if (tcgetattr(fd, &tty) != 0) {
        perror("tcgetattr");
        close(fd);
        return -1;
    }

    cfsetospeed(&tty, B115200);
    cfsetispeed(&tty, B115200);

    tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;    // 8-bit chars
    tty.c_iflag &= ~IGNBRK;         // disable break processing
    tty.c_lflag = 0;                // no signaling chars, no echo, no canonical processing
    tty.c_oflag = 0;                // no remapping, no delays
    tty.c_cc[VMIN] = 1;            // read doesn't block
    tty.c_cc[VTIME] = 1;            // 0.1 seconds read timeout

    tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl

    tty.c_cflag |= (CLOCAL | CREAD);    // ignore modem controls, enable reading
    tty.c_cflag &= ~(PARENB | PARODD);  // shut off parity
    tty.c_cflag &= ~CSTOPB;
    tty.c_cflag &= ~CRTSCTS;

    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
        perror("tcsetattr");
        close(fd);
        return -1;
    }

    return fd;
}

int subMain() {

    int fd = open_serial("/dev/ttyUSB0");
    if (fd < 0) return 1;

    char buf[4096];
    size_t buf_pos = 0;

    memset(&telemetryPacket, 0, sizeof(TelemetryPacket));
    memset(&down, 0, sizeof(DownBoundPacket));

    unsigned char decoded[256];
    size_t decoded_len;
//    if (base64_decode("UGxlY2hEb2xlADsAAAAADuo+Ai9is1eY/vw/dP4AAAAAAAAAAAAAAABgD8sBCAAB/wPGGEYARQCgHQgAoksEKgUAnwIBAQQAe5APQtL3pEGycXlEqDLaSDcAAAAAAJqZeUAAAFBCAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA/zw=", decoded, &decoded_len) == 0) {
//        printf("Decoded data (%zu bytes):\n", decoded_len);
//        handle_downlink_packet(decoded, decoded_len);
//    } else {
//        printf("Base64 decode failed\n");
//    }

    printTelemetryPacketNcurses(&telemetryPacket, &down);
    while (1) {
        char ch;
        size_t n = read(fd, &ch, 1);
        if (n > 0) {
            if (ch == '\n') {
                buf[buf_pos] = '\0';
                printf("%s\n", buf);

                if (strncmp(buf, "Got DownLink data", 17) == 0) {
                    char *colon = strchr(buf, ':');
                    if (colon) {
                        colon++;
                        while (*colon == ' ') colon++; // skip spaces
                        printf("Detected base64 payload: %s\n", colon);

                        if (base64_decode(colon, decoded, &decoded_len) == 0) {
                            printf("Decoded data (%zu bytes):\n", decoded_len);
                            handle_downlink_packet(decoded, decoded_len);
                        } else {
                            printf("Base64 decode failed\n");
                        }
                    }
                }
                buf_pos = 0;
            } else {
                if (buf_pos < sizeof(buf) - 1)
                    buf[buf_pos++] = ch;
            }
        } else if (n < 0) {
            perror("read");
            break;
        }
    }

    close(fd);

    return 0;
}

int main(int argc, char *argv[]) {

    init_crc32_table();
    initscr();              // Start ncurses mode
    start_color();          // Enable color
    use_default_colors();
    init_pair(1, COLOR_CYAN, -1);
    init_pair(2, COLOR_GREEN, -1);
    init_pair(3, COLOR_YELLOW, -1);
    init_pair(4, COLOR_RED, -1);
    init_pair(5, COLOR_CYAN, -1);
    init_pair(6, COLOR_MAGENTA, -1);
    curs_set(0);            // Hide cursor
    int x = subMain();
    endwin(); // End ncurses mode
    return x;
}