// ==================== ETC 高速公路收费模拟系统 V1.0 (舵机修正版) ====================
// 功能: 车辆检测 → 读取标签 → 扣费 → 抬杆放行 → 落杆
// 适配 Wokwi: 按键=D2, 舵机=D6, 绿灯=D4, 红灯=D5

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>

#define CS_PIN      10
#define NRST_PD     9

// ==================== 引脚定义 ====================
#define BUTTON_PIN  2
#define SERVO_PIN   6
#define LED_GREEN   4
#define LED_RED     5

// ==================== OLED 配置 ====================
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_ADDR 0x3C

// ==================== ETC 系统参数 ====================
#define TOLL_FEE 15
#define DEFAULT_BALANCE 100

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Servo myServo;

// ==================== 授权车辆标签 ====================
byte g_ValidCardID[4] = {0x55, 0x77, 0xCC, 0x00};

// ==================== 车辆账户数据 ====================
struct VehicleAccount {
  byte cardID[4];
  char plateNumber[9];
  long balance;
  bool isActive;
  bool isBlocked;
};

VehicleAccount g_Vehicle;
bool g_AccountInit = false;
byte g_ScannedUID[4];

// ==================== RC522 寄存器 ====================
enum RC522_REG {
  CMD_REG=0x01, IRQ_REG=0x04, ERR_REG=0x06, STA2_REG=0x08,
  FIFO_DAT=0x09, FIFO_LVL=0x0A, BIT_FRM=0x0D, MOD_REG=0x11,
  TX_CTL=0x14, TX_ASK=0x15, RF_CFG=0x26,
  T_MODE=0x2A, T_PSC=0x2B, T_REL_L=0x2D, T_REL_H=0x2C
};

const byte CMD_RESET=0x0F, CMD_XCVR=0x0C, CMD_IDLE=0x00;
const byte CARD_REQA=0x26, CARD_ANTI=0x93, CARD_SEL=0x93, CARD_HALT=0x50;

// ==================== SPI 函数 ====================
void SPI_Setup() {
  pinMode(CS_PIN, OUTPUT);
  digitalWrite(CS_PIN, HIGH);
  SPI.begin();
}

void RC522_WriteReg(byte addr, byte val) {
  digitalWrite(CS_PIN, LOW);
  SPI.transfer((addr<<1)&0x7E);
  SPI.transfer(val);
  digitalWrite(CS_PIN, HIGH);
}

byte RC522_ReadReg(byte addr) {
  digitalWrite(CS_PIN, LOW);
  SPI.transfer(((addr<<1)&0x7E)|0x80);
  byte val=SPI.transfer(0x00);
  digitalWrite(CS_PIN, HIGH);
  return val;
}

void RC522_SetMask(byte addr, byte mask) { RC522_WriteReg(addr, RC522_ReadReg(addr)|mask); }
void RC522_ClrMask(byte addr, byte mask) { RC522_WriteReg(addr, RC522_ReadReg(addr)&~mask); }

void RC522_Init() {
  pinMode(NRST_PD, OUTPUT);
  digitalWrite(NRST_PD, LOW); delay(20);
  digitalWrite(NRST_PD, HIGH); delay(20);
  
  RC522_WriteReg(CMD_REG, CMD_RESET); delay(50);
  while(RC522_ReadReg(CMD_REG)&0x10);
  
  RC522_WriteReg(T_MODE, 0x80);
  RC522_WriteReg(T_PSC, 0xA9);
  RC522_WriteReg(T_REL_L, 0x03);
  RC522_WriteReg(T_REL_H, 0xE8);
  
  RC522_WriteReg(TX_ASK, 0x40);
  RC522_WriteReg(MOD_REG, 0x3D);
  RC522_WriteReg(RF_CFG, 0x7F);
  RC522_WriteReg(TX_CTL, 0x83);
  
  RC522_WriteReg(CMD_REG, CMD_IDLE);
  Serial.println(F("RFID Init OK"));
}

