#include <Arduino.h>

// === 引脚定义 ===
#define PIN_BTN         PA12
#define PIN_LED_FRONT   PB0     // D3
#define PIN_LED_LEFT    PA15    // D4
#define PIN_LED_RIGHT   PA8     // D6
#define PIN_BUZZER      PB6     // D5

// ADC 通道（内部通道号）
#define ADC_CH_FRONT    0       // A0 = PA0 = IN0
#define ADC_CH_LEFT     1       // A1 = PA1 = IN1
#define ADC_CH_RIGHT    3       // A2 = PA4 = IN4
#define ADC_CH_SPEED    4       // A3 = PA5 = IN5

// === 系统常量 ===
#define SAMPLE_NUM      8
#define FILTER_WINDOW   5
#define LOOP_DELAY_MS   20
#define PRINT_INTERVAL  1000

#define TH_DEFAULT_FRONT 40
#define TH_DEFAULT_LEFT  30
#define TH_DEFAULT_RIGHT 30

#define TTC_SAFE        3.0f
#define TTC_WARNING     1.5f
#define TTC_DANGER      0.8f
#define TTC_BRAKE       0.5f

#define VOLTAGE_MIN     0.0f
#define VOLTAGE_MAX     3.3f

// === 全局变量 ===
enum SystemMode {
  MODE_NORMAL = 0,
  MODE_SILENT,
  MODE_ACTIVE_BRAKE
};
volatile SystemMode mode = MODE_NORMAL;
volatile bool btn_pressed = false;

float th_front = TH_DEFAULT_FRONT;
float th_left  = TH_DEFAULT_LEFT;
float th_right = TH_DEFAULT_RIGHT;

float dist_front = 100.0f;
float dist_left  = 100.0f;
float dist_right = 100.0f;
float speed_kmh  = 30.0f;

bool fault_front = false;
bool fault_left  = false;
bool fault_right = false;
bool fault_speed = false;

unsigned long last_print = 0;
unsigned long last_led_toggle = 0;

bool led_front_on = false;
bool led_left_on  = false;
bool led_right_on = false;

uint8_t buzzer_state = 0;
unsigned long buzzer_last_toggle = 0;

float buf_front[FILTER_WINDOW] = {0};
float buf_left[FILTER_WINDOW]  = {0};
float buf_right[FILTER_WINDOW] = {0};
float buf_speed[FILTER_WINDOW]  = {0};
uint8_t buf_idx = 0;

// === 函数声明 ===
void SystemClock_Config(void);
void ADC_Init(void);
float ReadADC_Ch(uint32_t channel);
float ReadADC_Avg(uint32_t channel);
float VoltageToDistance(float v);
float VoltageToSpeed(float v);
void UpdateSensors(void);
float CalculateTTC(float distance_cm, float speed_kmh);
void ProcessAlarms(void);
void ProcessBuzzer(void);
void ProcessSerialCommand(void);
void PrintStatus(void);
void CheckFaults(void);
void btnISR(void);

// === 中断服务函数 ===
void btnISR(void) {
  static unsigned long last_interrupt = 0;
  unsigned long now = millis();
  if (now - last_interrupt > 50) {
    btn_pressed = true;
    last_interrupt = now;
  }
}

// === 主函数 ===
void setup() {
  HAL_Init();
  SystemClock_Config();
  ADC_Init();

  pinMode(PIN_LED_FRONT, OUTPUT);
  pinMode(PIN_LED_LEFT, OUTPUT);
  pinMode(PIN_LED_RIGHT, OUTPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  digitalWrite(PIN_LED_FRONT, LOW);
  digitalWrite(PIN_LED_LEFT, LOW);
  digitalWrite(PIN_LED_RIGHT, LOW);
  digitalWrite(PIN_BUZZER, LOW);

  pinMode(PIN_BTN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PIN_BTN), btnISR, FALLING);

  Serial.begin(115200);
  while (!Serial);
  Serial.println("\n===== Multi-Direction Collision Warning System =====");
  Serial.println("Commands: TH F/L/R <cm> | MUTE | NORMAL | BRAKE | STATUS");
  Serial.print("Front threshold: "); Serial.print(th_front);
  Serial.print(" cm, Left: "); Serial.print(th_left);
  Serial.print(" cm, Right: "); Serial.println(th_right);
  Serial.println("Mode: NORMAL");
  last_print = millis();
}

