#include <Arduino.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <math.h>
#include <WiFi.h>
#include <time.h>

// --- Configuración de Pines ---
#define TFT_CS    15
#define TFT_DC     2
#define TFT_RST    4
#define BUTTON_PIN 5
#define LED_PIN   26

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// Parámetros del elipsoide WGS84 para considerar el achatamiento de la Tierra
const double a_earth = 6378137.0;       // Radio ecuatorial (m)
const double f = 1.0 / 298.257223563;   // Aplanamiento
const double e2 = f * (2 - f);          // Excentricidad^2

// --- Parámetros TLE (ISS ZARYA) ---  
const double SMA = 6798694.11;   // Semieje mayor (m)
const double EXC = 0.0006224;    // Excentricidad
const double INC = 0.9011728340; // Inclinación (rad)
const double RAAN = 5.6123488106; // RAAN (rad)
const double ARG_PER = 4.5019319432; // Argumento Perigeo (rad)
const double AM0 = 1.7817612548; // Anomalía Media del Epoch (rad)
const double NME = 0.001126239190; // Velocidad Angular Media (rad/s)

const time_t TLE_EPOCH = 1775010328; // Unix Timestamp del TLE
uint32_t UNIX_NOW_START; // Tiempo de referencia inicial

// --- Variables de Optimización (Precalculadas) ---
double Px, Py, Pz, Qx, Qy, Qz, SQRT_1, rad2deg, pi2;

// Variables de estado
volatile bool trackingActive = false;
volatile double currentLat = 0.0, currentLon = 0.0, currentAlt = 0.0;
volatile uint32_t executionTime = 0;
volatile int currentIterations = 0; // Guarda el número de iteraciones

TaskHandle_t TaskCalcHandle;
TaskHandle_t TaskUIHandle;

void precomputeOrbit() {
    double cosO = cos(RAAN);
    double sinO = sin(RAAN);
    double cosi = cos(INC);
    double sini = sin(INC);
    double cosw = cos(ARG_PER);
    double sinw = sin(ARG_PER);

    SQRT_1 = sqrt(1.0 - EXC * EXC);
    rad2deg = 180.0 / M_PI;
    pi2 = 2.0 * M_PI;

    Px = cosO * cosw - sinO * sinw * cosi;
    Py = sinO * cosw + cosO * sinw * cosi;
    Pz = sinw * sini;

    Qx = -cosO * sinw - sinO * cosw * cosi;
    Qy = -sinO * sinw + cosO * cosw * cosi;
    Qz = cosw * sini;
}

// Iterador de la orbita de Kepler con NEWTON-RAPHSON
double solveKepler(double M, double e) {
    double E = M;
    double tolerance = 1e-10;
    int maxIter = 50; 
    int iter = 0;

    for (iter = 0; iter < maxIter; iter++) {
        double f_E = E - e * sin(E) - M;
        double f_prime_E = 1.0 - e * cos(E);
        double E_next = E - (f_E / f_prime_E);

        if (fabs(E_next - E) < tolerance) {
            E = E_next;
            iter++; // Contamos la iteración en la que converge
            break;
        }
        E = E_next;
    }
    
    currentIterations = iter; // Guardamos las iteraciones para la interfaz
    return E;
}

bool syncTimeNTP() {
    configTime(0, 0, "pool.ntp.org", "time.nist.gov");
    struct tm timeinfo;
    for (int i = 0; i < 10; i++) { 
        if (getLocalTime(&timeinfo)) {
            time_t now = time(nullptr);
            UNIX_NOW_START = (uint32_t)now;
            return true;
        }
        delay(300);
    }
    Serial.println("Error de conexion NTP");
    return false;
}

