\
/****************************************************
 * LADEBOKS V2
 * ESP8266 + INA226 + 20x4 LCD
 * Måler V/A/W
 * Henter robotstatus fra HA
 ****************************************************/

#include <ESP8266WiFi.h>
#include <ESP8266HTTPClient.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <time.h>

// ---------------- WIFI / HA ----------------
const char* WIFI_SSID     = "DIT_WIFI";
const char* WIFI_PASSWORD = "DIT_PASS";

const char* HA_HOST       = "192.168.2.10";
const uint16_t HA_PORT    = 8123;
const char* HA_TOKEN      = "DIN_LONG_LIVED_ACCESS_TOKEN";

// ---------------- LCD ----------------
LiquidCrystal_I2C lcd(0x27, 20, 4);

// ---------------- I2C pins ----------------
const uint8_t I2C_SDA = D2;
const uint8_t I2C_SCL = D1;

// ---------------- INA226 ----------------
const uint8_t INA226_ADDR = 0x40;
const uint8_t REG_CONFIG      = 0x00;
const uint8_t REG_BUS_VOLT    = 0x02;
const uint8_t REG_POWER       = 0x03;
const uint8_t REG_CURRENT     = 0x04;
const uint8_t REG_CALIBRATION = 0x05;

const float SHUNT_OHMS = 0.01f;
const float CURRENT_LSB = 0.0005f;
const uint16_t INA226_CAL = (uint16_t)(0.00512f / (CURRENT_LSB * SHUNT_OHMS));

// ---------------- Buffers ----------------
const int FILTER_SAMPLES = 10;
float voltsBuf[FILTER_SAMPLES] = {0};
float ampsBuf[FILTER_SAMPLES]  = {0};
float wattsBuf[FILTER_SAMPLES] = {0};
int filterIndex = 0;
bool filterFilled = false;

// ---------------- Status fra HA ----------------
String gpsStatus = "NOFX";
String motionStatus = "STOP";
String homeStatus = "AWAY";
String chargeStatus = "IDLE";

// ---------------- Timing ----------------
unsigned long lastMeasureMs = 0;
unsigned long lastDisplayMs = 0;
unsigned long lastPushMs = 0;
unsigned long lastFetchMs = 0;

// ---------------- Helpers ----------------
String haBaseUrl() {
  return "http://" + String(HA_HOST) + ":" + String(HA_PORT);
}

void wifiConnect() {
  WiFi.mode(WIFI_STA);
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);

  unsigned long startMs = millis();
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    if (millis() - startMs > 30000) {
      ESP.restart();
    }
  }
}

void ntpSetup() {
  // UTC offset sættes via tzset nedenfor
  configTime(0, 0, "pool.ntp.org", "dk.pool.ntp.org", "time.google.com");
  setenv("TZ", "CET-1CEST,M3.5.0,M10.5.0/3", 1);
  tzset();
}

String nowString() {
  time_t now = time(nullptr);
  struct tm* t = localtime(&now);
  if (!t) return "--- --/-- --:--";
  char buf[21];
  snprintf(buf, sizeof(buf), "%02d-%02d-%04d %02d:%02d",
           t->tm_mday, t->tm_mon + 1, t->tm_year + 1900, t->tm_hour, t->tm_min);
  return String(buf);
}

void writeRegister16(uint8_t reg, uint16_t value) {
  Wire.beginTransmission(INA226_ADDR);
  Wire.write(reg);
  Wire.write((value >> 8) & 0xFF);
  Wire.write(value & 0xFF);
  Wire.endTransmission();
}

