extern "C" {
  #include "ABS_Controller.h"
}

const int ABS_ONPin = D12;
const int slpPin = PA1;
const int ppPin = PA5;
const int brakeLedPin = D5;

void setup() {
  Serial.begin(115200);

  pinMode(ABS_ONPin, INPUT);
  pinMode(slpPin, INPUT);
  pinMode(ppPin, INPUT);
  pinMode(brakeLedPin, OUTPUT);

  ABS_Controller_initialize();
}

void loop() {

  // INPUT
  int rawABS_ON = digitalRead(ABS_ONPin);
  int rawslp = analogRead(slpPin);
  int rawpp = analogRead(ppPin);

  // CONVERT
  bool ABS_ON = rawABS_ON;
  float slp = rawslp / 1023.0;
  float pp  = rawpp / 1023.0;

  // MODEL INPUT
  ABS_Controller_U.ABS_ON = ABS_ON;
  ABS_Controller_U.slp = slp;
  ABS_Controller_U.pp = pp;

  // STEP
  ABS_Controller_step();

  // OUTPUT
  bool valveState = ABS_Controller_Y.controllerout_Hansome;

  if (pp >= slp && valveState == 1) {
    digitalWrite(brakeLedPin, HIGH);
  } else {
    digitalWrite(brakeLedPin, LOW);
  }

}  // ✅ ปิด loop ถูกต้อง