void FIFO_Flush() { RC522_WriteReg(FIFO_LVL, 0x80); }
void FIFO_Push(byte *d, byte n) { for(byte i=0;i<n;i++) RC522_WriteReg(FIFO_DAT,d[i]); }
byte FIFO_Pop(byte *b, byte m) {
  byte n=RC522_ReadReg(FIFO_LVL);
  if(n>m)n=m;
  for(byte i=0;i<n;i++)b[i]=RC522_ReadReg(FIFO_DAT);
  return n;
}

byte RF_Transceive(byte *tx, byte tl, byte *rx, byte *rl) {
  RC522_WriteReg(IRQ_REG, 0x7F);
  FIFO_Flush(); FIFO_Push(tx, tl);
  RC522_WriteReg(BIT_FRM, 0x00);
  RC522_WriteReg(CMD_REG, CMD_XCVR);
  RC522_SetMask(BIT_FRM, 0x80);
  
  word to=2500; byte irq;
  do{irq=RC522_ReadReg(IRQ_REG);if(--to==0)return 0;}while(!(irq&0x30));
  
  if(RC522_ReadReg(ERR_REG)&0x13)return 0;
  
  if(irq&0x20){*rl=FIFO_Pop(rx,*rl);}else{*rl=0;}
  return 1;
}

bool Card_Detect() {
  byte tx=CARD_REQA, rx[2], rl=2;
  RC522_ClrMask(STA2_REG,0x08);
  RC522_WriteReg(BIT_FRM,0x07);
  if(RF_Transceive(&tx,1,rx,&rl))return(rl==2);
  return false;
}

bool Card_GetUID() {
  byte tx[2]={CARD_ANTI,0x20}, rx[5], rl=5;
  RC522_ClrMask(STA2_REG,0x08);
  if(RF_Transceive(tx,2,rx,&rl)){
    if(rl>=4){for(byte i=0;i<4;i++)g_ScannedUID[i]=rx[i];return true;}
  }
  return false;
}

void Card_Halt() {
  byte tx[2]={CARD_HALT,0x00}, rx[1], rl=1;
  RF_Transceive(tx,2,rx,&rl);
}

// ==================== 工具函数 ====================
void PrintHex(byte *a, byte n) {
  for(byte i=0;i<n;i++){
    if(a[i]<0x10)Serial.print("0");
    Serial.print(a[i],HEX);
    if(i<n-1)Serial.print(" ");
  }
}

bool IDMatch(byte *a, byte *b, byte n) {
  for(byte i=0;i<n;i++)if(a[i]!=b[i])return false;
  return true;
}

// ==================== 道闸控制 (修正版 - 增加延时和调试) ====================
void Gate_Open() {
  Serial.println(F("[Gate] Opening..."));
  myServo.write(90);
  delay(300);  // 等待舵机到达位置
  digitalWrite(LED_GREEN, HIGH);
  digitalWrite(LED_RED, LOW);
  Serial.print(F("[Gate] Open - Angle: "));
  Serial.println(myServo.read());
}

void Gate_Close() {
  Serial.println(F("[Gate] Closing..."));
  myServo.write(0);
  delay(300);  // 等待舵机到达位置
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_RED, HIGH);
  Serial.print(F("[Gate] Close - Angle: "));
  Serial.println(myServo.read());
}

// ==================== OLED 显示 ====================
void OLED_Init() {
  if(!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    Serial.println(F("OLED init fail"));
    return;
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(F("ETC System V1.0"));
  display.println(F("Loading..."));
  display.display();
  delay(1500);
  Serial.println(F("OLED init OK"));
}

void OLED_ShowETCReady() {
  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(0, 0);
  display.println(F("ETC Lane"));
  display.setTextSize(1);
  display.setCursor(0, 35);
  display.println(F("Waiting..."));
  display.setCursor(0, 50);
  display.println(F("Press button"));
  display.display();
}

void OLED_ShowVehicleInfo(String plate, long balance) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print(F("Plate: "));
  display.println(plate);
  display.print(F("Balance: "));
  display.print(balance);
  display.println(F(" Yuan"));
  display.setCursor(0, 30);
  display.println(F("Deducting..."));
  display.display();
}