void propagatorTask(void * pvParameters) {
    for (;;) {
        if (trackingActive) {
            uint32_t t_start = micros();
            
            double now = UNIX_NOW_START + millis() * 0.001;
            double dt = now - (double)TLE_EPOCH;
            double M = fmod(AM0 + NME * dt, pi2);
            if (M < 0) M += pi2; 
            
            // Resolvemos Kepler con Newton-Raphson
            double E = solveKepler(M, EXC);
            
            double x_orb = SMA * (cos(E) - EXC);
            double y_orb = SMA * SQRT_1 * sin(E);
            double x = Px * x_orb + Qx * y_orb;
            double y = Py * x_orb + Qy * y_orb;
            double z = Pz * x_orb + Qz * y_orb;
            
            double p = sqrt(x*x + y*y);
            double lat = atan2(z, p * (1 - e2));
            double sinLat = sin(lat);
            double N = a_earth / sqrt(1 - e2 * sinLat * sinLat);
            lat = atan2(z + e2 * N * sinLat, p);
            sinLat = sin(lat);
            N = a_earth / sqrt(1 - e2 * sinLat * sinLat);
            double alt = p / cos(lat) - N;

            currentLat = lat * rad2deg;
            currentAlt = alt / 1000.0;

            double jd = (now / 86400.0) + 2440587.5; 
            double t = (jd - 2451545.0) / 36525.0; 
            double gmst = fmod(6.30038809898489 + 481211.846813445 * t, pi2);

            double x_ecef = cos(gmst) * x + sin(gmst) * y;
            double y_ecef = -sin(gmst) * x + cos(gmst) * y;

            currentLon = atan2(y_ecef, x_ecef) * rad2deg;
            
            executionTime = (micros() - t_start); 
        }
        vTaskDelay(pdMS_TO_TICKS(600));
    }
}

void uiTask(void * pvParameters) {
    for (;;) {
        if (!trackingActive) {
            if (digitalRead(BUTTON_PIN) == LOW) {
                tft.setCursor(10, 180);
                tft.setTextColor(ILI9341_YELLOW);
                tft.println("Conectando NTP...");
                if (syncTimeNTP()) {
                    trackingActive = true;
                    tft.fillScreen(ILI9341_BLACK);
                    tft.drawFastHLine(0, 40, 320, ILI9341_CYAN);
                    digitalWrite(LED_PIN, HIGH);
                } else {
                    tft.setCursor(10, 200);
                    tft.setTextColor(ILI9341_RED);
                    tft.println("Fallo de Red NTP");
                }
                vTaskDelay(pdMS_TO_TICKS(600)); 
            }
        } else {
            // Textos de estado actualizados
            tft.setTextColor(ILI9341_GREEN, ILI9341_BLACK);
            tft.setTextSize(2); 
            
            tft.setCursor(10, 15);  tft.print("INFO SATELITE ISS");
            tft.setCursor(10, 60);  tft.printf("Latitud: %7.4f  ", currentLat);
            tft.setCursor(10, 90);  tft.printf("Longitud:%7.4f  ", currentLon);
            tft.setCursor(10, 120); tft.printf("Altitud: %5.1f km ", currentAlt);
            
            // Añadimos Tiempos e Iteraciones en Naranja
            tft.setTextColor(ILI9341_ORANGE, ILI9341_BLACK);
            tft.setCursor(10, 180); tft.printf("T.Ejec: %u us   ", executionTime);
            tft.setCursor(10, 210); tft.printf("Iteraciones: %d   ", currentIterations);
        }
        vTaskDelay(pdMS_TO_TICKS(300));
    }
}

void setup() {
    Serial.begin(115200);

    const char* ssid = "Wokwi-GUEST";
    const char* password = "";
    WiFi.begin(ssid, password);

    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }

    pinMode(BUTTON_PIN, INPUT_PULLUP);
    pinMode(LED_PIN, OUTPUT);
    digitalWrite(LED_PIN, LOW);

    tft.begin();
    tft.setRotation(1);
    tft.fillScreen(ILI9341_BLACK);
    
    // Pantalla de inicio personalizada
    tft.setTextColor(ILI9341_CYAN);
    tft.setTextSize(3);
    tft.setCursor(20, 70);
    tft.println("MONITOR");
    tft.setCursor(20, 105);
    tft.println("SATELITE ISS");
    
    tft.setTextColor(ILI9341_GREEN);
    tft.setTextSize(2);
    tft.setCursor(15, 150);
    tft.println("-> Pulse el boton");

    precomputeOrbit();

    xTaskCreatePinnedToCore(propagatorTask, "CalcTask", 4096, NULL, 2, &TaskCalcHandle, 0);
    xTaskCreatePinnedToCore(uiTask, "UITask", 4096, NULL, 1, &TaskUIHandle, 1);
}

void loop() { vTaskDelete(NULL); }