\
/****************************************************
 * ROBOT V2
 * ESP32-C6 + NEO-M8N + MPU6050 + MMC5883MA + BMP180
 * Sender data til Home Assistant via REST
 ****************************************************/

#include <WiFi.h>
#include <HTTPClient.h>
#include <Wire.h>
#include <TinyGPSPlus.h>
#include <Adafruit_BMP085.h>
#include <MPU6050_tockn.h>
#include <math.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";

// ---------------- PINS ----------------
const int I2C_SDA_PIN = 6;
const int I2C_SCL_PIN = 7;

const int GPS_RX_PIN = 17; // ESP RX <- GPS TX
const int GPS_TX_PIN = 16; // ESP TX -> GPS RX

// ---------------- GPS ----------------
HardwareSerial GPSserial(1);
TinyGPSPlus gps;

// ---------------- BMP180 ----------------
Adafruit_BMP085 bmp;

// ---------------- MPU6050 ----------------
MPU6050 mpu6050(Wire);

// ---------------- MMC5883MA ----------------
const uint8_t MMC5883_ADDR = 0x30;

// MMC5883MA registers
const uint8_t MMC_REG_XOUT_L   = 0x00;
const uint8_t MMC_REG_STATUS   = 0x07;
const uint8_t MMC_REG_CTRL_0   = 0x08;
const uint8_t MMC_REG_CTRL_1   = 0x09;

// ---------------- Logic ----------------
bool isMoving = false;
bool gpsFix = false;

double acceptedLat = 0.0;
double acceptedLon = 0.0;
double acceptedSpeedKmh = 0.0;
double acceptedHdop = 99.9;
int acceptedSats = 0;
float headingDeg = 0.0;
unsigned long lastAcceptedFixMs = 0;

unsigned long lastImuMs = 0;
unsigned long lastGpsLogicMs = 0;
unsigned long lastPushMs = 0;

const float MOTION_THRESHOLD = 0.08f;
const unsigned long STILL_HOLD_MS = 3000;
unsigned long lastMotionDetectedMs = 0;

// ---------------- Utils ----------------
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();
    }
  }
}

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);
}

// ---------------- MMC5883MA helpers ----------------
void mmcWrite8(uint8_t reg, uint8_t val) {
  Wire.beginTransmission(MMC5883_ADDR);
  Wire.write(reg);
  Wire.write(val);
  Wire.endTransmission();
}

