#include <Wire.h>
#include <SoftwareSerial.h>
#include <TM1650.h>

TM1650 display;

const int rxPin = 2;  // RS485 接收引脚
const int txPin = 3;  // RS485 发送引脚

SoftwareSerial softSerial(rxPin, txPin);

const int switch1Pin = 7;
const int switch2Pin = 8;
const int switch3Pin = 9;
const int footSwitchPin = 10;

const uint8_t initData[] = {0x3A, 0x55, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x8F, 0x0D, 0x0A};
const uint8_t expectedResponse[] = {0xA3, 0xAA, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4D, 0x0D, 0x0A};

const uint8_t data24[] = {0x3A, 0xFA, 0x01, 0x00, 0x11, 0x78, 0x57, 0x32, 0x20, 0x67, 0x0D, 0x0A};
const uint8_t data27[] = {0x3A, 0xFA, 0x01, 0x00, 0x12, 0x96, 0x42, 0x32, 0x20, 0x71, 0x0D, 0x0A};
const uint8_t data36[] = {0x3A, 0xFA, 0x01, 0x00, 0x16, 0x50, 0x00, 0x32, 0x20, 0xED, 0x0D, 0x0A};

bool stopExecution = false;

void setup() {
  Serial.begin(115200);  // 用于调试
  softSerial.begin(115200);  // 用于RS485通信

  // 初始化TM1650显示器
  display.init();
  display.displayOff();  // 初始状态不显示任何内容

  bool initSuccess = false;

  // 进行初始化尝试最多10次
  for (int attempt = 1; attempt <= 10; attempt++) {
    softSerial.write(initData, sizeof(initData));
    Serial.print("Attempt ");
    Serial.print(attempt);
    Serial.println(": Init data sent.");

    unsigned long startTime = millis();
    while (millis() - startTime < 1000) {  // 每次尝试等待1秒
      if (softSerial.available() >= 12) {
        uint8_t receivedData[12];
        for (int i = 0; i < 12; i++) {
          receivedData[i] = softSerial.read();
        }
        Serial.print("Received data: ");
        for (int i = 0; i < 12; i++) {
          Serial.print(receivedData[i], HEX);
          Serial.print(" ");
        }
        Serial.println();

        if (memcmp(receivedData, expectedResponse, 12) == 0) {
          Serial.println("Expected response received.");
          initSuccess = true;
          break;
        }
      }
    }

    if (initSuccess) {
      break;  // 初始化成功，退出循环
    } else {
      Serial.println("No expected response, retrying...");
      delay(1000);  // 等待1秒再重试
    }
  }

  if (!initSuccess) {
    stopExecution = true;
    Serial.println("Error: Failed to receive expected response after 10 attempts.");
    display.displayString("ERR");  // 显示错误信息
  }
}

void loop() {
  if (stopExecution) {
    return;
  }

  bool footSwitchState = digitalRead(footSwitchPin) == LOW;
  bool switch1State = digitalRead(switch1Pin) == LOW;
  bool switch2State = digitalRead(switch2Pin) == LOW;
  bool switch3State = digitalRead(switch3Pin) == LOW;

  int switchCount = switch1State + switch2State + switch3State;

  if (switchCount == 1 && footSwitchState) {
    if (switch1State) {
      softSerial.write(data24, sizeof(data24));
      display.displayString("24");
    } else if (switch2State) {
      softSerial.write(data27, sizeof(data27));
      display.displayString("27");
    } else if (switch3State) {
      softSerial.write(data36, sizeof(data36));
      display.displayString("36");
    }
  } else {
    display.displayOff();  // 清除显示内容
  }

  delay(100);
}