//MAP TPS比较输出模式  三模式切换 eeprom 可用版本  已经增加舵机
//TODO： 增加失效保用
//todo：以ALL IN ONE 硬件为标准 增加显示部分
#include <Arduino.h>
#include <EEPROM.h> // 引入EEPROM库
#include <Servo.h> // Include the Servo library
#include "OLEDstuff.cpp"


const int button = 12; // 按钮引脚
const int TPSPin = A1; // TPS传感器引脚
const int MAPPin = A0; // map输入引脚
const int outputPin = 6; // 输出引脚，用于输出电压
const int servoPin = 13; // 舵机引脚
const int eepromAddress = 0; // eeprom中存储MODE的地址
const char* mode = "ver 3.2";

int tpsValue = 0; // TPS传感器数值
int mapValue = 0; // map传感器数值
int outputValue = 0; // 输出值
int tpsoutValue = 0; // TPS传感器数值
int mapoutValue = 0; // map传感器数值
int value = 0; // 新增的变量，用于存储三种情况的值
int MODE = 0;//保持按钮按下的变量
int old = 0;//保持按钮状态的变量
int buttonPoll = 0;

Servo cableservo; // 创建一个Servo对象

void setup() {

  pinMode(SW1, INPUT_PULLUP);//设置按钮为输入
  pinMode(SW2, INPUT_PULLUP);//设置按钮为输入
  pinMode(LED, OUTPUT);
  pinMode(GEAR_P, INPUT);
  pinMode(GEAR_R, INPUT);
  pinMode(GEAR_D, INPUT); //确认和重置
  pinMode(GEAR_2, INPUT);
  pinMode(GEAR_L, INPUT);
  pinMode(GEAR_F, INPUT_PULLUP);
  pinMode(GEAR_LOCK, INPUT_PULLUP);
  pinMode(outputPin, OUTPUT);
  pinMode(button, INPUT_PULLUP); //设置按钮为输入

  cableservo.attach(servoPin); // 将舵机连接到13号引脚
  Serial.begin(115200); // 初始化串口通信
  EEPROM.begin();
  MODE = EEPROM.get(eepromAddress, MODE); // 从eeprom中读取MODE的值

  OLED.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
  OLED.clearDisplay();
  OLED.drawBitmap(0, 0, SS4logo, 128, 64, WHITE);
  OLED.display();

  delay(500);
  OLED.clearDisplay();
  OLED.drawBitmap(0, 0, logo, 128, 64, WHITE);
  OLED.display();

  delay(500);

}

void loop() {


  buttonPoll = digitalRead(button);
  if (buttonPoll == 1) {
    delay(50);
    buttonPoll = digitalRead(button);
    if (buttonPoll == 0) {
      MODE = old + 1;
    }
  }

  else {
    delay(100);
  }

  EEPROM.put(eepromAddress, MODE); // 将MODE的值写入eeprom中
  switch (MODE) { // 根据当前的值切换到下一个值

    default:  // 默认模式
      value = 255;
      old = 0;
      mode = "ECO";
      break; // 跳出switch语句
    case 1:
      value = 305;
      old = MODE;
      mode = "NORMAL";
      break; // 跳出switch语句
    case 2:
      value = 355;
      old = MODE;
      mode = "RACING";
      break; // 跳出switch语句
  }


  tpsValue = analogRead(TPSPin);
  mapValue = analogRead(MAPPin); // 读取tps和map的电压

  float tpspercent = map(tpsValue, 27, 900, 0, 100);
  float KPA = map(mapValue, 0, 1023, 9, 208);
  tpsoutValue = map(tpsValue, 61, 660, 0, value);
  mapoutValue = map(mapValue, 123, 750, 0, 255);

  /*
    比较TPS和MAP的输入电压大小，然后输出较大的数值
  */
  if (tpsoutValue > mapoutValue) {

    outputValue = map(tpsValue, 61, 660, 0, value);

  }  else {

    outputValue = map(mapValue, 123, 750, 0, 255);
  }
  outputValue = constrain(outputValue, 20, 170 ); //限制输出范围



  analogWrite(outputPin, outputValue); // 输出比较后的电压

  // 控制舵机的位置
  int servoPosition = map(outputValue, 20, 170, 0, 90); // 将TPS传感器的值映射到舵机的位置范围
  servoPosition = constrain(servoPosition, 0, 90 ); //限制输出范围
  cableservo.write(servoPosition); // 将舵机转到指定位置


  {

    SW1Valve = digitalRead(SW1);
    SW2Valve = digitalRead(SW2);
    GEAR_PValve = digitalRead(GEAR_P);
    GEAR_RValve = digitalRead(GEAR_R);
    GEAR_DValve = digitalRead(GEAR_D);
    GEAR_2Valve = digitalRead(GEAR_2);
    GEAR_LValve = digitalRead(GEAR_L);
    GEAR_FValve = digitalRead(GEAR_F);
    GEAR_LOCKValve = digitalRead(GEAR_LOCK);
    //delay(5);
    //digitalWrite (LED,LOW);
    //delay(10);


    if     (GEAR_PValve == HIGH)
    {
      Gear = "P"; //gear p
    }
    else if (GEAR_RValve == HIGH)
    {
      Gear = "R"; //gear R
    }
    else if (GEAR_DValve == HIGH)
    {
      Gear = "D"; //gear D
    }
    else if (GEAR_2Valve == HIGH)
    {
      Gear = "2"; //gear 2
    }
    else if (GEAR_LValve == HIGH)
    {
      Gear = "L"; //gear L
    }
    else
    {
      Gear = "N"; //gear N
    }

    if   (GEAR_FValve == LOW)
    {
      DRIVE = "4WD"; //gear L
    }

    else
    {
      DRIVE = "";
    }

    if (SW1Valve == HIGH && SW2Valve == HIGH)
    { AT = "ECO";
    }
    // 输入控制代码

    else if (SW1Valve == HIGH && SW2Valve == LOW)
    { AT = "POWER";

    }
    else if (SW1Valve == LOW && SW2Valve == HIGH)
    { AT = "TPS";
      //输入控制代码
    }



    /*
      以下是串口输出内容
    */
    Serial.print(AT);
    Serial.print("  |  ");
    Serial.print(Gear);
    Serial.print("  |  ");
    Serial.print("MODE: ");
    Serial.print(mode);
    Serial.print(" TPS:");
    Serial.print(tpspercent);
    Serial.print("% ");
    Serial.print("  Map: ");
    Serial.print(KPA);
    Serial.print("Kpa ");
    Serial.print(" V=: ");
    Serial.println(outputValue * (5.0 / 255.0));

    /*
      以下是OLED输出内容
    */
    OLED.clearDisplay();
    OLED.setTextSize(9);
    OLED.setTextColor(WHITE);
    OLED.setCursor(43, 0);
    OLED.println(Gear);
    OLED.setTextSize(2);
    OLED.setTextColor(WHITE);
    OLED.setCursor(92, 0);
    OLED.println(DRIVE);
    OLED.setTextSize(1);
    OLED.setTextColor(WHITE);
    OLED.setCursor(98, 20);
    OLED.println(VCU);
    OLED.setTextSize(1);
    OLED.setTextColor(WHITE);
    OLED.setCursor(100, 50);
    OLED.println(AT);
    OLED.display();

  }
}