/*
  Arca exact-Amazon-cart prototype firmware

  Controller: Seeed Studio XIAO ESP32-S3 (ASIN B0BYSB66S5)
  Optical:   AS7341 breakout (ASIN B0DBQKDV67)
  Motion:    LSM6DS3 breakout (ASIN B0FKT9ZR2X)
  Temp:      TMP36GT9Z, TO-92 (ASIN B07LG758H3)
  Identity:  passive 25 mm NTAG215 sticker, programmed separately by phone

  USB powers the presentation prototype. No battery is assumed.

  Serial commands at 115200 baud:
    e  capture current spectrum as EMPTY baseline
    f  capture current spectrum as FILLED baseline
    r  clear both optical baselines

  Output is newline-delimited JSON compatible with the Arca OS demo.
*/

#include <Wire.h>
#include <math.h>
#include <Adafruit_AS7341.h>

static const char TUBE_ID[] = "ARC-DEMO-001";
static const int TEMP_PIN = A0;  // XIAO D0 / A0
static const bool NFC_TAG_PROGRAMMED = false;  // set true after phone programming

static const uint8_t LSM6DS3_ADDR_LOW = 0x6A;
static const uint8_t LSM6DS3_ADDR_HIGH = 0x6B;
static const uint8_t LSM6DS3_WHO_AM_I = 0x0F;
static const uint8_t LSM6DS3_CTRL1_XL = 0x10;
static const uint8_t LSM6DS3_CTRL2_G = 0x11;
static const uint8_t LSM6DS3_CTRL3_C = 0x12;
static const uint8_t LSM6DS3_OUTX_L_XL = 0x28;

Adafruit_AS7341 optical;

bool opticalReady = false;
bool temperatureReady = false;
bool motionReady = false;
uint8_t motionAddress = 0;

uint16_t spectrum[8] = {0};
float emptyBaseline[8] = {0};
float filledBaseline[8] = {0};
bool emptyCalibrated = false;
bool filledCalibrated = false;

float temperatureC = 0.0f;
float accelG = 0.0f;
float dynamicG = 0.0f;
uint32_t handlingEvents = 0;
uint32_t lastHandlingEventMs = 0;
uint32_t lastReportMs = 0;

const as7341_color_channel_t CHANNELS[8] = {
  AS7341_CHANNEL_415nm_F1,
  AS7341_CHANNEL_445nm_F2,
  AS7341_CHANNEL_480nm_F3,
  AS7341_CHANNEL_515nm_F4,
  AS7341_CHANNEL_555nm_F5,
  AS7341_CHANNEL_590nm_F6,
  AS7341_CHANNEL_630nm_F7,
  AS7341_CHANNEL_680nm_F8
};

bool i2cWriteRegister(uint8_t address, uint8_t reg, uint8_t value) {
  Wire.beginTransmission(address);
  Wire.write(reg);
  Wire.write(value);
  return Wire.endTransmission() == 0;
}

bool i2cReadRegisters(uint8_t address, uint8_t reg, uint8_t *data, size_t length) {
  Wire.beginTransmission(address);
  Wire.write(reg);
  if (Wire.endTransmission(false) != 0) return false;
  const size_t received = Wire.requestFrom(address, (uint8_t)length);
  if (received != length) return false;
  for (size_t i = 0; i < length; ++i) data[i] = Wire.read();
  return true;
}

bool beginMotionAt(uint8_t address) {
  uint8_t who = 0;
  if (!i2cReadRegisters(address, LSM6DS3_WHO_AM_I, &who, 1)) return false;
  if (who != 0x69) return false;
  // 104 Hz, +/-2 g accelerometer; 104 Hz, 245 dps gyro.
  // BDU and register auto-increment are enabled for stable burst reads.
  return i2cWriteRegister(address, LSM6DS3_CTRL3_C, 0x44) &&
         i2cWriteRegister(address, LSM6DS3_CTRL1_XL, 0x40) &&
         i2cWriteRegister(address, LSM6DS3_CTRL2_G, 0x40);
}

void beginMotion() {
  if (beginMotionAt(LSM6DS3_ADDR_LOW)) {
    motionAddress = LSM6DS3_ADDR_LOW;
    motionReady = true;
  } else if (beginMotionAt(LSM6DS3_ADDR_HIGH)) {
    motionAddress = LSM6DS3_ADDR_HIGH;
    motionReady = true;
  }
}

void readOptical() {
  if (!opticalReady || !optical.readAllChannels()) return;
  for (int i = 0; i < 8; ++i) spectrum[i] = optical.getChannel(CHANNELS[i]);
}

void readTemperature() {
  if (!temperatureReady) return;
  uint32_t millivolts = 0;
  for (int i = 0; i < 16; ++i) millivolts += analogReadMilliVolts(TEMP_PIN);
  const float averageMillivolts = millivolts / 16.0f;
  temperatureC = (averageMillivolts - 500.0f) / 10.0f;
}