void OLED_ShowPass(String plate, long newBalance, int fee) {
  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(0, 0);
  display.println(F("PASS!"));
  display.setTextSize(1);
  display.setCursor(0, 25);
  display.print(F("Plate: "));
  display.println(plate);
  display.print(F("Fee: "));
  display.print(fee);
  display.print(F("  Bal: "));
  display.print(newBalance);
  display.display();
}

void OLED_ShowDenied(String reason) {
  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(0, 0);
  display.println(F("DENIED!"));
  display.setTextSize(1);
  display.setCursor(0, 30);
  display.println(reason);
  display.display();
}

void OLED_ShowLowBalance(long balance, int fee) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println(F("Low Balance!"));
  display.print(F("Balance: "));
  display.print(balance);
  display.println(F(" Yuan"));
  display.print(F("Need: "));
  display.print(fee);
  display.println(F(" Yuan"));
  display.setCursor(0, 45);
  display.println(F("Use manual lane"));
  display.display();
}

void OLED_ShowMessage(const String& line1, const String& line2) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println(line1);
  display.setCursor(0, 20);
  display.println(line2);
  display.display();
}

// ==================== ETC 账户管理 ====================
void Account_Init() {
  if(!g_AccountInit) {
    memset(&g_Vehicle, 0, sizeof(g_Vehicle));
    g_AccountInit = true;
    Serial.println(F("[System] ETC account init OK"));
  }
}

void Account_Create(byte *cardUID) {
  Account_Init();
  memcpy(g_Vehicle.cardID, cardUID, 4);
  strcpy(g_Vehicle.plateNumber, "粤A12345");
  g_Vehicle.balance = DEFAULT_BALANCE;
  g_Vehicle.isActive = true;
  g_Vehicle.isBlocked = false;
  
  Serial.print(F("[Create] Card: "));
  PrintHex(cardUID, 4);
  Serial.print(F("  Plate: "));
  Serial.print(g_Vehicle.plateNumber);
  Serial.print(F("  Balance: "));
  Serial.println(g_Vehicle.balance);
}

void Account_Recharge(long amount) {
  if(g_AccountInit && g_Vehicle.isActive) {
    g_Vehicle.balance += amount;
    Serial.print(F("[Recharge] OK! New balance: "));
    Serial.println(g_Vehicle.balance);
  }
}

// ==================== ETC 扣费处理 ====================
bool ETC_ProcessTransaction() {
  if(!g_AccountInit || !g_Vehicle.isActive) {
    Serial.println(F("[ETC] Error: Vehicle not registered"));
    OLED_ShowMessage(F("Error"), F("Not registered"));
    return false;
  }
  
  if(g_Vehicle.isBlocked) {
    Serial.println(F("[ETC] Denied: Blacklisted"));
    OLED_ShowDenied(F("Blacklisted"));
    return false;
  }
  
  if(g_Vehicle.balance < TOLL_FEE) {
    Serial.print(F("[ETC] Low balance: "));
    Serial.print(g_Vehicle.balance);
    Serial.print(F(" Need: "));
    Serial.println(TOLL_FEE);
    OLED_ShowLowBalance(g_Vehicle.balance, TOLL_FEE);
    return false;
  }
  
  g_Vehicle.balance -= TOLL_FEE;
  
  Serial.println(F("========================================"));
  Serial.print(F("[ETC] PASS! Plate: "));
  Serial.println(g_Vehicle.plateNumber);
  Serial.print(F("[ETC] Fee: "));
  Serial.print(TOLL_FEE);
  Serial.print(F("  Balance: "));
  Serial.println(g_Vehicle.balance);
  Serial.println(F("========================================"));
  
  OLED_ShowPass(g_Vehicle.plateNumber, g_Vehicle.balance, TOLL_FEE);
  
  Gate_Open();
  delay(3000);
  Gate_Close();
  
  return true;
}

// ==================== 车辆检测 (按键) ====================
bool CheckVehicleDetected() {
  static bool lastState = HIGH;
  bool currentState = digitalRead(BUTTON_PIN);
  
  if (currentState == LOW && lastState == HIGH) {
    delay(50);
    if (digitalRead(BUTTON_PIN) == LOW) {
      lastState = LOW;
      return true;
    }
  }
  
  if (currentState == HIGH && lastState == LOW) {
    lastState = HIGH;
  }
  
  return false;
}

