#include <ESP32Servo.h>

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define OLED_ADDR 0x3C
Adafruit_SSD1306 display(128, 64, &Wire, -1);
int count700ms = 0;    // count to 700ms for first servo motor


const int servoPin = 13;
const int servo2Pin = 14;
const int servo3Pin = 15;

const int LedRedPin = 2;
const int LedBluePin = 15;
const int LdrPin = 4;
int pos = 0; //servo1初始位置
Servo servo1;
Servo servo2;
Servo servo3;
const unsigned long interval = 10;
// 待机模式
void standbyMode(void *parameter) {
  while (true) {
    digitalWrite(LedRedPin, HIGH);
    delay(100);
    digitalWrite(LedRedPin, LOW);
    digitalWrite(LedBluePin, HIGH);
    delay(100);
    digitalWrite(LedBluePin, LOW);
  }
  vTaskDelete(NULL);
}

// 监听传感器状态
void sensorListener(void *parameter) {
  while (true) {
    if (digitalRead(LdrPin) == LOW) {
      for (pos = 0; pos <= 70; pos += 1) { // 从0度转到70度
        servo1.write(pos);              // 告诉舵机转到pos位置
        delay(5);                       // 等待15毫秒，让舵机达到目标位置
      }
      for (pos = 70; pos >= 0; pos -= 1) { // 从70度转回0度
        servo1.write(pos);              // 告诉舵机转到pos位置
        delay(5);                       // 等待15毫秒，让舵机达到目标位置
      }
    }
    // Serial.println("sensorListener end");
  }
  vTaskDelete(NULL);
}
void oledDisplay(void *parameter) {
  while (true) {
    display.clearDisplay();
    delay(300);
    // 画一个示例图像
    display.fillCircle(32, 32, 20, WHITE);
    display.fillCircle(96, 32, 20, WHITE);
    display.display();
    delay(2000);
    display.clearDisplay();
    display.display();


    Serial.println("oledDisplay end");

  }
  vTaskDelete(NULL);
}


void setup() {
  Serial.begin(9600);
  servo1.attach(servoPin);
  servo2.attach(servo2Pin);

  pinMode(LedRedPin, OUTPUT);
  pinMode(LedBluePin, OUTPUT);
  pinMode(LdrPin, INPUT);
  move(0, 5000, 0, 0);


  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);

  xTaskCreate(
    standbyMode,   /* 任务函数. */
    "StandbyMode", /* 任务名. */
    10000,     /* 栈大小. */
    NULL,      /* 传递给任务函数的参数. */
    1,         /* 任务优先级. */
    NULL);     /* 任务句柄. */

  xTaskCreate(
    sensorListener,   /* 任务函数. */
    "SensorListener", /* 任务名. */
    10000,     /* 栈大小. */
    NULL,      /* 传递给任务函数的参数. */
    1,         /* 任务优先级. */
    NULL);     /* 任务句柄. */

  xTaskCreate(
    oledDisplay,   /* 任务函数. */
    "OledDisplay", /* 任务名. */
    10000,     /* 栈大小. */
    NULL,      /* 传递给任务函数的参数. */
    1,         /* 任务优先级. */
    NULL);     /* 任务句柄. */
}

void loop() {
}
//
void move( int leftRpm, int leftRuntime, int rightRpm, int rightRuntime) {
  //开始时间
  unsigned long beginMillis = millis();
  int pos = 0;
  while (true) {
    delay(5);
    unsigned long currentMillis = millis();
    if ( currentMillis - beginMillis < leftRuntime) {
      servo2.write(pos);
      if (pos == 180) {
        pos = 0;
      } else {
        pos += 1;
      }
    }
  }

}