void readMotion() {
  if (!motionReady) return;
  uint8_t bytes[6];
  if (!i2cReadRegisters(motionAddress, LSM6DS3_OUTX_L_XL, bytes, sizeof(bytes))) return;

  const int16_t rawX = (int16_t)((uint16_t)bytes[1] << 8 | bytes[0]);
  const int16_t rawY = (int16_t)((uint16_t)bytes[3] << 8 | bytes[2]);
  const int16_t rawZ = (int16_t)((uint16_t)bytes[5] << 8 | bytes[4]);
  const float scaleG = 0.000061f;  // LSM6DS3 at +/-2 g
  const float xG = rawX * scaleG;
  const float yG = rawY * scaleG;
  const float zG = rawZ * scaleG;
  accelG = sqrtf(xG * xG + yG * yG + zG * zG);
  dynamicG = fabsf(accelG - 1.0f);

  const uint32_t now = millis();
  if (dynamicG > 0.65f && now - lastHandlingEventMs > 650) {
    ++handlingEvents;
    lastHandlingEventMs = now;
  }
}

void captureBaseline(float target[8]) {
  for (int i = 0; i < 8; ++i) target[i] = (float)spectrum[i];
}

float normalizedDistance(const uint16_t sample[8], const float baseline[8]) {
  float sampleNorm = 0.0f;
  float baselineNorm = 0.0f;
  for (int i = 0; i < 8; ++i) {
    sampleNorm += (float)sample[i] * (float)sample[i];
    baselineNorm += baseline[i] * baseline[i];
  }
  sampleNorm = sqrtf(sampleNorm);
  baselineNorm = sqrtf(baselineNorm);
  if (sampleNorm < 1.0f || baselineNorm < 1.0f) return 999.0f;

  float sum = 0.0f;
  for (int i = 0; i < 8; ++i) {
    const float delta = ((float)sample[i] / sampleNorm) - (baseline[i] / baselineNorm);
    sum += delta * delta;
  }
  return sqrtf(sum);
}

float fillScore() {
  if (!emptyCalibrated || !filledCalibrated) return -1.0f;
  const float distanceFromEmpty = normalizedDistance(spectrum, emptyBaseline);
  const float distanceFromFilled = normalizedDistance(spectrum, filledBaseline);
  const float denominator = distanceFromEmpty + distanceFromFilled;
  if (denominator < 0.0001f) return 50.0f;
  return 100.0f * distanceFromEmpty / denominator;
}

const char *fillStatus(float score) {
  if (score < 0.0f) return "CALIBRATE";
  if (score < 40.0f) return "EMPTY";
  if (score > 60.0f) return "FILLED";
  return "TRANSITION";
}

void handleSerialCommands() {
  while (Serial.available()) {
    const char command = (char)Serial.read();
    if (command == 'e' || command == 'E') {
      readOptical();
      captureBaseline(emptyBaseline);
      emptyCalibrated = true;
    } else if (command == 'f' || command == 'F') {
      readOptical();
      captureBaseline(filledBaseline);
      filledCalibrated = true;
    } else if (command == 'r' || command == 'R') {
      emptyCalibrated = false;
      filledCalibrated = false;
    }
  }
}

void printBoolean(bool value) {
  Serial.print(value ? "true" : "false");
}

void emitJson() {
  const float score = fillScore();
  Serial.print("{\"tube_id\":\"");
  Serial.print(TUBE_ID);
  Serial.print("\",\"uptime_ms\":");
  Serial.print(millis());
  Serial.print(",\"ready\":{\"optical\":");
  printBoolean(opticalReady);
  Serial.print(",\"temperature\":");
  printBoolean(temperatureReady);
  Serial.print(",\"motion\":");
  printBoolean(motionReady);
  Serial.print(",\"nfc\":");
  printBoolean(NFC_TAG_PROGRAMMED);
  Serial.print("},\"nfc_written\":");
  printBoolean(NFC_TAG_PROGRAMMED);
  Serial.print(",\"temperature_c\":");
  Serial.print(temperatureC, 2);
  Serial.print(",\"accel_g\":");
  Serial.print(accelG, 3);
  Serial.print(",\"dynamic_g\":");
  Serial.print(dynamicG, 3);
  Serial.print(",\"handling_events\":");
  Serial.print(handlingEvents);
  Serial.print(",\"fill_score\":");
  Serial.print(score, 1);
  Serial.print(",\"fill_status\":\"");
  Serial.print(fillStatus(score));
  Serial.print("\",\"cal_empty\":");
  printBoolean(emptyCalibrated);
  Serial.print(",\"cal_filled\":");
  printBoolean(filledCalibrated);
  Serial.print(",\"spectral\":[");
  for (int i = 0; i < 8; ++i) {
    if (i) Serial.print(',');
    Serial.print(spectrum[i]);
  }
  Serial.println("]}");
}

void setup() {
  Serial.begin(115200);
  delay(700);
  Wire.begin();  // XIAO ESP32-S3 defaults: D4=SDA, D5=SCL
  Wire.setClock(100000);

  analogReadResolution(12);
  temperatureReady = true;

  opticalReady = optical.begin(AS7341_I2CADDR_DEFAULT, &Wire);
  if (opticalReady) {
    optical.setATIME(100);
    optical.setASTEP(999);
    optical.setGain(AS7341_GAIN_128X);
  }
  beginMotion();
}

void loop() {
  handleSerialCommands();
  readOptical();
  readTemperature();
  readMotion();

  const uint32_t now = millis();
  if (now - lastReportMs >= 250) {
    emitJson();
    lastReportMs = now;
  }
  delay(10);
}