#include <Arduino.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <WiFi.h>
#include <WiFiClientSecure.h>
#include <HTTPClient.h>
#include <PubSubClient.h>
#include <ArduinoJson.h>


#define TFT_CS   15
#define TFT_DC   2
#define TFT_RST  4
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

const int TRIG_PIN = 14;
const int ECHO_PIN = 27;
const int POT_PIN = 35; 
const int BTN_PIN = 5;


const int GPS_RX_PIN = 16; 
const int GPS_TX_PIN = 17; 


const int SAMPLE_RATE_HZ = 5;
const int WINDOW_SEC = 30;
const int MAX_SAMPLES = SAMPLE_RATE_HZ * WINDOW_SEC + 4;
const unsigned long SAMPLE_INTERVAL_MS = 1000 / SAMPLE_RATE_HZ;
const unsigned long PUBLISH_INTERVAL_MS = 1000;
const unsigned long RENDER_INTERVAL_MS = 50; 


const char* WIFI_SSID = "Wokwi-GUEST";
const char* WIFI_PASS = "";
const char* MQTT_SERVER = "broker.hivemq.com"; 
const int MQTT_PORT = 1883;
const char* MQTT_TOPIC_ECO = "car/eco";
const char* MQTT_TOPIC_TIP = "car/tip";


const bool DIRECT_LLM_ENABLED = false;
const char* LLM_PROXY_URL = "https://openrouter.ai/api/v1/chat/completions"; 
const char* LLM_PROXY_APIKEY = "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"; 


WiFiClientSecure wifiClientSecure;
WiFiClient wifiClient;
PubSubClient mqttClient(wifiClient);


struct Kalman1D {
  float q, r, x, p, k;
  Kalman1D(float q_ = 0.01f, float r_ = 0.5f, float init_x = 0.0f) : q(q_), r(r_), x(init_x), p(1.0f), k(0) {}
  float update(float measurement) {
    p = p + q;
    k = p / (p + r);
    x = x + k * (measurement - x);
    p = (1 - k) * p;
    return x;
  }
};
Kalman1D kf(0.01f, 0.5f, 0.0f);


float emaSpeed = 0.0f;
const float EMA_ALPHA = 0.2f;
float medWindow[3] = {0,0,0}; int medIdx = 0;
float median3(float v) {
  medWindow[medIdx++ % 3] = v;
  float a=medWindow[0], b=medWindow[1], c=medWindow[2];
  if ((a<=b && b<=c) || (c<=b && b<=a)) return b;
  if ((b<=a && a<=c) || (c<=a && a<=b)) return a;
  return c;
}


struct Sample {
  float speed_kmh;
  float accel; 
  bool brake_event;
  unsigned long ts;
};
Sample samples[MAX_SAMPLES];
int sampHead = 0, sampCount = 0;
float prevSpeed_ms = 0.0f; unsigned long prevSampleTs = 0;
const float BRAKE_ACCEL_THRESH = -1.5f;
const float IDLE_SPEED_KMH = 2.0f;


const float w_a = 0.30f, w_b = 0.20f, w_o = 0.25f, w_s = 0.15f, w_i = 0.10f;


const char* hintBank[] = {
  "Smooth acceleration saves fuel.",
  "Avoid sudden throttle bursts; steady inputs save energy.",
  "Maintain steady speed on highways for better economy.",
  "Slow down gradually before stops to reduce consumption.",
  "Turn off engine during long stops if safe.",
  "Gentle acceleration after traffic lights conserves energy.",
  "Keep safe distance to reduce emergency braking.",
  "Smooth braking saves fuel and vehicle resources.",
  "Anticipate traffic flow to avoid unnecessary stops.",
  "Regular maintenance improves fuel efficiency.",
  "Proper tire pressure reduces rolling resistance.",
  "Reduce air conditioning usage when possible.",
  "Remove unnecessary weight from vehicle.",
  "Use cruise control on highways for consistent speed.",
  "Plan routes to avoid heavy traffic."
};
const int HINT_BANK_N = sizeof(hintBank)/sizeof(hintBank[0]);
String currentHint = "";