// ==================== 菜单 ====================
void Show_Menu() {
  Serial.println(F("\n+==========================================+"));
  Serial.println(F("|       ETC Highway System V1.0            |"));
  Serial.println(F("|  Button = Vehicle arrive                  |"));
  Serial.println(F("|  Tap card = Auto deduct + gate open      |"));
  Serial.println(F("+==========================================+"));
  Serial.println(F("|  Button = Simulate vehicle arrival        |"));
  Serial.println(F("|  Tap card = Auto deduct + gate open       |"));
  Serial.println(F("|  1 = Register ETC tag                    |"));
  Serial.println(F("|  2 = View account info                   |"));
  Serial.println(F("|  3 = Recharge 100 Yuan                   |"));
  Serial.println(F("|  4 = Add to blacklist                    |"));
  Serial.println(F("|  5 = Remove from blacklist               |"));
  Serial.println(F("|  9 = Full demo                           |"));
  Serial.println(F("|  m = Menu                                |"));
  Serial.println(F("+==========================================+"));
  Serial.print(F("[Toll] Fee: "));
  Serial.print(TOLL_FEE);
  Serial.println(F(" Yuan"));
}

// ==================== Setup ====================
void setup() {
  Serial.begin(9600);
  delay(2000);
  Serial.println(F(""));
  Serial.println(F("+==========================================+"));
  Serial.println(F("|     ETC System Starting...               |"));
  Serial.println(F("+==========================================+"));

  pinMode(BUTTON_PIN, INPUT_PULLUP);
  Serial.println(F("[Button] D2 init"));
  
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_RED, OUTPUT);

  // ========== 舵机初始化 (关键修正) ==========
  Serial.println(F("[Servo] Initializing..."));
  myServo.attach(SERVO_PIN);
  delay(200);                    // 等待舵机稳定
  myServo.write(0);              // 先写 0°
  delay(500);                    // 等待舵机转到 0°
  Serial.println(F("[Servo] Initialized to 0 degree"));
  // ==========================================

  OLED_Init();
  OLED_ShowETCReady();
  
  SPI_Setup();
  RC522_Init();
  Account_Init();
  
  Show_Menu();
  
  Serial.println(F("[System] ETC lane ready"));
}

