#include <Arduino.h>
#include "IndSensorLUT.hpp"
#include "HeaveController.hpp"
#include "ADC.hpp"
#include "FastPWM.hpp"

// ── PID Gains (Kp, Ki, Kd) ──────────────────────────────────
HeavePIDGains heaveGains = { 400.0f, 0.0f, 300.0f };

// ── Reference ────────────────────────────────────────────────
float avgRef = 12.2f;  // Target gap height (mm)

// ── Sampling ─────────────────────────────────────────────────
#define SAMPLING_RATE 200  // Hz

// ── EMA filter alpha (all sensors) ───────────────────────────
#define ALPHA_VAL 0.7f

// ═══════════════════════════════════════════════════════════════
// ABOVE THIS LINE IS TUNING VALUES ONLY, BELOW IS ACTUAL CODE.
// ═══════════════════════════════════════════════════════════════

unsigned long tprior;
unsigned int tDiffMicros;

HeaveController controller(indF, indB, heaveGains, avgRef);

const int dt_micros = 1000000 / SAMPLING_RATE;

void setup() {
  Serial.begin(2000000);
  setupADC();
  setupFastPWM();

  indF.alpha = ALPHA_VAL;
  indB.alpha = ALPHA_VAL;

  tprior = micros();

  pinMode(dirFL, OUTPUT); pinMode(pwmFL, OUTPUT);
  pinMode(dirBL, OUTPUT); pinMode(pwmBL, OUTPUT);
  pinMode(dirFR, OUTPUT); pinMode(pwmFR, OUTPUT);
  pinMode(dirBR, OUTPUT); pinMode(pwmBR, OUTPUT);
}

// Dispatch a newline-terminated command line.
void handleCommand(char* line) {
  switch (line[0]) {
    case '0':
      controller.outputOn = false;
      controller.setFullAttract(false);
      break;
    case '1':
      controller.outputOn = true;
      controller.setFullAttract(false);
      break;
    case '2':
      controller.outputOn = true;
      controller.setFullAttract(true);
      break;
    case 'R': {
      avgRef = atof(line + 1);
      controller.updateReference(avgRef);
      break;
    }
    case 'P': {
      char* kpTok = strtok(line + 1, ",");
      char* kiTok = strtok(NULL, ",");
      char* kdTok = strtok(NULL, ",");
      if (kpTok && kiTok && kdTok) {
        heaveGains = { (float)atof(kpTok),
                       (float)atof(kiTok),
                       (float)atof(kdTok) };
        controller.updatePID(heaveGains);
      }
      break;
    }
    case 'F':
      // F1 / F0 — toggle feedforward
      controller.setFeedforward(line[1] != '0');
      break;
    default:
      break;
  }
}

void loop() {
  // Serial commands (all newline-terminated):
  //   0            → output off
  //   1            → output on, PID control
  //   2            → output on, full attract
  //   R<float>         → update reference (mm)     e.g.  R12.5
  //   P<kp>,<ki>,<kd>  → update PID gains          e.g.  P10,0,8
  //   F1 / F0          → feedforward on/off
  static char lineBuf[40];
  static uint8_t lineLen = 0;
  while (Serial.available() > 0) {
    char c = Serial.read();
    if (c == '\n' || c == '\r') {
      if (lineLen == 0) continue;
      lineBuf[lineLen] = '\0';
      handleCommand(lineBuf);
      lineLen = 0;
    } else if (lineLen < sizeof(lineBuf) - 1) {
      lineBuf[lineLen++] = c;
    }
  }

  tDiffMicros = micros() - tprior;

  if (tDiffMicros >= dt_micros) {
    controller.update();
    controller.report();
    controller.sendOutputs();

    tprior = micros();
  }
}