unsigned long lastUltrasonicMs = 0;
float lastDistanceM = 999.0f;
float measureUltrasonic() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  unsigned long dur = pulseIn(ECHO_PIN, HIGH, 30000);
  if (dur == 0) return 999.0f;
  float dist = (dur / 2.0f) * 0.000343f;
  return dist;
}


String nmeaBuf = "";
float parseSpeedFromGPRMC(const String &line) {

  int idx = 0;
  int start = 0;
  int field = 0;
  String token;
  for (int i=0;i<=(int)line.length();i++) {
    char c = (i<(int)line.length())?line[i]:',';
    if (c==',' || c=='*' || i==(int)line.length()) {
      token = line.substring(start, i);
      start = i+1;
      field++;
      if (field==7) {
        
        float knots = token.toFloat();
        float kmh = knots * 1.852f;
        return kmh;
      }
    }
  }
  return NAN;
}


void mqttCallback(char* topic, byte* payload, unsigned int length) {
  
  String s;
  for (unsigned int i=0;i<length;i++) s += (char)payload[i];
  
  if (s.startsWith("{")) {
    StaticJsonDocument<256> doc;
    DeserializationError err = deserializeJson(doc, s);
    if (!err) {
      if (doc.containsKey("hint")) {
        String h = doc["hint"].as<String>();
        if (h.length() > 80) h = h.substring(0,80);
        currentHint = h;
        return;
      }
    }
  }
  
  if ((int)s.length() > 80) s = s.substring(0,80);
  currentHint = s;
}


void mqttReconnect() {
  while (!mqttClient.connected()) {
    Serial.print("MQTT connecting...");
    String clientId = "esp32-eco-";
    clientId += String((uint32_t)ESP.getEfuseMac(), HEX);
    if (mqttClient.connect(clientId.c_str())) {
      Serial.println("connected");
      mqttClient.subscribe(MQTT_TOPIC_TIP);
    } else {
      Serial.print("failed rc=");
      Serial.print(mqttClient.state());
      Serial.println(" retry in 2s");
      delay(2000);
    }
  }
}


void renderUI(float speed_kmh, int ecoScore, const String &hint, float distM) {
 
  tft.fillScreen(ILI9341_BLACK);
  
  tft.setTextSize(5); tft.setTextColor(ILI9341_WHITE); tft.setCursor(10, 10);
  tft.print((int)round(speed_kmh)); tft.print(" km/h");
 
  tft.setTextSize(2); tft.setCursor(10, 90); tft.setTextColor(ILI9341_WHITE);
  tft.print("ECO: "); tft.print(ecoScore); tft.print("%");
  int bx = 10, by = 130, bw = 200, bh = 18;
  tft.drawRect(bx-1, by-1, bw+2, bh+2, ILI9341_WHITE);
  int fillW = (ecoScore * bw) / 100;
  tft.fillRect(bx, by, fillW, bh, ILI9341_GREEN);
  
  tft.setCursor(10, 160); tft.setTextSize(2);
  if (distM < 10.0) { 
    tft.setTextColor(ILI9341_YELLOW); 
    tft.print("Obstacle: "); tft.print(distM,1); tft.print(" m"); 
  }
  else { 
    tft.setTextColor(ILI9341_CYAN); 
    tft.print("Clear ahead"); 
  }

  String h = hint; 
  if (h.length() > 80) h = h.substring(0,80);
  tft.setCursor(10, 190); tft.setTextSize(2); tft.setTextColor(ILI9341_MAGENTA);
  tft.print(h);
}


void addSample(float speed_kmh, float accel_m_s2) {
  Sample s; s.speed_kmh = speed_kmh; s.accel = accel_m_s2; s.brake_event = (accel_m_s2 < BRAKE_ACCEL_THRESH); s.ts = millis();
  samples[(sampHead + sampCount) % MAX_SAMPLES] = s;
  if (sampCount < MAX_SAMPLES) sampCount++; else sampHead = (sampHead + 1) % MAX_SAMPLES;
}