// ==================== Loop ====================
void loop() {
  // 1. 车辆检测
  if (CheckVehicleDetected()) {
    Serial.println(F("\n[Vehicle] Arrived!"));
    OLED_ShowETCReady();
    
    Serial.println(F("[ETC] Reading tag..."));
    OLED_ShowMessage(F("Reading"), F("Please wait..."));
    
    int retryCount = 0;
    bool tagRead = false;
    
    while (retryCount < 30) {
      if (Card_Detect() && Card_GetUID()) {
        tagRead = true;
        break;
      }
      delay(100);
      retryCount++;
    }
    
    if (!tagRead) {
      Serial.println(F("[ETC] No tag detected"));
      OLED_ShowDenied(F("No tag"));
      delay(2000);
      OLED_ShowETCReady();
    } else {
      Serial.print(F("[RFID] UID: "));
      PrintHex(g_ScannedUID, 4);
      Serial.println();
      
      if (IDMatch(g_ScannedUID, g_Vehicle.cardID, 4)) {
        OLED_ShowVehicleInfo(g_Vehicle.plateNumber, g_Vehicle.balance);
        delay(500);
        ETC_ProcessTransaction();
      } else {
        Serial.println(F("[ETC] Unregistered tag"));
        OLED_ShowDenied(F("Unregistered"));
        delay(2000);
        OLED_ShowETCReady();
      }
      
      Card_Halt();
    }
  }
  
  // 2. 串口命令
  if(Serial.available()) {
    char cmd = Serial.read();
    while(Serial.available()) Serial.read();
    
    switch(cmd) {
      case '1': {
        Serial.println(F("\n[Register] Place tag..."));
        OLED_ShowMessage(F("Register"), F("Place tag"));
        
        int retry = 0;
        bool found = false;
        while (retry < 30) {
          if (Card_Detect() && Card_GetUID()) {
            found = true;
            break;
          }
          delay(100);
          retry++;
        }
        
        if (found) {
          Serial.print(F("[Register] UID: "));
          PrintHex(g_ScannedUID, 4);
          Serial.println();
          
          Account_Create(g_ScannedUID);
          OLED_ShowMessage(F("Registered!"), String(g_Vehicle.plateNumber));
          delay(1500);
          OLED_ShowETCReady();
        } else {
          Serial.println(F("[Register] No tag"));
          OLED_ShowMessage(F("Failed"), F("No tag"));
          delay(1500);
          OLED_ShowETCReady();
        }
        Card_Halt();
        break;
      }
      
      case '2': {
        Serial.println(F("\n[Account]"));
        if(g_AccountInit && g_Vehicle.isActive) {
          Serial.print(F("  Plate: "));
          Serial.println(g_Vehicle.plateNumber);
          Serial.print(F("  UID: "));
          PrintHex(g_Vehicle.cardID, 4);
          Serial.println();
          Serial.print(F("  Balance: "));
          Serial.print(g_Vehicle.balance);
          Serial.println(F(" Yuan"));
          Serial.print(F("  Status: "));
          Serial.println(g_Vehicle.isBlocked ? F("Blacklisted") : F("Normal"));
        } else {
          Serial.println(F("  No vehicle registered"));
        }
        break;
      }
      
      case '3': {
        if(g_AccountInit && g_Vehicle.isActive) {
          Account_Recharge(100);
          String msg = "New: " + String(g_Vehicle.balance) + "Yuan";
          OLED_ShowMessage(F("Recharged"), msg);
          delay(1500);
          OLED_ShowETCReady();
        } else {
          Serial.println(F("[Recharge] Register first"));
        }
        break;
      }
      
      case '4': {
        if(g_AccountInit && g_Vehicle.isActive) {
          g_Vehicle.isBlocked = true;
          Serial.println(F("[Blacklist] Added"));
          OLED_ShowMessage(F("Blacklisted"), String(g_Vehicle.plateNumber));
          delay(1500);
          OLED_ShowETCReady();
        }
        break;
      }
      
      case '5': {
        if(g_AccountInit && g_Vehicle.isActive) {
          g_Vehicle.isBlocked = false;
          Serial.println(F("[Blacklist] Removed"));
          OLED_ShowMessage(F("Removed"), String(g_Vehicle.plateNumber));
          delay(1500);
          OLED_ShowETCReady();
        }
        break;
      }
      
      case '9': {
        Serial.println(F("\n+==========================================+"));
        Serial.println(F("|          ETC Full Demo                   |"));
        Serial.println(F("+==========================================+"));
        Serial.println(F("Place tag..."));
        OLED_ShowMessage(F("Demo"), F("Place tag"));
        
        int retry = 0;
        bool found = false;
        while (retry < 30) {
          if (Card_Detect() && Card_GetUID()) {
            found = true;
            break;
          }
          delay(100);
          retry++;
        }
        
        if (found) {
          Serial.print(F("[Demo] UID: "));
          PrintHex(g_ScannedUID, 4);
          Serial.println();
          
          if (!g_AccountInit || !g_Vehicle.isActive) {
            Serial.println(F("[Demo] Auto register..."));
            Account_Create(g_ScannedUID);
          }
          
          if (IDMatch(g_ScannedUID, g_Vehicle.cardID, 4)) {
            OLED_ShowVehicleInfo(g_Vehicle.plateNumber, g_Vehicle.balance);
            delay(500);
            ETC_ProcessTransaction();
          }
        } else {
          Serial.println(F("[Demo] No tag"));
          OLED_ShowMessage(F("Demo failed"), F("No tag"));
          delay(1500);
          OLED_ShowETCReady();
        }
        Card_Halt();
        break;
      }
      
      case 'm':
      case 'M':
        Show_Menu();
        break;
    }
    delay(100);
  }
  
  delay(50);
}