#pragma once
#include <string>
#include <vector>
#include <lwip/sockets.h>
#include <lwip/netdb.h>

static const char* ICAR2_IP   = "192.168.0.10";  // Standard iCar2 IP
static const int   ICAR2_PORT = 35000;            // Standard iCar2 Port
static int obd_sock = -1;
static bool obd_data_received = false;

std::string obd_send(const char* cmd) {
  if (obd_sock < 0) return "";
  char flush_buf[256];
  while (::recv(obd_sock, flush_buf, sizeof(flush_buf)-1, 0) > 0) {}
  std::string full_cmd = std::string(cmd) + "\r";
  ::send(obd_sock, full_cmd.c_str(), full_cmd.size(), 0);
  std::string response = "";
  char buf[512];
  unsigned long start = millis();
  while (millis() - start < 800) {
    int len = ::recv(obd_sock, buf, sizeof(buf) - 1, 0);
    if (len > 0) {
      buf[len] = '\0';
      for (int i = 0; i < len; i++) {
        char c = buf[i];
        if (c == '>') goto done;
        if (c != '\r' && c != '\n' && c != ' ')
          response += c;
      }
    }
    delay(10);
  }
  done:
  ESP_LOGD("OBD", "CMD: %s -> %s", cmd, response.c_str());
  return response;
}

void obd_disconnect() {
  if (obd_sock >= 0) {
    ::close(obd_sock);
    obd_sock = -1;
  }
  obd_data_received = false;
}

