#include <Arduino.h>

// 定义硬件引脚
#define POWER_PIN   PB8      // 电源控制引脚
#define LED_PIN     PC13     // LED控制引脚
#define ADC_PIN     PA0      // ADC输入引脚
#define BUZZER_PIN  PA15     // 蜂鸣器控制引脚

// ========== 用户配置区域 ==========
// 只需修改这里的工具数量，系统会自动启用相应通道
const uint8_t TOOL_COUNT = 5;  // 设置实际使用的工具数量 (1-16)

// 系统参数
const uint32_t BLINK_INTERVAL = 500;     // LED闪烁间隔时间（ms）
const uint32_t SHUTDOWN_DELAY = 2000;    // 自动关机延迟时间（ms）
const uint32_t DATA_INTERVAL = 1000;     // 数据发送间隔时间（ms）
const uint32_t ALARM_DURATION = 3000;    // 低电压报警持续时间（ms）
const uint32_t STATE_CHECK_INTERVAL = 50; // 状态检查间隔（ms）

const uint8_t FILTER_SIZE = 10;          // 电压采样滤波器大小
const float REF_VOLTAGE = 3.3f;          // ADC参考电压
const float DIVIDER_RATIO = 11.0f;       // 电压分压比
const float VOLTAGE_MIN = 3.2f;          // 最低电压对应0%
const float VOLTAGE_MAX = 4.2f;          // 最高电压对应100%
const float VOLTAGE_SHUTDOWN = 3.0f;     // 低电压关机阈值
// ========== 配置区域结束 ==========

// 完整的输入引脚映射表 (通道0-15)
const uint8_t FULL_INPUT_PINS[16] = {
  PB9,   // 通道0 (INPUT1)
  PA1,   // 通道1 (INPUT2) 
  PA2,   // 通道2 (INPUT3)
  PA3,   // 通道3 (INPUT4)
  PA4,   // 通道4 (INPUT5)
  PA5,   // 通道5 (INPUT6)
  PA6,   // 通道6 (INPUT7)
  PA7,   // 通道7 (INPUT8)
  PB0,   // 通道8 (INPUT9)
  PB1,   // 通道9 (INPUT10)
  PA8,   // 通道10 (INPUT11) - 根据映射25
  PA9,   // 通道11 (INPUT12) - 根据映射26  
  PA10,  // 通道12 (INPUT13) - 根据映射27
  PA11,  // 通道13 (INPUT14) - 根据映射28
  PA12,  // 通道14 (INPUT15) - 根据映射29
  PB5    // 通道15 (INPUT16) - 根据映射36
};

// 实际使用的输入引脚数组 (根据TOOL_COUNT自动生成)
uint8_t inputPins[TOOL_COUNT];

// 系统状态枚举
enum SystemState {
  STATE_NORMAL,           // 正常状态
  STATE_TOOLS_MISSING,    // 工具缺失状态
  STATE_LOW_VOLTAGE,      // 低电压报警状态
  STATE_SHUTDOWN          // 关机状态
};

// 全局状态变量
HardwareSerial Serial3(USART3);
SystemState currentState = STATE_NORMAL;
uint32_t lastBlinkTime = 0;
uint32_t lastDataSendTime = 0;
uint32_t lastStateCheckTime = 0;
uint32_t allReturnedStartTime = 0;
uint32_t alarmStartTime = 0;
bool ledState = false;

// 电压采样相关
uint16_t voltageSamples[FILTER_SIZE] = {0};
uint8_t sampleIndex = 0;
float currentVoltage = 0.0f;
uint8_t currentBatteryPercent = 0;

// 工具状态
bool toolStates[TOOL_COUNT] = {false};
bool anyToolMissing = false;
uint8_t missingToolCount = 0;

// 前置声明函数
void initializeInputPins();
void sendHMICommand(const char* command);
void updateToolDisplay(uint8_t channel, bool isMissing);
void updateBatteryDisplay(float voltage, uint8_t percent);
float readFilteredVoltage();
uint8_t calculateBatteryPercent(float voltage);
void checkToolStates();
void updateSystemState();
void handleNormalState();
void handleToolsMissingState();
void handleLowVoltageState();
void handleLED();
void handleBuzzer();
void triggerShutdown();
void sendSystemInfo();