void loop() {
  if (btn_pressed) {
    btn_pressed = false;
    mode = static_cast<SystemMode>((mode + 1) % 3);
    const char* mode_str[] = {"NORMAL", "SILENT", "ACTIVE BRAKE"};
    Serial.print("\n[MODE] Switched to: ");
    Serial.println(mode_str[mode]);
    digitalWrite(PIN_LED_FRONT, LOW);
    digitalWrite(PIN_LED_LEFT, LOW);
    digitalWrite(PIN_LED_RIGHT, LOW);
    digitalWrite(PIN_BUZZER, LOW);
    buzzer_state = 0;
  }

  UpdateSensors();
  CheckFaults();
  ProcessAlarms();
  ProcessBuzzer();

  if (millis() - last_print >= PRINT_INTERVAL) {
    last_print = millis();
    PrintStatus();
  }

  ProcessSerialCommand();
  delay(LOOP_DELAY_MS);
}

// === 时钟配置 ===
void SystemClock_Config(void) {
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
  HAL_RCC_OscConfig(&RCC_OscInitStruct);

  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
                              | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
  HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0);
}

// === ADC 初始化 ===
void ADC_Init(void) {
  __HAL_RCC_ADC1_CLK_ENABLE();
  ADC1->CR = 0;
  ADC1->CR |= ADC_CR_ADVREGEN;
  HAL_Delay(1);
  ADC1->CFGR1 = 0;
  ADC1->CFGR2 = ADC_CFGR2_CKMODE_0 | ADC_CFGR2_CKMODE_1;
  ADC1->SMPR = ADC_SMPR_SMP_0 | ADC_SMPR_SMP_1 | ADC_SMPR_SMP_2;
  ADC1->CR |= ADC_CR_ADEN;
  while (!(ADC1->ISR & ADC_ISR_ADRDY));
}

float ReadADC_Ch(uint32_t channel) {
  ADC1->CHSELR = (1UL << channel);
  ADC1->CR |= ADC_CR_ADSTART;
  while (!(ADC1->ISR & ADC_ISR_EOC));
  uint16_t val = ADC1->DR;
  return (val / 4095.0f) * 3.3f;
}

float ReadADC_Avg(uint32_t channel) {
  float sum = 0;
  for (int i = 0; i < SAMPLE_NUM; i++) {
    sum += ReadADC_Ch(channel);
  }
  return sum / SAMPLE_NUM;
}

// === 传感器转换 ===
float VoltageToDistance(float v) {
  if (v < 0.4f) return 150.0f;
  if (v > 3.0f) return 2.0f;
  float dist = 27.5f / (v - 0.1f);
  if (dist < 2.0f) dist = 2.0f;
  if (dist > 150.0f) dist = 150.0f;
  return dist;
}

float VoltageToSpeed(float v) {
  if (v < 0.0f) v = 0.0f;
  if (v > 3.3f) v = 3.3f;
  return v / 3.3f * 100.0f;
}

void UpdateSensors(void) {
  float v_front = ReadADC_Avg(ADC_CH_FRONT);
  float v_left  = ReadADC_Avg(ADC_CH_LEFT);
  float v_right = ReadADC_Avg(ADC_CH_RIGHT);
  float v_speed = ReadADC_Avg(ADC_CH_SPEED);

  buf_front[buf_idx] = v_front;
  buf_left[buf_idx]  = v_left;
  buf_right[buf_idx] = v_right;
  buf_speed[buf_idx] = v_speed;
  buf_idx = (buf_idx + 1) % FILTER_WINDOW;

  float sum_f = 0, sum_l = 0, sum_r = 0, sum_s = 0;
  for (int i = 0; i < FILTER_WINDOW; i++) {
    sum_f += buf_front[i];
    sum_l += buf_left[i];
    sum_r += buf_right[i];
    sum_s += buf_speed[i];
  }
  dist_front = VoltageToDistance(sum_f / FILTER_WINDOW);
  dist_left  = VoltageToDistance(sum_l / FILTER_WINDOW);
  dist_right = VoltageToDistance(sum_r / FILTER_WINDOW);
  speed_kmh  = (sum_s / FILTER_WINDOW) / 3.3f * 100.0f;
}

