#ifndef HEAVE_CONTROLLER_HPP
#define HEAVE_CONTROLLER_HPP

#include <stdint.h>
#include "IndSensorLUT.hpp"

// ── Pin Mapping (mirrors Controller.hpp) ─────────────────────
#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 HEAVE_CAP 250

// ── Feedforward LUT (gap mm → equilibrium PWM) ───────────────
// Source: FF_PWM_LUT in Controller.cpp (pod 9.4 kg, R 1.1 Ω, V 12 V).
// Positive = repel, negative = attract. Lives in PROGMEM.
#define HEAVE_FF_LUT_SIZE 64
#define HEAVE_FF_GAP_MIN  3.0f
#define HEAVE_FF_GAP_MAX  20.0f
#define HEAVE_FF_GAP_STEP 0.269841f

// ── PID Gains / State ────────────────────────────────────────
typedef struct HeavePIDGains {
  float kp;
  float ki;
  float kd;
} HeavePIDGains;

typedef struct HeavePIDState {
  float e;
  float eDiff;
  float eInt;
} HeavePIDState;

// ── Heave-only Controller ────────────────────────────────────
// Single PID on the average gap across all four inductive sensors.
// Drives all four motor channels with the same PWM magnitude + direction.
class HeaveController {
public:
  bool oor;
  bool outputOn;

  HeaveController(IndSensorL& f, IndSensorL& b,
                  HeavePIDGains gains, float avgRef,
                  bool useFeedforward = true);

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

  void updatePID(HeavePIDGains gains);
  void updateReference(float avgReference);

  // Manual override: drive all channels at -HEAVE_CAP (full attract),
  // bypassing the PID. OOR still zeroes the output via sendOutputs().
  void setFullAttract(bool enabled);
  bool isFullAttract() const { return fullAttract; }

  void setFeedforward(bool enabled) { ffEnabled = enabled; }

private:
  int16_t pidCompute();
  int16_t feedforward(float gapMM);

  IndSensorL& Front;
  IndSensorL& Back;

  HeavePIDGains gains;
  HeavePIDState state;

  float AvgRef;
  float avg;

  int16_t PWM;
  int16_t ffPWM;       // last feedforward value (for debugging/reporting)
  bool fullAttract;
  bool ffEnabled;
};

#endif // HEAVE_CONTROLLER_HPP