void setup() {
  // 初始化实际使用的输入引脚
  initializeInputPins();

  // 初始化硬件
  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, LOW);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);
  pinMode(POWER_PIN, OUTPUT);
  digitalWrite(POWER_PIN, HIGH);

  // ADC初始化
  pinMode(ADC_PIN, INPUT_ANALOG);
  analogReadResolution(12);
  
  // 初始化电压采样数组
  for(uint8_t i = 0; i < FILTER_SIZE; i++){
    voltageSamples[i] = analogRead(ADC_PIN);
  }

  // 初始化串口通信
  Serial3.begin(115200);
  while(!Serial3);

  // 发送系统启动信息
  sendHMICommand("sys:start");
  
  // 读取初始电压
  currentVoltage = readFilteredVoltage();
  currentBatteryPercent = calculateBatteryPercent(currentVoltage);
}

// 初始化实际使用的输入引脚
void initializeInputPins() {
  for (uint8_t i = 0; i < TOOL_COUNT; i++) {
    if (i < 16) {  // 确保不超出完整引脚映射范围
      inputPins[i] = FULL_INPUT_PINS[i];
      pinMode(inputPins[i], INPUT_PULLDOWN);
    }
  }
}

void sendHMICommand(const char* command) {
  Serial3.print(command);
  Serial3.write(0xFF);
  Serial3.write(0xFF);
  Serial3.write(0xFF);
}

void updateToolDisplay(uint8_t channel, bool isMissing) {
  // 只更新实际启用的通道
  if (channel < TOOL_COUNT) {
    char buffer[32];
    uint16_t color = isMissing ? (ledState ? 2016 : 63488) : 2016;
    snprintf(buffer, sizeof(buffer), "t%d.bco=%d", channel, color);
    sendHMICommand(buffer);
  }
}

void updateBatteryDisplay(float voltage, uint8_t percent) {
  static uint8_t lastPercent = 0;
  static float lastVoltage = 0.0f;

  if(abs(percent - lastPercent) >= 2 || abs(voltage - lastVoltage) >= 0.1f) {
    char buffer[32];
    
    // 更新电池百分比
    snprintf(buffer, sizeof(buffer), "t11.txt=\"%d%%\"", percent);
    sendHMICommand(buffer);
    
    // 更新电压显示
    snprintf(buffer, sizeof(buffer), "t12.txt=\"%.2fV\"", voltage);
    sendHMICommand(buffer);
    
    // 更新电池条
    snprintf(buffer, sizeof(buffer), "j0.val=%d", percent);
    sendHMICommand(buffer);
    
    lastPercent = percent;
    lastVoltage = voltage;
  }
}

float readFilteredVoltage() {
  voltageSamples[sampleIndex] = analogRead(ADC_PIN);
  sampleIndex = (sampleIndex + 1) % FILTER_SIZE;

  uint32_t sum = 0;
  for(uint8_t i = 0; i < FILTER_SIZE; i++) {
    sum += voltageSamples[i];
  }
  
  float avgADC = sum / (float)FILTER_SIZE;
  return (avgADC * REF_VOLTAGE / 4095.0f) * DIVIDER_RATIO;
}

uint8_t calculateBatteryPercent(float voltage) {
  voltage = constrain(voltage, VOLTAGE_MIN, VOLTAGE_MAX);
  return static_cast<uint8_t>((voltage - VOLTAGE_MIN) * 100 / (VOLTAGE_MAX - VOLTAGE_MIN));
}

void checkToolStates() {
  anyToolMissing = false;
  missingToolCount = 0;
  
  for (uint8_t i = 0; i < TOOL_COUNT; i++) {
    bool newState = (digitalRead(inputPins[i]) == HIGH);
    
    if (newState != toolStates[i]) {
      toolStates[i] = newState;
      updateToolDisplay(i, newState);
    }
    
    if (newState) {
      anyToolMissing = true;
      missingToolCount++;
    }
  }
}

