#include <ESP32Servo.h>
#include <Arduino.h>
#include <analogWrite.h>



int Sensor_Limit_Max[]={2500,2500,2500,3000};
int Sensor_Limit_Min[]= {2300,2300,2300,0};

int Servo_Value[]= {180,180,180,180,180,180,180,180};

int Servo_Value_Mode1[]= {0,180,180};

int Servo_Value_Mode2[]= {90,90,90};

int Servo_Value_Mode3[]= {180,0,0};

int Mode = 2;

int marix[] = {5,6,7,4};

int verb_marix[] = {0,0,0,0};










int tmp[10];
Servo myservo;  // create servo object to control a servo

Servo myservo2;  // create servo object to control a servo
Servo myservo3;  // create servo object to control a servo
Servo myservo4;  // create servo object to control a servo
Servo myservo5;  // create servo object to control a servo
Servo myservo6;  // create servo object to control a servo
Servo myservo7;  // create servo object to control a servo
Servo myservo8;  // create servo object to control a servo
// 16 servo objects can be created on the ESP32
int pos = 0;  // variable to store the servo position
int servoPin = 42;
void setup() {
  Serial.begin(9600);
  // Allow allocation of all timers
  //::allocateTimer(0);
  //ESP32PWM::allocateTimer(1);
  //ESP32PWM::allocateTimer(2);
  //ESP32PWM::allocateTimer(3);
  pinMode(35,OUTPUT);
  pinMode(36,OUTPUT);
   
  pinMode(37,OUTPUT);
  pinMode(38,OUTPUT);
  pinMode(39,OUTPUT);
  pinMode(40,OUTPUT);
  pinMode(41,OUTPUT);
  pinMode(42,OUTPUT);
  myservo.setPeriodHertz(50);  // standard 50 hz servo
  myservo.attach(35, 500, 2500);
  myservo2.setPeriodHertz(50);  // standard 50 hz servo
  myservo2.attach(36, 500, 2500);
  myservo3.setPeriodHertz(50);  // standard 50 hz servo
  myservo3.attach(37, 500, 2500);
  myservo4.setPeriodHertz(50);  // standard 50 hz servo
  myservo4.attach(38, 500, 2500);
  myservo5.setPeriodHertz(500);  // standard 50 hz servo
  myservo5.attach(39, 500, 2500);
  myservo6.setPeriodHertz(50);  // standard 50 hz servo
  myservo6.attach(40, 500, 2500);
  myservo7.setPeriodHertz(50);  // standard 50 hz servo
  myservo7.attach(41, 500, 2500);
  myservo8.setPeriodHertz(50);  // standard 50 hz servo
  myservo8.attach(42, 500, 2500);
   
  pinMode(4,OUTPUT);
  pinMode(5,OUTPUT);
   
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  pinMode(8,OUTPUT);
  digitalWrite(4,0);
  digitalWrite(5,0);
  digitalWrite(6,0);
  digitalWrite(7,0);
  digitalWrite(8,0);
  
  pinMode(10,INPUT);
  pinMode(11,INPUT);
  pinMode(12,INPUT);
  pinMode(13,INPUT);

  myservo.write(Servo_Value[0]);  // tell servo to go to position in variable 'pos'
  myservo2.write(Servo_Value[1]);  // tell servo to go to position in variable 'pos'
  myservo3.write(Servo_Value[2]);  // tell servo to go to position in variable 'pos'
  myservo4.write(Servo_Value[3]);  // tell servo to go to position in variable 'pos'
  myservo5.write(Servo_Value[4]);  // tell servo to go to position in variable 'pos'
  myservo6.write(Servo_Value[5]);  // tell servo to go to position in variable 'pos'
  myservo7.write(Servo_Value[6]);  // tell servo to go to position in variable 'pos'
  myservo8.write(Servo_Value[7]);  // tell servo to go to position in variable 'pos'
  Serial.println("Welcome to XXXX！Please input the type you want to simulate：\n1：A\n2:B\n3:C");
}
void loop() {
  if (Serial.available() > 0) {
    // read the incoming byte:
    char user = (char)Serial.read();    
      if (user == '0') {
  myservo.write(Servo_Value[0]);  // tell servo to go to position in variable 'pos'
  myservo2.write(Servo_Value[1]);  // tell servo to go to position in variable 'pos'
  myservo5.write(Servo_Value[4]);  // tell servo to go to position in variable 'pos'
  //Serial.println("您选择了疾病类型1:A");
      } else if (user == '1') {
  myservo.write(Servo_Value_Mode1[0]);  // tell servo to go to position in variable 'pos'
  myservo2.write(Servo_Value_Mode1[1]);  // tell servo to go to position in variable 'pos'
  myservo5.write(Servo_Value_Mode1[4]);  // tell servo to go to position in variable 'pos'
  
  Serial.println("You choose the type 1:A");
      } else if (user == '2') {
  myservo.write(Servo_Value_Mode2[0]);  // tell servo to go to position in variable 'pos'
  myservo2.write(Servo_Value_Mode2[1]);  // tell servo to go to position in variable 'pos'
  myservo5.write(Servo_Value_Mode2[4]);  // tell servo to go to position in variable 'pos'
  Serial.println("You choose the type 2:B");
      }else if (user == '3') {
  myservo.write(Servo_Value_Mode3[0]);  // tell servo to go to position in variable 'pos'
  myservo2.write(Servo_Value_Mode3[1]);  // tell servo to go to position in variable 'pos'
  myservo5.write(Servo_Value_Mode3[4]);  // tell servo to go to position in variable 'pos'
  Serial.println("You choose the type 3:C");
      }
      else{
      }
  }
  /*
Serial.println("10:");
Serial.println(analogRead(10));
Serial.println("11:");
Serial.println(analogRead(11));
Serial.println("12:");
Serial.println(analogRead(12));
Serial.println("13:");
Serial.println(analogRead(13));
  delay(200);
  
  int tempvar = 0;
  */
  for(int i =0;i < 4;i++)
  {
    tmp[i] = analogRead(10 + i);
    if(tmp[i] < Sensor_Limit_Min[i])
    {
      if( Sensor_Limit_Min[i] - tmp[i] <255)
        verb_marix[i]=Sensor_Limit_Min[i] - tmp[i];
      else
        verb_marix[i]= 254;
    }
    else if(tmp[i] > Sensor_Limit_Max[i])
    {
      if(tmp[i]- Sensor_Limit_Max[i] <255)
        verb_marix[i]=tmp[i]-Sensor_Limit_Max[i] ;
      else
        verb_marix[i]= 254;
    }
    else
        verb_marix[i]= 0;

  }
    switch(Mode)
      {
        case 0:
          for(int i =0;i < 4;i++)
            analogWrite(marix[i], verb_marix[i]);
          delay(200);
          break;
        case 1:
          for(int i =0;i < 4;i++)
            analogWrite(marix[i], verb_marix[i]);
          delay(500);
          for(int i =0;i < 4;i++)
            analogWrite(marix[i], 0);
          delay(500);
          break;
        case 2:
          for(int i =0;i < 4;i++)
            analogWrite(marix[i], verb_marix[i]);
          delay(200);
          for(int i =0;i < 4;i++)
            analogWrite(marix[i], 0);
          delay(200);
          for(int i =0;i < 4;i++)
            analogWrite(marix[i], verb_marix[i]);
          delay(200);
          for(int i =0;i < 4;i++)
            analogWrite(marix[i], 0);
          delay(200);
          break;
        case 3:
          for(int i =0;i < 4;i++)
            analogWrite(marix[i], verb_marix[i]);
          delay(800);
          for(int i =0;i < 4;i++)
            analogWrite(marix[i], 0);
          delay(200);
          break;
  
  }
  
/*
  for (pos = 0; pos <= 180; pos += 1) {  // goes from 0 degrees to 180 degrees
    myservo.write(pos);  // tell servo to go to position in variable 'pos'
    myservo2.write(pos);  // tell servo to go to position in variable 'pos'
    myservo3.write(pos);  // tell servo to go to position in variable 'pos'
    myservo4.write(pos);  // tell servo to go to position in variable 'pos'
    myservo5.write(pos);  // tell servo to go to position in variable 'pos'
    myservo6.write(pos);  // tell servo to go to position in variable 'pos'
    myservo7.write(pos);  // tell servo to go to position in variable 'pos'
    myservo8.write(pos);  // tell servo to go to position in variable 'pos'
    delay(15);           // waits 15ms for the servo to reach the position
  }
  delay(1000);
  for (pos = 180; pos >= 0; pos -= 1) {  // goes from 180 degrees to 0 degrees
    myservo.write(pos);  // tell servo to go to position in variable 'pos'
    myservo2.write(pos);  // tell servo to go to position in variable 'pos'
    myservo3.write(pos);  // tell servo to go to position in variable 'pos'
    myservo4.write(pos);  // tell servo to go to position in variable 'pos'
    myservo5.write(pos);  // tell servo to go to position in variable 'pos'
    myservo6.write(pos);  // tell servo to go to position in variable 'pos'
    myservo7.write(pos);  // tell servo to go to position in variable 'pos'
    myservo8.write(pos);  // tell servo to go to position in variable 'pos'
    delay(15);                           // waits 15ms for the servo to reach the position
  }
  delay(1000);
  
if(analogRead(12) < 10)
{
  myservo.write(0);
    analogWrite(1, 0);
}else
{
  
  myservo.write(100);
    analogWrite(1, 244);
}
  for (pos = 0; pos <= 180; pos += 1) {  // goes from 0 degrees to 180 degrees
    myservo.write(pos);  // tell servo to go to position in variable 'pos'
    delay(15);           // waits 15ms for the servo to reach the position
  }
  delay(3000);
  for (pos = 180; pos >= 0; pos -= 1) {  // goes from 180 degrees to 0 degrees
    myservo.write(pos);                  // tell servo to go to position in variable 'pos'
    delay(15);                           // waits 15ms for the servo to reach the position
  }
  delay(3000);
  
  for (pos = 0; pos <= 255; pos += 1) {  // goes from 0 degrees to 180 degrees
    
    analogWrite(1, pos); // 通道0输出， PWM输出0~100%（0~2^10=1024）
    delay(15);           // waits 15ms for the servo to reach the position
  }
  pos=0;
  delay(3000);
  */
}