// === 故障检测 ===
void CheckFaults(void) {
  float v_f = ReadADC_Avg(ADC_CH_FRONT);
  float v_l = ReadADC_Avg(ADC_CH_LEFT);
  float v_r = ReadADC_Avg(ADC_CH_RIGHT);
  float v_s = ReadADC_Avg(ADC_CH_SPEED);

  fault_front = (v_f < VOLTAGE_MIN || v_f > VOLTAGE_MAX);
  fault_left  = (v_l < VOLTAGE_MIN || v_l > VOLTAGE_MAX);
  fault_right = (v_r < VOLTAGE_MIN || v_r > VOLTAGE_MAX);
  fault_speed = (v_s < VOLTAGE_MIN || v_s > VOLTAGE_MAX);

  static unsigned long last_fault_f = 0, last_fault_l = 0, last_fault_r = 0, last_fault_s = 0;
  unsigned long now = millis();
  if (fault_front && now - last_fault_f > 1000) { Serial.println("[FAULT] Front sensor fault!"); last_fault_f = now; }
  if (fault_left  && now - last_fault_l > 1000) { Serial.println("[FAULT] Left sensor fault!");  last_fault_l = now; }
  if (fault_right && now - last_fault_r > 1000) { Serial.println("[FAULT] Right sensor fault!"); last_fault_r = now; }
  if (fault_speed && now - last_fault_s > 1000) { Serial.println("[FAULT] Speed sensor fault!"); last_fault_s = now; }
}

// === TTC 计算 ===
float CalculateTTC(float distance_cm, float speed_kmh) {
  if (speed_kmh < 0.5f) return 999.0f;
  float speed_ms = speed_kmh / 3.6f;
  return (distance_cm / 100.0f) / speed_ms;
}

// === 报警处理 ===
void ProcessAlarms(void) {
  if (mode == MODE_SILENT) {
    digitalWrite(PIN_LED_FRONT, LOW);
    digitalWrite(PIN_LED_LEFT, LOW);
    digitalWrite(PIN_LED_RIGHT, LOW);
    led_front_on = led_left_on = led_right_on = false;
    buzzer_state = 0;
    return;
  }

  float ttc[3] = {
    fault_front ? 999.0f : CalculateTTC(dist_front, speed_kmh),
    fault_left  ? 999.0f : CalculateTTC(dist_left, speed_kmh),
    fault_right ? 999.0f : CalculateTTC(dist_right, speed_kmh)
  };
  float min_ttc = min(ttc[0], min(ttc[1], ttc[2]));

  bool front_alarm = (!fault_front && dist_front < th_front && ttc[0] < TTC_SAFE);
  bool left_alarm  = (!fault_left  && dist_left  < th_left  && ttc[1] < TTC_SAFE);
  bool right_alarm = (!fault_right && dist_right < th_right && ttc[2] < TTC_SAFE);

  int alarm_level = 0;
  if (min_ttc < TTC_BRAKE && mode == MODE_ACTIVE_BRAKE) alarm_level = 3;
  else if (min_ttc < TTC_DANGER) alarm_level = 2;
  else if (min_ttc < TTC_WARNING) alarm_level = 1;

  unsigned long now = millis();
  int toggle_ms = 0;
  switch (alarm_level) {
    case 1: toggle_ms = 500; break;
    case 2: toggle_ms = 150; break;
    case 3: toggle_ms = 50;  break;
  }

  if (toggle_ms > 0 && (now - last_led_toggle >= toggle_ms)) {
    last_led_toggle = now;
    if (front_alarm) { led_front_on = !led_front_on; digitalWrite(PIN_LED_FRONT, led_front_on ? HIGH : LOW); }
    else { digitalWrite(PIN_LED_FRONT, LOW); led_front_on = false; }
    if (left_alarm)  { led_left_on  = !led_left_on;  digitalWrite(PIN_LED_LEFT, led_left_on   ? HIGH : LOW); }
    else { digitalWrite(PIN_LED_LEFT, LOW); led_left_on = false; }
    if (right_alarm) { led_right_on = !led_right_on; digitalWrite(PIN_LED_RIGHT, led_right_on ? HIGH : LOW); }
    else { digitalWrite(PIN_LED_RIGHT, LOW); led_right_on = false; }
  }
  if (alarm_level == 0) {
    digitalWrite(PIN_LED_FRONT, LOW);
    digitalWrite(PIN_LED_LEFT, LOW);
    digitalWrite(PIN_LED_RIGHT, LOW);
    led_front_on = led_left_on = led_right_on = false;
  }

  if (mode == MODE_SILENT || alarm_level == 0) buzzer_state = 0;
  else if (alarm_level == 3) buzzer_state = 3;
  else if (alarm_level == 2) buzzer_state = 2;
  else buzzer_state = 1;
}