void updateSystemState() {
  // 检查电压状态（优先级最高）
  if (currentVoltage < VOLTAGE_SHUTDOWN && currentState != STATE_SHUTDOWN) {
    currentState = STATE_LOW_VOLTAGE;
    alarmStartTime = millis();
    return;
  }
  
  if (currentState == STATE_SHUTDOWN) {
    return;
  }
  
  // 根据工具状态更新系统状态
  if (anyToolMissing) {
    currentState = STATE_TOOLS_MISSING;
    allReturnedStartTime = 0;
  } else {
    if (currentState == STATE_TOOLS_MISSING) {
      currentState = STATE_NORMAL;
    }
    
    if (allReturnedStartTime == 0) {
      allReturnedStartTime = millis();
    }
  }
}

void handleNormalState() {
  uint32_t currentTime = millis();
  
  if (!anyToolMissing && allReturnedStartTime > 0) {
    if (currentTime - allReturnedStartTime >= SHUTDOWN_DELAY) {
      triggerShutdown();
    }
  }
}

void handleToolsMissingState() {
  // 工具缺失状态的特殊处理
}

void handleLowVoltageState() {
  uint32_t currentTime = millis();
  
  if (currentTime - alarmStartTime >= ALARM_DURATION) {
    triggerShutdown();
  }
}

void handleLED() {
  uint32_t currentTime = millis();
  
  if (currentTime - lastBlinkTime >= BLINK_INTERVAL) {
    ledState = !ledState;
    lastBlinkTime = currentTime;
    digitalWrite(LED_PIN, ledState);
    
    // 在工具缺失状态下，更新缺失工具的显示颜色
    if (currentState == STATE_TOOLS_MISSING) {
      for (uint8_t i = 0; i < TOOL_COUNT; i++) {
        if (toolStates[i]) {
          updateToolDisplay(i, true);
        }
      }
    }
  }
}

void handleBuzzer() {
  uint32_t currentTime = millis();
  bool buzzerState = LOW;
  
  switch (currentState) {
    case STATE_TOOLS_MISSING:
      // 工具缺失时跟随LED闪烁
      buzzerState = ledState ? HIGH : LOW;
      break;
      
    case STATE_LOW_VOLTAGE:
      // 低电压报警时，500ms周期内响250ms
      buzzerState = ((currentTime - alarmStartTime) % 500 < 250) ? HIGH : LOW;
      break;
      
    default:
      buzzerState = LOW;
      break;
  }
  
  digitalWrite(BUZZER_PIN, buzzerState);
}

void triggerShutdown() {
  currentState = STATE_SHUTDOWN;
  digitalWrite(POWER_PIN, LOW);
  digitalWrite(BUZZER_PIN, LOW);
  digitalWrite(LED_PIN, LOW);
}

void loop() {
  uint32_t currentTime = millis();
  
  // 状态检查（50ms周期）
  if (currentTime - lastStateCheckTime >= STATE_CHECK_INTERVAL) {
    checkToolStates();
    updateSystemState();
    lastStateCheckTime = currentTime;
  }
  
  // 数据处理（1s周期）
  if (currentTime - lastDataSendTime >= DATA_INTERVAL) {
    currentVoltage = readFilteredVoltage();
    currentBatteryPercent = calculateBatteryPercent(currentVoltage);
    updateBatteryDisplay(currentVoltage, currentBatteryPercent);
    lastDataSendTime = currentTime;
  }
  
  // 状态机处理
  switch (currentState) {
    case STATE_NORMAL:
      handleNormalState();
      break;
    case STATE_TOOLS_MISSING:
      handleToolsMissingState();
      break;
    case STATE_LOW_VOLTAGE:
      handleLowVoltageState();
      break;
    case STATE_SHUTDOWN:
      // 关机状态不执行任何操作
      break;
  }
  
  // 输出设备控制
  handleLED();
  handleBuzzer();
}