// compute eco score
int computeEcoScoreAndFeatures(float &mean_accel, float &brake_freq, float &overspeed_pct, float &stop_go_index, float &idle_pct) {
  if (sampCount == 0) { mean_accel=0; brake_freq=0; overspeed_pct=0; stop_go_index=0; idle_pct=0; return 100; }
  float sum_pos_accel = 0.0f; int pos_accel_cnt = 0; int brake_cnt = 0; int idle_cnt = 0; int overspeed_cnt = 0;
  const float SPEED_LIMIT = 80.0f;
  for (int i=0;i<sampCount;i++) {
    int idx = (sampHead + i) % MAX_SAMPLES;
    Sample &s = samples[idx];
    if (s.accel > 0.01f) { sum_pos_accel += s.accel; pos_accel_cnt++; }
    if (s.brake_event) brake_cnt++;
    if (s.speed_kmh <= IDLE_SPEED_KMH) idle_cnt++;
    if (s.speed_kmh > SPEED_LIMIT) overspeed_cnt++;
  }
  mean_accel = pos_accel_cnt ? (sum_pos_accel / pos_accel_cnt) : 0.0f;
  float windowMin = (float)WINDOW_SEC / 60.0f;
  brake_freq = (float)brake_cnt / windowMin; 
  overspeed_pct = (float)overspeed_cnt / (float)sampCount;
  idle_pct = (float)idle_cnt / (float)sampCount;
  int stop_cnt = 0; bool wasStopped=false;
  for (int i=0;i<sampCount;i++) {
    int idx = (sampHead + i) % MAX_SAMPLES;
    if (samples[idx].speed_kmh <= IDLE_SPEED_KMH) { if (!wasStopped) { stop_cnt++; wasStopped=true; } }
    else wasStopped=false;
  }
  stop_go_index = stop_cnt / windowMin; 
  
  float P_accel = constrain(mean_accel / 1.5f, 0.0f, 1.0f);
  float P_brake = constrain(brake_freq / 6.0f, 0.0f, 1.0f);
  float P_overspeed = constrain(overspeed_pct, 0.0f, 1.0f);
  float P_stopgo = constrain(stop_go_index / 3.0f, 0.0f, 1.0f);
  float P_idle = constrain(idle_pct, 0.0f, 1.0f);
  float penalty = w_a*P_accel + w_b*P_brake + w_o*P_overspeed + w_s*P_stopgo + w_i*P_idle;
  float eco = 100.0f * (1.0f - penalty);
  if (eco < 0) eco = 0; if (eco > 100) eco = 100;
  return (int)round(eco);
}


float logistic_prob_economical(float mean_accel, float brake_freq, float overspeed_pct, float stop_go_index, float idle_pct) {
  // initial weights (tuning needed)
  float w0 = -0.2f, w1 = -1.5f, w2 = -0.9f, w3 = -2.0f, w4 = -1.2f, w5 = -0.8f;
  float z = w0 + w1*mean_accel + w2*brake_freq + w3*overspeed_pct + w4*stop_go_index + w5*idle_pct;
  float p = 1.0f / (1.0f + exp(-z));
  return p;
}