// === 蜂鸣器驱动 ===
void ProcessBuzzer(void) {
  static bool buzzer_on = false;
  unsigned long now = millis();
  int period = 0;
  switch (buzzer_state) {
    case 1: period = 500; break;
    case 2: period = 150; break;
    case 3: digitalWrite(PIN_BUZZER, HIGH); return;
    default: digitalWrite(PIN_BUZZER, LOW); buzzer_on = false; return;
  }
  if (now - buzzer_last_toggle >= period) {
    buzzer_last_toggle = now;
    buzzer_on = !buzzer_on;
    digitalWrite(PIN_BUZZER, buzzer_on ? HIGH : LOW);
  }
}

// === 状态打印 ===
void PrintStatus(void) {
  Serial.print("F:");
  if (fault_front) Serial.print("ERR"); else { Serial.print(dist_front, 1); Serial.print("cm"); }
  Serial.print(" L:");
  if (fault_left) Serial.print("ERR"); else { Serial.print(dist_left, 1); Serial.print("cm"); }
  Serial.print(" R:");
  if (fault_right) Serial.print("ERR"); else { Serial.print(dist_right, 1); Serial.print("cm"); }
  Serial.print(" Sp:");
  if (fault_speed) Serial.print("ERR"); else { Serial.print(speed_kmh, 1); Serial.print("km/h"); }

  Serial.print(" | TTC:");
  if (!fault_front) { Serial.print(CalculateTTC(dist_front, speed_kmh), 2); Serial.print("s"); }
  else Serial.print("---");

  Serial.print(" Mode:");
  const char* mode_str[] = {"NORMAL", "SILENT", "BRAKE"};
  Serial.println(mode_str[mode]);
}

// === 串口命令 ===
void ProcessSerialCommand(void) {
  if (Serial.available() > 0) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    if (cmd.length() == 0) return;

    if (cmd == "MUTE") {
      mode = MODE_SILENT; Serial.println("[CMD] Silent mode");
    } else if (cmd == "NORMAL") {
      mode = MODE_NORMAL; Serial.println("[CMD] Normal mode");
    } else if (cmd == "BRAKE") {
      mode = MODE_ACTIVE_BRAKE; Serial.println("[CMD] Active brake mode");
    } else if (cmd == "STATUS") {
      PrintStatus();
    } else if (cmd.startsWith("TH ")) {
      char dir; int val;
      if (sscanf(cmd.c_str(), "TH %c %d", &dir, &val) == 2) {
        switch (dir) {
          case 'F': case 'f': th_front = val; break;
          case 'L': case 'l': th_left  = val; break;
          case 'R': case 'r': th_right = val; break;
          default: Serial.println("[CMD] Invalid direction"); return;
        }
        Serial.print("[CMD] Threshold "); Serial.print(dir);
        Serial.print(" set to "); Serial.print(val); Serial.println("cm");
      } else {
        Serial.println("[CMD] Usage: TH <F/L/R> <cm>");
      }
    } else {
      Serial.println("[CMD] Unknown. Use: MUTE, NORMAL, BRAKE, STATUS, TH F/L/R <cm>");
    }
  }
}