uint16_t readRegister16u(uint8_t reg) {
  Wire.beginTransmission(INA226_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom((int)INA226_ADDR, 2);
  if (Wire.available() < 2) return 0;
  return ((uint16_t)Wire.read() << 8) | Wire.read();
}

int16_t readRegister16s(uint8_t reg) {
  Wire.beginTransmission(INA226_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom((int)INA226_ADDR, 2);
  if (Wire.available() < 2) return 0;
  return ((int16_t)Wire.read() << 8) | Wire.read();
}

void ina226Setup() {
  writeRegister16(REG_CONFIG, 0x8000);
  delay(10);

  uint16_t config = 0;
  config |= (0b100 << 9); // avg 16
  config |= (0b100 << 6); // bus conv 1.1ms
  config |= (0b100 << 3); // shunt conv 1.1ms
  config |= (0b111);      // continuous shunt+bus
  writeRegister16(REG_CONFIG, config);

  writeRegister16(REG_CALIBRATION, INA226_CAL);
}

float readBusVoltageV() {
  return readRegister16u(REG_BUS_VOLT) * 0.00125f;
}

float readCurrentA() {
  return readRegister16s(REG_CURRENT) * CURRENT_LSB;
}

float readPowerW() {
  float powerLSB = 25.0f * CURRENT_LSB;
  return readRegister16u(REG_POWER) * powerLSB;
}

void pushFilter(float v, float a, float w) {
  voltsBuf[filterIndex] = v;
  ampsBuf[filterIndex] = a;
  wattsBuf[filterIndex] = w;

  filterIndex++;
  if (filterIndex >= FILTER_SAMPLES) {
    filterIndex = 0;
    filterFilled = true;
  }
}

float avgBuf(float* buf) {
  int count = filterFilled ? FILTER_SAMPLES : filterIndex;
  if (count <= 0) return 0;
  float sum = 0;
  for (int i = 0; i < count; i++) sum += buf[i];
  return sum / count;
}

bool postState(const String& entityId, const String& state, const String& unit = "", const String& devClass = "", const String& stateClass = "") {
  WiFiClient client;
  HTTPClient http;

  String url = haBaseUrl() + "/api/states/" + entityId;
  if (!http.begin(client, url)) return false;

  http.addHeader("Authorization", "Bearer " + String(HA_TOKEN));
  http.addHeader("Content-Type", "application/json");

  String json = "{\"state\":\"" + state + "\",\"attributes\":{";
  bool first = true;

  if (unit.length()) {
    json += "\"unit_of_measurement\":\"" + unit + "\"";
    first = false;
  }
  if (devClass.length()) {
    if (!first) json += ",";
    json += "\"device_class\":\"" + devClass + "\"";
    first = false;
  }
  if (stateClass.length()) {
    if (!first) json += ",";
    json += "\"state_class\":\"" + stateClass + "\"";
    first = false;
  }
  if (!first) json += ",";
  json += "\"friendly_name\":\"" + entityId + "\"";
  json += "}}";

  int code = http.POST(json);
  http.end();
  return (code >= 200 && code < 300);
}

String extractState(const String& payload) {
  int p = payload.indexOf("\"state\":\"");
  if (p < 0) return "unknown";
  p += 9;
  int e = payload.indexOf("\"", p);
  if (e < 0) return "unknown";
  return payload.substring(p, e);
}

String getEntityState(const String& entityId) {
  WiFiClient client;
  HTTPClient http;
  String url = haBaseUrl() + "/api/states/" + entityId;

  if (!http.begin(client, url)) return "unknown";
  http.addHeader("Authorization", "Bearer " + String(HA_TOKEN));
  int code = http.GET();
  String payload = (code > 0) ? http.getString() : "";
  http.end();

  if (code < 200 || code >= 300) return "unknown";
  return extractState(payload);
}

void updateDisplay(float v, float a, float w) {
  // Linje 1: dato/tid
  lcd.setCursor(0, 0);
  lcd.print("                    ");
  lcd.setCursor(0, 0);
  lcd.print(nowString());

  // Linje 2: spænding / strøm
  lcd.setCursor(0, 1);
  lcd.print("U:");
  lcd.print(v, 2);
  lcd.print(" I:");
  lcd.print(a, 2);
  lcd.print("     ");

  // Linje 3: effekt / GPS
  lcd.setCursor(0, 2);
  lcd.print("P:");
  lcd.print(w, 1);
  lcd.print("W GPS:");
  lcd.print(gpsStatus);
  lcd.print("   ");

  // Linje 4: HOME/AWAY CHG/IDLE RUN/STOP
  lcd.setCursor(0, 3);
  lcd.print(homeStatus);
  lcd.print(" ");
  lcd.print(chargeStatus);
  lcd.print(" ");
  lcd.print(motionStatus);
  lcd.print("        ");
}

void setup() {
  Wire.begin(I2C_SDA, I2C_SCL);

  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Starter ladeboks");

  wifiConnect();
  ntpSetup();
  ina226Setup();
}

void loop() {
  if (WiFi.status() != WL_CONNECTED) {
    wifiConnect();
  }

  unsigned long now = millis();

  if (now - lastMeasureMs >= 250) {
    lastMeasureMs = now;
    float v = readBusVoltageV();
    float a = readCurrentA();
    float w = readPowerW();

    if (a < 0) a = 0;
    if (w < 0) w = 0;

    pushFilter(v, a, w);
  }

  if (now - lastFetchMs >= 5000) {
    lastFetchMs = now;

    String gpsRaw = getEntityState("sensor.robot_gps_status");
    String motionRaw = getEntityState("sensor.robot_motion_status");
    String homeRaw = getEntityState("sensor.robot_home_status");
    String chargeRaw = getEntityState("sensor.robot_charge_status");

    gpsStatus = (gpsRaw == "FIX") ? "FIX" : "NOFX";
    motionStatus = (motionRaw == "MOVING") ? "RUN" : "STOP";
    homeStatus = (homeRaw == "HOME") ? "HOME" : "AWAY";
    chargeStatus = (chargeRaw == "CHARGING") ? "CHG" : "IDLE";
  }

  if (now - lastDisplayMs >= 1000) {
    lastDisplayMs = now;
    updateDisplay(avgBuf(voltsBuf), avgBuf(ampsBuf), avgBuf(wattsBuf));
  }

  if (now - lastPushMs >= 5000) {
    lastPushMs = now;
    float v = avgBuf(voltsBuf);
    float a = avgBuf(ampsBuf);
    float w = avgBuf(wattsBuf);

    postState("sensor.robot_dock_voltage", String(v, 2), "V", "voltage", "measurement");
    postState("sensor.robot_dock_current", String(a, 3), "A", "current", "measurement");
    postState("sensor.robot_dock_power", String(w, 1), "W", "power", "measurement");
    postState("binary_sensor.robot_dock_online", "on");
  }
}