#ifndef CONTROLLER_HPP
#define CONTROLLER_HPP

#include <stdint.h>
#include <avr/pgmspace.h>
#include "IndSensorMap.hpp"

// ── Pin Mapping ──────────────────────────────────────────────
#define dirBL 2
#define pwmBL 3
#define dirBR 4
#define pwmBR 10
#define pwmFL 11
#define dirFL 7
#define dirFR 8
#define pwmFR 9

// ── Output Cap ───────────────────────────────────────────────
#define CAP 250   // Max PWM magnitude (0-255 Arduino range)

// ── PID Gains (single set per loop — matches simulation) ────
typedef struct PIDGains {
  float kp;
  float ki;
  float kd;
} PIDGains;

// ── PID Error State ──────────────────────────────────────────
typedef struct PIDState {
  float e;        // Current error
  float eDiff;    // Derivative of error (e[k] - e[k-1])
  float eInt;     // Integral of error (accumulated)
} PIDState;

// ── Feedforward LUT (PROGMEM) ────────────────────────────────
// Generated by gen_ff_lut.py from MaglevPredictor model
// Pod mass: 9.4 kg, Coil R: 1.1Ω, V_supply: 12V
// Positive = repelling, Negative = attracting
// Beyond ~16mm: clamped to -CAP (no equilibrium exists)
#define FF_LUT_SIZE 64
#define FF_GAP_MIN  3.0f
#define FF_GAP_MAX  20.0f
#define FF_GAP_STEP 0.269841f

// ── Geometry (mm, matching simulation) ───────────────────────
// Sensor-to-sensor distances for angle computation
#define Y_DISTANCE_MM 101.6f   // Left↔Right sensor spacing (mm)
#define X_DISTANCE_MM 251.8f   // Front↔Back sensor spacing (mm)

// ── Controller Class ─────────────────────────────────────────
class FullController {
public:
  bool oor;        // Any sensor out-of-range
  bool outputOn;   // Enable/disable output

  FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
                 PIDGains heightGains, PIDGains rollGains, PIDGains pitchGains,
                 float avgRef, bool useFeedforward);

  void update();
  void zeroPWMs();
  void sendOutputs();
  void report();

  // Runtime tuning
  void updateHeightPID(PIDGains gains);
  void updateRollPID(PIDGains gains);
  void updatePitchPID(PIDGains gains);
  void updateReference(float avgReference);
  void setFeedforward(bool enabled);

private:
  int16_t pidCompute(PIDGains& gains, PIDState& state, float maxOutput);
  int16_t feedforward(float gapMM);

  IndSensor& Left;
  IndSensor& Right;
  IndSensor& Front;
  IndSensor& Back;

  PIDGains heightGains;
  PIDGains rollGains;
  PIDGains pitchGains;

  PIDState heightErr;
  PIDState rollErr;
  PIDState pitchErr;

  float AvgRef;     // Target gap height (mm)
  float avg;        // Current average gap (mm)
  bool  ffEnabled;  // Feedforward on/off

  int16_t FLPWM;
  int16_t BLPWM;
  int16_t FRPWM;
  int16_t BRPWM;
};

#endif // CONTROLLER_HPP