String callLLMProxy(float ecoScore, float mean_accel, float brake_freq, float overspeed_pct, float stop_go_index, float idle_pct) {
  if (!DIRECT_LLM_ENABLED) return "";
  // Build JSON prompt for proxy
  StaticJsonDocument<512> doc;
  doc["eco_score"] = ecoScore;
  doc["mean_accel"] = mean_accel;
  doc["brake_freq"] = brake_freq;
  doc["overspeed_pct"] = overspeed_pct;
  doc["stop_go_index"] = stop_go_index;
  doc["idle_pct"] = idle_pct;
  doc["max_chars"] = 80;
 
  doc["lang"] = "EN";
  String body; serializeJson(doc, body);
  // HTTP POST
  wifiClientSecure.setInsecure(); 
  HTTPClient https;
  https.begin(wifiClientSecure, LLM_PROXY_URL);
  https.addHeader("Content-Type", "application/json");
 
  if (LLM_PROXY_APIKEY && strlen(LLM_PROXY_APIKEY)>0) https.addHeader("X-API-Key", LLM_PROXY_APIKEY);
  int code = https.POST(body);
  String payload="";
  if (code > 0) {
    payload = https.getString();
   
    StaticJsonDocument<512> resp;
    DeserializationError err = deserializeJson(resp, payload);
    if (!err && resp.containsKey("hint")) {
      String hint = resp["hint"].as<String>();
      if (hint.length() > 80) hint = hint.substring(0,80);
      https.end();
      return hint;
    } else {
      https.end();
      return "";
    }
  } else {
    https.end();
    return "";
  }
}


unsigned long lastSampleMillis = 0, lastPublishMillis = 0, lastRenderMillis = 0;
unsigned long lastNMEATime = 0;
bool gpsActive = false;


void setup() {
  Serial.begin(115200);
  delay(100);
  pinMode(TRIG_PIN, OUTPUT); pinMode(ECHO_PIN, INPUT);
  pinMode(BTN_PIN, INPUT_PULLUP);
  tft.begin(); tft.setRotation(1); tft.fillScreen(ILI9341_BLACK);

  // WiFi
  WiFi.mode(WIFI_STA);
  WiFi.begin(WIFI_SSID, WIFI_PASS);
  Serial.print("Connecting WiFi");
  unsigned long st = millis();
  while (WiFi.status() != WL_CONNECTED && millis() - st < 10000) { delay(200); Serial.print('.'); }
  Serial.println();
  if (WiFi.status() == WL_CONNECTED) {
    Serial.println("WiFi connected");
    mqttClient.setServer(MQTT_SERVER, MQTT_PORT);
    mqttClient.setCallback(mqttCallback);
  } else {
    Serial.println("WiFi not connected - MQTT disabled");
  }

  // Serial2 for GPS
  Serial2.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN); // GPS on UART2
  Serial.println("Serial2 started for GPS (UART2)");

  prevSampleTs = millis();
  randomSeed(analogRead(0));
  currentHint = String(hintBank[random(HINT_BANK_N)]);
}