uint8_t mmcRead8(uint8_t reg) {
  Wire.beginTransmission(MMC5883_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom((int)MMC5883_ADDR, 1);
  if (!Wire.available()) return 0;
  return Wire.read();
}

void mmcReadXYZ(uint16_t &x, uint16_t &y, uint16_t &z) {
  // Start measurement
  mmcWrite8(MMC_REG_CTRL_0, 0x01); // TM_M
  delay(10);

  // Wait until measurement done
  unsigned long start = millis();
  while (!(mmcRead8(MMC_REG_STATUS) & 0x01)) {
    if (millis() - start > 20) break;
    delay(1);
  }

  Wire.beginTransmission(MMC5883_ADDR);
  Wire.write(MMC_REG_XOUT_L);
  Wire.endTransmission(false);
  Wire.requestFrom((int)MMC5883_ADDR, 6);

  if (Wire.available() >= 6) {
    uint8_t xl = Wire.read();
    uint8_t xh = Wire.read();
    uint8_t yl = Wire.read();
    uint8_t yh = Wire.read();
    uint8_t zl = Wire.read();
    uint8_t zh = Wire.read();

    x = ((uint16_t)xh << 8) | xl;
    y = ((uint16_t)yh << 8) | yl;
    z = ((uint16_t)zh << 8) | zl;
  } else {
    x = y = z = 0;
  }
}

void mmcInit() {
  // Optional SET/RESET cycle
  mmcWrite8(MMC_REG_CTRL_0, 0x08); // SET
  delay(2);
  mmcWrite8(MMC_REG_CTRL_0, 0x10); // RESET
  delay(2);

  // BW = 00 default 16-bit
  mmcWrite8(MMC_REG_CTRL_1, 0x00);
}

// Raw unsigned output is centered around 32768 counts
float mmcReadHeading() {
  uint16_t rx, ry, rz;
  mmcReadXYZ(rx, ry, rz);

  float x = (float)rx - 32768.0f;
  float y = (float)ry - 32768.0f;

  float heading = atan2f(y, x) * 180.0f / PI;
  if (heading < 0) heading += 360.0f;
  return heading;
}

// ---------------- Motion detect ----------------
void updateMotion() {
  mpu6050.update();

  float ax = mpu6050.getAccX();
  float ay = mpu6050.getAccY();
  float az = mpu6050.getAccZ();

  float magnitude = sqrtf(ax * ax + ay * ay + az * az);
  float deviation = fabsf(magnitude - 1.0f);

  if (deviation > MOTION_THRESHOLD) {
    lastMotionDetectedMs = millis();
    isMoving = true;
  } else {
    if (millis() - lastMotionDetectedMs > STILL_HOLD_MS) {
      isMoving = false;
    }
  }
}

// ---------------- GPS logic ----------------
void processGPS() {
  while (GPSserial.available()) {
    gps.encode(GPSserial.read());
  }

  if (millis() - lastGpsLogicMs < 1000) return;
  lastGpsLogicMs = millis();

  bool validLocation = gps.location.isValid();
  int sats = gps.satellites.isValid() ? gps.satellites.value() : 0;
  double hdop = gps.hdop.isValid() ? gps.hdop.hdop() : 99.9;

  gpsFix = validLocation && sats >= 4 && hdop <= 5.0;

  if (isMoving && gpsFix) {
    acceptedLat = gps.location.lat();
    acceptedLon = gps.location.lng();
    acceptedSpeedKmh = gps.speed.isValid() ? gps.speed.kmph() : 0.0;
    acceptedHdop = hdop;
    acceptedSats = sats;
    lastAcceptedFixMs = millis();
  }

  if (lastAcceptedFixMs == 0 && gpsFix) {
    acceptedLat = gps.location.lat();
    acceptedLon = gps.location.lng();
    acceptedSpeedKmh = gps.speed.isValid() ? gps.speed.kmph() : 0.0;
    acceptedHdop = hdop;
    acceptedSats = sats;
    lastAcceptedFixMs = millis();
  }

  if (isMoving) {
    headingDeg = mmcReadHeading();
  }
}

// ---------------- Push to HA ----------------
void pushToHA() {
  unsigned long ageSec = (lastAcceptedFixMs == 0) ? 999999 : ((millis() - lastAcceptedFixMs) / 1000);

  float tempC = bmp.readTemperature();
  float pressureHpa = bmp.readPressure() / 100.0f;

  postState("sensor.robot_gps_fix_raw", gpsFix ? "on" : "off");
  postState("sensor.robot_motion_raw", isMoving ? "on" : "off");

  postState("sensor.robot_gps_latitude", String(acceptedLat, 6));
  postState("sensor.robot_gps_longitude", String(acceptedLon, 6));
  postState("sensor.robot_gps_satellites", String(acceptedSats));
  postState("sensor.robot_gps_hdop", String(acceptedHdop, 1));
  postState("sensor.robot_gps_speed_kmh", String(acceptedSpeedKmh, 1), "km/h", "speed", "measurement");
  postState("sensor.robot_heading_deg", String(headingDeg, 1), "°");
  postState("sensor.robot_last_position_age", String(ageSec), "s", "duration", "measurement");
  postState("sensor.robot_temperature", String(tempC, 1), "°C", "temperature", "measurement");
  postState("sensor.robot_pressure", String(pressureHpa, 1), "hPa", "atmospheric_pressure", "measurement");
}

void setup() {
  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
  Wire.setClock(400000);

  wifiConnect();

  GPSserial.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);

  if (!bmp.begin()) {
    // BMP180 ikke fundet - fortsæt alligevel
  }

  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);

  mmcInit();

  lastMotionDetectedMs = millis();
}

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

  if (millis() - lastImuMs >= 50) {
    lastImuMs = millis();
    updateMotion();
  }

  processGPS();

  if (millis() - lastPushMs >= 5000) {
    lastPushMs = millis();
    pushToHA();
  }
}