/*
  MEDUSA - STM32 Blue Pill Wokwi controller simulation

  Wokwi substitutions:
    PA0 target potentiometer       -> desired position
    PA1 feedback potentiometer     -> Hall-sensor position feedback
    PA8 PWM LED                    -> right-coil driver command
    PB6 PWM LED                    -> left-coil driver command
    PB12 status LED                -> target reached / delivery enabled

  This validates ADC scaling, discrete PID timing, PWM direction, arrival
  detection, and serial telemetry. It does not simulate magnetic motion.
*/

constexpr uint8_t TARGET_PIN = PA0;
constexpr uint8_t FEEDBACK_PIN = PA1;
constexpr uint8_t RIGHT_PWM_PIN = PA8;
constexpr uint8_t LEFT_PWM_PIN = PB6;
constexpr uint8_t ARRIVAL_LED_PIN = PB12;

constexpr unsigned long CONTROL_PERIOD_MS = 10;
constexpr int ADC_MAX = 4095;              // 12-bit STM32 ADC
constexpr float ARRIVAL_TOLERANCE = 0.010; // 1% of the virtual travel range

// Firmware gains use normalised ADC position (-1.0..+1.0), so they are not
// numerically identical to the SI-unit gains from the Python emulator.
constexpr float KP = 250.0f;
constexpr float KI = 35.0f;
constexpr float KD = 15.0f;

float integral = 0.0f;
float previousFeedback = 0.0f;
unsigned long previousControlMs = 0;

float normaliseAdc(int adcValue) {
  return (static_cast<float>(adcValue) / ADC_MAX) * 2.0f - 1.0f;
}

void writeDifferentialPwm(float command) {
  int duty = constrain(static_cast<int>(fabs(command)), 0, 255);

  if (command >= 0.0f) {
    analogWrite(RIGHT_PWM_PIN, duty);
    analogWrite(LEFT_PWM_PIN, 0);
  } else {
    analogWrite(RIGHT_PWM_PIN, 0);
    analogWrite(LEFT_PWM_PIN, duty);
  }
}

void setup() {
  analogReadResolution(12);
  pinMode(RIGHT_PWM_PIN, OUTPUT);
  pinMode(LEFT_PWM_PIN, OUTPUT);
  pinMode(ARRIVAL_LED_PIN, OUTPUT);
  digitalWrite(ARRIVAL_LED_PIN, LOW);

  Serial.begin(115200);
  Serial.println("target,feedback,error,pwm,status");
}

void loop() {
  unsigned long now = millis();
  if (now - previousControlMs < CONTROL_PERIOD_MS) {
    return;
  }
  previousControlMs = now;

  const float dt = CONTROL_PERIOD_MS / 1000.0f;
  const int targetRaw = analogRead(TARGET_PIN);
  const int feedbackRaw = analogRead(FEEDBACK_PIN);
  const float target = normaliseAdc(targetRaw);
  const float feedback = normaliseAdc(feedbackRaw);
  const float error = target - feedback;
  const float derivative = -(feedback - previousFeedback) / dt;
  previousFeedback = feedback;

  const float proposedIntegral = integral + error * dt;
  const float rawCommand = KP * error + KI * proposedIntegral + KD * derivative;
  const float command = constrain(rawCommand, -255.0f, 255.0f);

  // Anti-wind-up: do not keep integrating while the PWM output is saturated.
  if (fabs(rawCommand) < 255.0f) {
    integral = proposedIntegral;
  }

  writeDifferentialPwm(command);
  const bool arrived = fabs(error) <= ARRIVAL_TOLERANCE;
  digitalWrite(ARRIVAL_LED_PIN, arrived ? HIGH : LOW);

  // CSV output is ready for Wokwi Serial Plotter or later CSV comparison.
  Serial.print(target, 4);
  Serial.print(',');
  Serial.print(feedback, 4);
  Serial.print(',');
  Serial.print(error, 4);
  Serial.print(',');
  Serial.print(command, 1);
  Serial.print(',');
  Serial.println(arrived ? 1 : 0);
}