#include <Arduino.h>
#include <U8g2lib.h>
#include <Servo.h>
#include <Wire.h>

//U8G2_SSD1306_128X64_NONAME_1_SW_I2C u8g2(U8G2_R0, /* clock=*/ SCL, /* data=*/ SDA, /* reset=*/ U8X8_PIN_NONE);   // All Boards without Reset of the Display,Default.PICO SCL=5 SDA=4
U8G2_SSD1306_128X64_NONAME_1_HW_I2C u8g2(U8G2_R0, /* clock=*/5, /* data=*/4, /* reset=*/U8X8_PIN_NONE);  // ESP32 Thing, HW I2C with pin remapping

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

const int buttonPinUp = 0;      // 上键
const int buttonPinDown = 1;    // 下键
const int buttonPinLeft = 2;    // 左键
const int buttonPinRight = 3;   // 右键
const int buttonPinCenter = 6;  // 中键

int buttonStateUp = HIGH;
int buttonStateDown = HIGH;
int buttonStateLeft = HIGH;
int buttonStateRight = HIGH;
int buttonStateCenter = HIGH;


unsigned long buttonPressTimeCenter = 0;
bool centerButtonLongPressed = false;
const int longPressThreshold = 1000;  // 长按时间阈值（毫秒）


int servoAnglesA[] = { 90, 90, 90, 90 };
int servoAnglesB[] = { 0, 0, 0, 0 };
int servo_angles_open_temp[4];
int servo_angles_close_temp[4];



int upButtonnum = 0;
int downButtonnum = 0;
int leftButtonnum = 0;
int rightButtonnum = 0;
int centerbuttonnum = 0;
int rownum = 0;
int columnnum = 0;

void setup() {
  Serial.begin(9600);
  servo1.attach(9);
  servo2.attach(10);
  servo3.attach(11);
  servo4.attach(12);

  pinMode(buttonPinUp, INPUT_PULLUP);
  pinMode(buttonPinDown, INPUT_PULLUP);
  pinMode(buttonPinLeft, INPUT_PULLUP);
  pinMode(buttonPinRight, INPUT_PULLUP);
  pinMode(buttonPinCenter, INPUT_PULLUP);

  u8g2.begin();
  u8g2.enableUTF8Print();
}

void PressedButton() {
  // 读取按键状态
  int readingUp = digitalRead(buttonPinUp);
  int readingDown = digitalRead(buttonPinDown);
  int readingLeft = digitalRead(buttonPinLeft);
  int readingRight = digitalRead(buttonPinRight);
  int readingCenter = digitalRead(buttonPinCenter);
  // 判断下降沿触发
  if (readingUp == LOW && buttonStateUp == HIGH) {
    // 上键按下
    Serial.println("Up button pressed");
  }
  if (readingDown == LOW && buttonStateDown == HIGH) {
    // 下键按下
    Serial.println("Down button pressed");
  }
  if (readingLeft == LOW && buttonStateLeft == HIGH) {
    // 左键按下
    Serial.println("Left button pressed");
  }
  if (readingRight == LOW && buttonStateRight == HIGH) {
    // 右键按下
    Serial.println("Right button pressed");
  }

  // 判断中键下降沿触发和长按
  if (readingCenter == LOW && buttonStateCenter == HIGH) {
    // 中键按下
    Serial.println("Center button pressed");
    buttonPressTimeCenter = millis();  // 记录按下时间
    centerButtonLongPressed = false;   // 重置长按标志
  }
  if (readingCenter == HIGH && buttonStateCenter == LOW) {
    // 中键释放
    Serial.println("Center button released");
    unsigned long buttonReleaseTime = millis();
    if (!centerButtonLongPressed && (buttonReleaseTime - buttonPressTimeCenter) < longPressThreshold) {
      // 中键短按
      Serial.println("Center button short pressed");
      centerbuttonnum++;
    }
    if (buttonReleaseTime - buttonPressTimeCenter > longPressThreshold) {
      // 中键长按
      Serial.println("Center button long pressed");
      centerbuttonnum--;
      centerButtonLongPressed = true;  // 设置长按标志
    }
  }

  // 更新按键状态
  buttonStateUp = readingUp;
  buttonStateDown = readingDown;
  buttonStateLeft = readingLeft;
  buttonStateRight = readingRight;
  buttonStateCenter = readingCenter;
  delay(100);
}

void default_display() {
  u8g2.setFont(u8g2_font_8x13_mr);
  u8g2.setContrast(1);  //0-255

  u8g2.firstPage();
  do {

    for (int i = 0; i < 4; i++) {

      u8g2.setCursor(0, 10);
      u8g2.print("Servo");

      u8g2.setCursor(45 + 22 * i, 10);
      u8g2.print(i + 1);

      u8g2.setCursor(0, 35);
      u8g2.print("start");

      u8g2.setCursor(0, 60);
      u8g2.print("end");

      u8g2.setCursor(45 + 22 * i, 35);
      u8g2.print(servoAnglesA[i]);

      u8g2.setCursor(45 + 22 * i, 60);
      u8g2.print(servoAnglesB[i]);
    }
  } while (u8g2.nextPage());
}