void obd_connect() {
  ESP_LOGI("OBD", "Verbinde mit iCar2 %s:%d", ICAR2_IP, ICAR2_PORT);
  obd_disconnect();
  struct sockaddr_in server_addr;
  obd_sock = ::socket(AF_INET, SOCK_STREAM, 0);
  if (obd_sock < 0) { ESP_LOGE("OBD", "Socket Fehler!"); return; }
  server_addr.sin_family = AF_INET;
  server_addr.sin_port = htons(ICAR2_PORT);
  inet_pton(AF_INET, ICAR2_IP, &server_addr.sin_addr);
  if (::connect(obd_sock, (struct sockaddr*)&server_addr, sizeof(server_addr)) != 0) {
    ESP_LOGE("OBD", "Verbindung fehlgeschlagen!");
    ::close(obd_sock); obd_sock = -1; return;
  }
  struct timeval timeout;
  timeout.tv_sec = 2; timeout.tv_usec = 0;
  setsockopt(obd_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
  delay(200);

  auto init_cmd = [&](const char* cmd) {
    ::send(obd_sock, cmd, strlen(cmd), 0);
    delay(150);
    char buf[128];
    ::recv(obd_sock, buf, sizeof(buf)-1, 0);
  };

  init_cmd("ATZ\r");
  delay(500);
  char rbuf[128]; ::recv(obd_sock, rbuf, sizeof(rbuf)-1, 0);
  init_cmd("ATE0\r");
  init_cmd("ATL0\r");
  init_cmd("ATS0\r");
  init_cmd("ATH1\r");
  init_cmd("ATSP6\r");
  init_cmd("ATSH7E4\r");
  init_cmd("ATFCSH7E4\r");
  init_cmd("ATFCSD300000\r");
  init_cmd("ATFCSM1\r");

  // BMS aufwecken
  ::send(obd_sock, "2101\r", 5, 0);
  delay(800);
  char sbuf[256];
  int slen = ::recv(obd_sock, sbuf, sizeof(sbuf)-1, 0);
  sbuf[slen > 0 ? slen : 0] = '\0';
  ESP_LOGI("OBD", "Wake-up: %s", sbuf);

  ESP_LOGI("OBD", "iCar2 initialisiert");
}

std::vector<int> parse_response(const std::string& raw, const std::string& service) {
  std::vector<int> bytes;
  std::string flat = "";
  for (size_t i = 0; i < raw.size(); i++) {
    if (i + 4 < raw.size() &&
        raw[i]=='7' && raw[i+1]=='E' && raw[i+2]=='C') {
      i += 4;
      continue;
    }
    flat += raw[i];
  }
  size_t pos = flat.find(service);
  if (pos == std::string::npos) return bytes;
  pos += service.size();
  for (size_t i = pos; i + 1 < flat.size(); i += 2) {
    std::string bs = flat.substr(i, 2);
    if (bs.find_first_not_of("0123456789abcdefABCDEF") != std::string::npos) break;
    bytes.push_back(std::stoi(bs, nullptr, 16));
  }
  return bytes;
}

void obd_keepalive() {
  if (id(g_obd_enabled)) return;
  if (!id(wifi_wlan).is_connected()) return;

  if (obd_sock < 0) {
    ESP_LOGI("OBD", "Keep-Alive: verbinde mit iCar2");
    struct sockaddr_in server_addr;
    obd_sock = ::socket(AF_INET, SOCK_STREAM, 0);
    if (obd_sock < 0) return;
    server_addr.sin_family = AF_INET;
    server_addr.sin_port = htons(ICAR2_PORT);
    inet_pton(AF_INET, ICAR2_IP, &server_addr.sin_addr);
    if (::connect(obd_sock, (struct sockaddr*)&server_addr, sizeof(server_addr)) != 0) {
      ESP_LOGW("OBD", "Keep-Alive: Verbindung fehlgeschlagen");
      ::close(obd_sock); obd_sock = -1; return;
    }
    struct timeval timeout;
    timeout.tv_sec = 2; timeout.tv_usec = 0;
    setsockopt(obd_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
    ESP_LOGI("OBD", "Keep-Alive: verbunden");
  }

  std::string resp = obd_send("ATI");
  if (resp.empty()) {
    ESP_LOGW("OBD", "Keep-Alive: keine Antwort, trenne");
    obd_disconnect();
  } else {
    ESP_LOGD("OBD", "Keep-Alive: %s", resp.c_str());
  }
}

void obd_query_bms() {
  if (!id(g_obd_enabled)) {
    obd_disconnect();
    return;
  }

  if (obd_sock < 0) obd_connect();
  if (obd_sock < 0) return;

  std::string raw = obd_send("220101");
  ESP_LOGI("OBD", "RAW 2101: %s", raw.c_str());

  // Bei CANERROR: einmal warten und retry, bevor disconnect
  if (raw.find("CANERROR") != std::string::npos ||
      raw.find("NODATA") != std::string::npos) {
    ESP_LOGW("OBD", "CAN Fehler, warte 3s und retry...");
    delay(3000);
    raw = obd_send("220101");
    ESP_LOGI("OBD", "Retry RAW: %s", raw.c_str());
  }

  // Nach Retry immer noch kein Ergebnis -> disconnect
  if (raw.find("CANERROR") != std::string::npos ||
      raw.find("NODATA") != std::string::npos ||
      raw.empty()) {
    ESP_LOGW("OBD", "Retry fehlgeschlagen, trenne Verbindung");
    obd_disconnect();
    return;
  }

  auto b = parse_response(raw, "620101");

  // Byte-Dump 2101
  for (int i = 0; i < (int)b.size(); i++) {
    ESP_LOGI("OBD", "2101 b[%d] = 0x%02X (%d)", i, b[i], b[i]);
  }

  if (b.size() < 24) {
    ESP_LOGW("OBD", "Zu wenig Bytes (%d)", b.size());
    return;
  }

  float soc      = b[4] / 2.0f;
  float voltage  = ((b[22] << 8) | b[23]) / 10.0f;  // echte Batteriespannung
  int16_t curr_raw = (int16_t)((b[10] << 8) | b[11]);
  float current  = -(curr_raw / 10.0f);              // negativ = Laden, invertieren
  float tmax     = (float)b[12];
  float tmin     = (float)b[17];
  float tavg     = (float)b[14];
  bool  charging = (current > 0.1f);

  id(g_soc)             = soc;
  id(g_battery_voltage) = voltage;
  id(g_battery_current) = current;
  id(g_bat_temp_min)    = tmin;
  id(g_bat_temp_max)    = tmax;
  id(g_bat_temp_avg)    = tavg;
  id(g_is_charging)     = charging;

  obd_data_received = true;
  ESP_LOGI("OBD", "SOC:%.1f%% U:%.1fV I:%.2fA T:%.0f/%.0fC Laden:%s",
    soc, voltage, current, tmin, tmax, charging ? "JA" : "NEIN");
}

void obd_query_soh() {
  if (!id(g_obd_enabled)) return;
  if (obd_sock < 0) obd_connect();
  if (obd_sock < 0) return;

  std::string raw = obd_send("220105");
  ESP_LOGI("OBD", "RAW 2105: %s", raw.c_str());

  if (raw.find("CANERROR") != std::string::npos ||
      raw.find("NODATA") != std::string::npos ||
      raw.empty()) return;

  auto b = parse_response(raw, "620105");

  // Byte-Dump 2105
  for (int i = 0; i < (int)b.size(); i++) {
    ESP_LOGI("OBD", "2105 b[%d] = 0x%02X (%d)", i, b[i], b[i]);
  }

  if (b.size() < 36) {
    ESP_LOGW("OBD", "2105 zu wenig Bytes (%d)", b.size());
    return;
  }

  float soh    = b[34] / 2.0f;
  float cycles = (b[25] << 8) | b[26];
  id(g_soh)           = soh;
  id(g_charge_cycles) = cycles;
  ESP_LOGI("OBD", "SOH: %.1f%% | Zyklen: %.0f", soh, cycles);
}

void send_data_uart() {
  int rssi = id(wifi_wlan).is_connected() ? (int)id(sensor_wifi_signal).state : 0;
  char buf[200];
  snprintf(buf, sizeof(buf),
    "SOC:%.1f,SOH:%.1f,TAVG:%.1f,TMIN:%.1f,TMAX:%.1f,V:%.1f,I:%.2f,CHG:%d,CYC:%.0f,RSSI:%d\n",
    id(g_soc), id(g_soh), id(g_bat_temp_avg),
    id(g_bat_temp_min), id(g_bat_temp_max),
    id(g_battery_voltage), id(g_battery_current),
    id(g_is_charging) ? 1 : 0, id(g_charge_cycles), rssi);
  id(uart_to_esp2).write_array((const uint8_t*)buf, strlen(buf));
  ESP_LOGD("OBD", "UART gesendet: %s", buf);
}