#include <Arduino.h>
#include <math.h>

/* --- CONFIGURATION PARAMETERS --- */
#define MAX_ALLOWED_DEVIATION  45.0f     // Maximum angle range for 100% scale
#define THRESHOLD_PERCENTAGE   15.0f     // Alert threshold (15% = 6.75 degrees)

/* --- STM32 NUCLEO C031C6 PIN MAPPING --- */
#define POT_PIN                A0        // Analog input from rotary sensor / potentiometer
#define CALIB_BTN_PIN          2         // Calibration Pushbutton on pin D2 (Active LOW)
#define ALERT_LED_PIN          13        // Alert LED on pin D13

/* --- GLOBAL VARIABLES --- */
float current_angle = 0.0f;
float base_calibrated_angle = 0.0f;
float deviation_pct = 0.0f;

void check_calibration_button();

void setup() {
  Serial.begin(115200);
  
  pinMode(ALERT_LED_PIN, OUTPUT);
  pinMode(POT_PIN, INPUT);
  pinMode(CALIB_BTN_PIN, INPUT_PULLUP); // Active-low button configuration
  
  digitalWrite(ALERT_LED_PIN, LOW);
  
  Serial.println("=============================================");
  Serial.println("  STM32 C031C6 POSTURE CORRECTION SYSTEM     ");
  Serial.println("=============================================");
  Serial.println("1. Adjust potentiometer to simulate posture tilt.");
  Serial.println("2. Press Button (D2) to lock current position as 0° baseline.");
}

void loop() {
  // 1. Read live angle from the rotary sensor / potentiometer (12-bit ADC: 0 to 4095)
  int raw_adc = analogRead(POT_PIN);
  current_angle = ((float)raw_adc / 4095.0f) * MAX_ALLOWED_DEVIATION;

  // 2. Check if the user pressed the calibration button
  check_calibration_button();

  // 3. Calculate absolute angular deviation relative to the CALIBRATED baseline
  float absolute_diff = fabsf(current_angle - base_calibrated_angle);
  deviation_pct = (absolute_diff / MAX_ALLOWED_DEVIATION) * 100.0f;

  // 4. Trigger alert if tilt exceeds threshold (>= 15% / >= 6.75° from calibrated angle)
  if (deviation_pct >= THRESHOLD_PERCENTAGE) {
    digitalWrite(ALERT_LED_PIN, HIGH);  // Bad posture: Turn LED ON
  } else {
    digitalWrite(ALERT_LED_PIN, LOW);   // Good posture: Turn LED OFF
  }

  // 5. Output telemetry to Serial Monitor
  Serial.print("Current Angle: ");
  Serial.print(current_angle, 2);
  Serial.print("° | Base Angle: ");
  Serial.print(base_calibrated_angle, 2);
  Serial.print("° | Deviation: ");
  Serial.print(deviation_pct, 1);
  Serial.print("% | Status: ");

  if (deviation_pct >= THRESHOLD_PERCENTAGE) {
    Serial.println("[ ALERT: INCORRECT POSTURE ]");
  } else {
    Serial.println("[ NORMAL POSTURE ]");
  }

  delay(150);
}

/**
 * @brief Calibration Handler
 * Saves the current rotary sensor angle as the user's neutral (0°) upright posture.
 */
void check_calibration_button() {
  if (digitalRead(CALIB_BTN_PIN) == LOW) { 
    delay(20); // Debounce delay
    if (digitalRead(CALIB_BTN_PIN) == LOW) {
      base_calibrated_angle = current_angle; // Lock current position as 0° baseline
      
      // Flash LED twice as confirmation
      digitalWrite(ALERT_LED_PIN, HIGH); delay(100);
      digitalWrite(ALERT_LED_PIN, LOW);  delay(100);
      digitalWrite(ALERT_LED_PIN, HIGH); delay(100);
      digitalWrite(ALERT_LED_PIN, LOW);
      
      Serial.println("\n*** CALIBRATION SUCCESS: Baseline set to current position! ***\n");
      
      while (digitalRead(CALIB_BTN_PIN) == LOW); // Wait for button release
    }
  }
}