void edit_mode() {
  if (centerbuttonnum = 2) {

    switch (PressedButton()) {
      case upButton:
        //upButtonnum ++;
        if (servo_angles_open_temp[columnnum] < 180) {
          servo_angles_open_temp[columnnum]++;
        }
        break;
      case downButton:
        //downButtonnum ++;
        if (servo_angles_open_temp[columnnum] > 0) {
          servo_angles_open_temp[columnnum]--;
        }
        break;
      case centerButton:
        centerbuttonnum++;
      default:
        // 没有按钮被按下
        break;
    }
    void edit_display() {

      /*  int centerbuttonlongPress = 0;
  int centerbuttonshortPress = 0;
  while (centerbuttonnum != 3) {
    Serial.println(centerbuttonnum);

    if (PressedButton() == centerButton) {
      if (centerbuttonnum < 3) {
        centerbuttonnum++;
      }
    }
    if (PressedButton() == canclecenterButton) {
      if (centerbuttonnum > 0) {
        centerbuttonnum--;
      }
    }
    if (centerbuttonnum = 1) {


      delay(100);
    }
    for (int i = 0; i < 4; i++) {
      servo_angles_open_temp[i] = servoAnglesA[i];
    }
*/

      /*    if (centerbuttonnum = 2) {

      switch (PressedButton()) {
        case upButton:
          //upButtonnum ++;
          if (servo_angles_open_temp[columnnum] < 180) {
            servo_angles_open_temp[columnnum]++;
          }
          break;
        case downButton:
          //downButtonnum ++;
          if (servo_angles_open_temp[columnnum] > 0) {
            servo_angles_open_temp[columnnum]--;
          }
          break;
        case centerButton:
          centerbuttonnum++;
        default:
          // 没有按钮被按下
          break;
      }
      
    }


    servo_angles_open_temp[columnnum];



*/
      int x = 20 + (22 * (columnnum + 1));  //方框的x坐标
      int y = 20 + (26 * rownum);           //方框的y坐标
      int w = 20;                           //方框的宽度
      int h = 18;                           //方框的高度
      u8g2.setFont(u8g2_font_8x13_mr);
      u8g2.setContrast(1);  //0-255
      u8g2.firstPage();
      do {
        u8g2.drawFrame(x, y, w, h);  //修改方框
        for (int i = 0; i < 4; i++) {

          u8g2.setCursor(0, 10);
          u8g2.print("Servo");

          u8g2.setCursor(45 + 22 * i, 10);
          u8g2.print(i + 1);

          u8g2.setCursor(0, 35);
          u8g2.print("open");

          u8g2.setCursor(0, 60);
          u8g2.print("close");

          u8g2.setCursor(45 + 22 * i, 35);
          u8g2.print(servo_angles_open_temp[i]);

          u8g2.setCursor(45 + 22 * i, 60);
          u8g2.print(servo_angles_close_temp[i]);
        }
      } while (u8g2.nextPage());
    }



    // 打开时的程序
    void open() {
      for (int i = 0; i < 4; i++) {
        // 设置舵机角度为 servoAnglesA 组的值
        servo1.write(servoAnglesA[i]);
        servo2.write(servoAnglesA[i]);
        servo3.write(servoAnglesA[i]);
        servo4.write(servoAnglesA[i]);
      }
      u8g2.firstPage();
      do {
        u8g2.setCursor(50, 35);
        u8g2.print("OPEN");
      } while (u8g2.nextPage());
      delay(1000);
    }
    // 关闭时的程序
    void close() {
      for (int i = 0; i < 4; i++) {
        // 设置舵机角度为 servoAnglesA 组的值
        servo1.write(servoAnglesB[i]);
        servo2.write(servoAnglesB[i]);
        servo3.write(servoAnglesB[i]);
        servo4.write(servoAnglesB[i]);
      }
      u8g2.firstPage();
      do {
        u8g2.setCursor(50, 35);
        u8g2.print("CLOSE");
      } while (u8g2.nextPage());
      delay(1000);
    }

    void loop() {

      // 读取按键状态
      int readingUp = digitalRead(buttonPinUp);
      int readingDown = digitalRead(buttonPinDown);
      int readingLeft = digitalRead(buttonPinLeft);
      int readingRight = digitalRead(buttonPinRight);
      int readingCenter = digitalRead(buttonPinCenter);

      u8g2.clearBuffer();
      PressedButton();

      Serial.println("中键点按次数");
      Serial.println(centerbuttonnum);
      /*
  switch (PressedButton()) {
    case upButton:
      open();
      break;
    case downButton:
      close();
      break;
    case centerButton:
      edit_mode();
      edit_display();
      break;
    default:
      if(centerbuttonnum == 1){
        edit_display();
      }
      else
        default_display();
      break;
  }
  */
      // 如果 upButton 被按下，执行 servoAnglesA 组的角度
      if (readingUp == LOW) {
        open();
      }

      // 如果 downButton 被按下，执行 servoAnglesB 组的角度
      if (readingDown == LOW) {
        close();
      }

      // 如果 centerButton 被按下，执行角度修改
      if (readingCenter == LOW) {
        edit_display();
      }

      default_display();

      u8g2.sendBuffer();
    }