extern "C" {
  #include "ABS_Controller.h" // ต้องใช้ extern "C" เพื่อให้ C++ เรียก C ได้
}

// กำหนดขา Pin สำหรับจำลอง
const int ABS_ONPin = D12;  // รับค่า '<Root>/ABS_ON'
const int slpPin = PA1; // รับค่า '<Root>/slp'
const int slp_b_Pin = PA7; // รับค่า '<Root>/slp'
const int brakeLedPin = D5;  // LED แสดงสถานะวาล์วเบรก (Dominant/Recessive)

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, STM32!");

// 1. กำหนดโหมดของ Pin
  pinMode(ABS_ONPin, INPUT);
  pinMode(slpPin, INPUT);
  pinMode(slp_b_Pin, INPUT);
  pinMode(brakeLedPin, OUTPUT);

  // 2. เรียกฟังก์ชัน Initialize ของ Model 
  ABS_Controller_initialize();

}

void loop() {
  // put your main code here, to run repeatedly:
  // --- ช่วงที่ 1: รับค่า Input จากโลกจริง (Simulator) ---
  int rawABS_ON   = digitalRead(ABS_ONPin);
  int rawslp = analogRead(slpPin); 
  int rawslp_b = analogRead(slp_b_Pin); 

  // แปลงค่า Analog (0-1023) ให้เป็น Unit ที่ Model ต้องการ 
  bool ABS_ON = rawABS_ON;
  float slp   = (rawslp / 1023.0) * 1; // Slp สูงสุด 1
  float slp_b   = (rawslp_b / 1023.0) * 1; // Slp สูงสุด 1
  
  // --- ช่วงที่ 2: ส่งค่าเข้า Model ---
  ABS_Controller_U.ABS_ON = ABS_ON;
  ABS_Controller_U.slp = slp;
  ABS_Controller_U.slp_b = slp_b;

  // --- ช่วงที่ 3: ประมวลผล Logic (Step Function) ---
  ABS_Controller_step();

  // --- ช่วงที่ 4: นำค่า Output ไปสั่งการ Hardware ---
  // bool valveState = ABS_Controller_Y.controllerout; // รับค่าผลลัพธ์จาก Model
  // bool data type = 0 / 1;
  // ABS_Controller_Y.controllerout (1) -> valveState(1)
  // <Bug> 
  // ABS_Controller_Y.controllerout (-1) -> valveState(1)
  bool valveState = ABS_Controller_Y.controllerout_Hansome; // รับค่าผลลัพธ์จาก Model

  digitalWrite(brakeLedPin, valveState);

  delay(10); // this speeds up the simulation
  //Debug
  int static Debug_cnt;
  Debug_cnt++;
  if (Debug_cnt > 100) // 1 sec
  {
    Debug_cnt = 0;
    Serial.print("ABS_ON : ");
    Serial.println(ABS_ON);
    Serial.print("slp : ");
    Serial.println(slp);
    Serial.print("slp_b : ");
    Serial.println(slp_b);
    Serial.print("valveState : ");
    Serial.println(valveState);
    Serial.print("ABS_Controller_Y.controllerout : ");
    Serial.println(ABS_Controller_Y.controllerout);
    Serial.print("ABS_Controller_Y.controllerout_Hansome : ");
    Serial.println(ABS_Controller_Y.controllerout_Hansome);
    Serial.print("rawslp_b : ");
    Serial.println(rawslp_b);
     Serial.print("rawslp : ");
    Serial.println(rawslp);
  }

}