// ---------------- loop ----------------
void loop() {
  unsigned long now = millis();

  // MQTT loop
  if (WiFi.status() == WL_CONNECTED) {
    if (!mqttClient.connected()) mqttReconnect();
    mqttClient.loop();
  }

  // Read GPS NMEA from Serial2 (if available)
  while (Serial2.available()) {
    char c = (char)Serial2.read();
    if (c == '\r' || c == '\n') {
      if (nmeaBuf.length() > 6) {
        if (nmeaBuf.startsWith("$GPRMC") || nmeaBuf.startsWith("$GNRMC")) {
          float kmh = parseSpeedFromGPRMC(nmeaBuf);
          if (!isnan(kmh)) {
            gpsActive = true;
            lastNMEATime = now;
            // feed this measurement into pipeline
            static float gpsSpeedMeas = 0;
            gpsSpeedMeas = kmh;
            // copy into ema/kalman immediately for responsiveness
            float kfSpeed = kf.update(gpsSpeedMeas);
            float med = median3(kfSpeed);
            if (emaSpeed == 0.0f) emaSpeed = med;
            emaSpeed = EMA_ALPHA * med + (1 - EMA_ALPHA) * emaSpeed;
            // update prevSpeed for accel calc
            prevSpeed_ms = emaSpeed * 1000.0f / 3600.0f;
            prevSampleTs = now;
          }
        }
      }
      nmeaBuf = "";
    } else {
      nmeaBuf += c;
      if (nmeaBuf.length() > 300) nmeaBuf = ""; // safety
    }
  }

  // Sampling fixed rate
  if (now - lastSampleMillis >= SAMPLE_INTERVAL_MS) {
    lastSampleMillis = now;
    float speed_kmh_meas = 0.0f;
    // prefer gps if recent
    if (gpsActive && (now - lastNMEATime) < 1500) {
      speed_kmh_meas = emaSpeed; // already smoothed
    } else {
      // fallback: potentiometer
      int raw = analogRead(POT_PIN);
      float potNorm = (float)raw / 4095.0f;
      speed_kmh_meas = potNorm * 150.0f;
      // Kalman + anti-jitter
      float kfSpeed = kf.update(speed_kmh_meas);
      float med = median3(kfSpeed);
      if (emaSpeed == 0.0f) emaSpeed = med;
      emaSpeed = EMA_ALPHA * med + (1 - EMA_ALPHA) * emaSpeed;
    }
    // accel approx
    unsigned long dt_ms = now - prevSampleTs;
    float speed_ms = emaSpeed * 1000.0f / 3600.0f;
    float accel = 0.0f;
    if (dt_ms > 0) accel = (speed_ms - prevSpeed_ms) / (dt_ms / 1000.0f);
    prevSpeed_ms = speed_ms;
    prevSampleTs = now;

    // ultrasonic update
    if (now - lastUltrasonicMs > 200) {
      lastUltrasonicMs = now;
      lastDistanceM = measureUltrasonic();
    }

    addSample(emaSpeed, accel);
  }

  // Publish eco every 1s
  if (now - lastPublishMillis >= PUBLISH_INTERVAL_MS) {
    lastPublishMillis = now;
    float mean_accel, brake_freq, overspeed_pct, stop_go_index, idle_pct;
    int eco = computeEcoScoreAndFeatures(mean_accel, brake_freq, overspeed_pct, stop_go_index, idle_pct);

    // flags quick detection
    bool over_accel=false, harsh_brake=false, idle_flag=false;
    for (int i=0;i<min(sampCount,5);i++) {
      int idx = (sampHead + sampCount - 1 - i + MAX_SAMPLES) % MAX_SAMPLES;
      if (samples[idx].accel > 1.2f) over_accel = true;
      if (samples[idx].accel < -1.6f) harsh_brake = true;
      if (samples[idx].speed_kmh <= IDLE_SPEED_KMH) idle_flag = true;
    }

    // prepare JSON
    StaticJsonDocument<256> doc;
    doc["ts"] = now/1000;
    doc["eco_score"] = eco;
    doc["speed"] = round(emaSpeed*10.0f)/10.0f;
    JsonArray flags = doc.createNestedArray("flags");
    if (over_accel) flags.add("over_accel");
    if (idle_flag) flags.add("idle");
    if (harsh_brake) flags.add("harsh_brake");
    String payload; serializeJson(doc, payload);
    Serial.println("MQTT publish: " + payload);
    if (WiFi.status() == WL_CONNECTED) mqttClient.publish(MQTT_TOPIC_ECO, payload.c_str());

    // Optionally call LLM proxy directly and set currentHint (if enabled)
    if (DIRECT_LLM_ENABLED) {
      String hint = callLLMProxy((float)eco, mean_accel, brake_freq, overspeed_pct, stop_go_index, idle_pct);
      if (hint.length() > 0) currentHint = hint;
    } else {
      // local fallback hint selection
      if (over_accel || idle_flag || harsh_brake) {
        currentHint = String(hintBank[random(HINT_BANK_N)]);
      }
      // else keep existing hint
    }
  }

  // Render UI
  if (now - lastRenderMillis >= RENDER_INTERVAL_MS) {
    lastRenderMillis = now;
    float mean_accel, brake_freq, overspeed_pct, stop_go_index, idle_pct;
    int eco = computeEcoScoreAndFeatures(mean_accel, brake_freq, overspeed_pct, stop_go_index, idle_pct);
    renderUI(emaSpeed, eco, currentHint, lastDistanceM);
  }

  delay(2);
}