#include <Servo.h>
#include <stdio.h>

#if defined(ARDUINO_ARCH_AVR)
#include <Servo.h>
#define DEFAULT_uS_LOW  MIN_PULSE_WIDTH
#define DEFAULT_uS_HIGH MAX_PULSE_WIDTH
#endif
#if defined(ARDUINO_ARCH_ESP32)
#include <ESP32Servo.h>
#endif

#define MIN_MICROS_VALUE 200
#define MAX_MICROS_VALUE 10000

/*
map   0 to  544, 
map 180 to 2400, 
so 90deg is 1472, 45deg is 1008;

*/

class ServoSpeedControl  : public Servo {

private:
    unsigned long oldTime=0;
    unsigned int minAngleMicros=DEFAULT_uS_LOW;
    unsigned int maxAngleMicros=DEFAULT_uS_HIGH;
    unsigned int goalAngleMicros=((minAngleMicros+maxAngleMicros)/2);
    unsigned int oldAngleMicros=goalAngleMicros;
    unsigned int delayMicros=MAX_MICROS_VALUE;
    byte minAngle=0;
    byte maxAngle=180;
    byte goalAngle=90;
    byte delay=0;
    byte speed=0;

public:
    void setMinAngleMicros(unsigned int value);
    unsigned int getMinAngleMicros();
    void setMaxAngleMicros(unsigned int value);
    unsigned int getMaxAngleMicros();
    void setGoalAngleMicros(unsigned int angle);
    unsigned int getGoalAngleMicros();
    void setDelayMicros(unsigned int delay);
    unsigned int getDelayMicros();
    void setMinAngle(byte value);
    byte getMinAngle();
    void setMaxAngle(byte value);
    byte getMaxAngle();
    void setGoalAngle(byte value);
    byte getGoalAngle();
    void setDelay(byte value);
    byte getDelay();
    void setSpeed(byte value);
    byte getSpeed();
    void doStep();
    bool isInGoal();
};



void ServoSpeedControl::setMinAngleMicros(unsigned int value){
        if (value<DEFAULT_uS_LOW)
            value=DEFAULT_uS_LOW;
        if (value<maxAngleMicros)
            minAngleMicros=value;
}

unsigned int ServoSpeedControl::getMinAngleMicros(){
        return minAngleMicros;
}

void ServoSpeedControl::setMaxAngleMicros(unsigned int value){
        if (value>DEFAULT_uS_HIGH)
            value=DEFAULT_uS_HIGH;
        if (value>minAngleMicros)
            maxAngleMicros=value;
}

unsigned int ServoSpeedControl::getMaxAngleMicros(){
        return maxAngleMicros;
}

void ServoSpeedControl::setGoalAngleMicros(unsigned int value){
        if ((value>=minAngleMicros)&&(value<=maxAngleMicros))
            goalAngleMicros=value;  
}
  
unsigned int ServoSpeedControl::getGoalAngleMicros(){
        return goalAngleMicros;
}

void ServoSpeedControl::setDelayMicros(unsigned int value){
            delayMicros=value;
}

unsigned int ServoSpeedControl::getDelayMicros(){
        return delayMicros;
}

void ServoSpeedControl::setMinAngle(byte value){
        if (value<0)
            value=0;
        if (value>180) 
            value=180;
        minAngle=value;
        this->setMinAngleMicros(map(value,0,180,DEFAULT_uS_LOW,DEFAULT_uS_HIGH));      
}

byte ServoSpeedControl::getMinAngle(){
        return minAngle;
}

void ServoSpeedControl::setMaxAngle(byte value){
        if (value<0)
            value=0;
        if (value>180) 
            value=180;
        maxAngleMicros=map(value,0,180,DEFAULT_uS_LOW,DEFAULT_uS_HIGH);
        maxAngle=value;
}

byte ServoSpeedControl::getMaxAngle(){
        return maxAngle;
}

void ServoSpeedControl::setGoalAngle(byte value){
        this->setGoalAngleMicros(map(value,0,180,DEFAULT_uS_LOW,DEFAULT_uS_HIGH));
        goalAngle=value;  
}
  
byte ServoSpeedControl::getGoalAngle(){
        return goalAngle;
}

void ServoSpeedControl::setDelay(byte value){           
        delayMicros=(value*1000);
        delay=value;
}

byte ServoSpeedControl::getDelay(){
        return delay;
}

void ServoSpeedControl::setSpeed(byte value){
        if ((value<=10)&&(value>=1)) {
            delayMicros=map(value,1,10,MAX_MICROS_VALUE,MIN_MICROS_VALUE);
            speed=value;
        }
}

byte ServoSpeedControl::getSpeed(){
        return speed;
}

void ServoSpeedControl::doStep() { 
        if (micros()>(oldTime+delayMicros)) { 
            if (goalAngleMicros>oldAngleMicros){
                Servo::writeMicroseconds(oldAngleMicros+1);
                oldAngleMicros++;
            }
            if (goalAngleMicros<oldAngleMicros){
                Servo::writeMicroseconds(oldAngleMicros-1); 
                oldAngleMicros--;
            }
            oldTime=micros();
        }
}

bool ServoSpeedControl::isInGoal() {
        if (goalAngleMicros==oldAngleMicros)
            return true;
        return false;
}

//======================================================



ServoSpeedControl myservo;
ServoSpeedControl myservo5;
ServoSpeedControl myservo3;

const byte servoPin1 = 7;
const byte servoPin5 = 5;
const byte servoPin3 = 3;

// initial angle is same as MinAngleMicros
unsigned int angle = myservo.getMinAngleMicros();
int inputAngle;

struct ServoPin
{
        Servo theservo;
        int servoPin;
        String servoName;
        int initialPosition;
};

ServoPin servoPins[3]=
{
        {Servo(), 11, "servo11", 2},
        {Servo(), 12, "servo12", 45},
        {Servo(), 13, "servo13", 135} 
};


struct RecordedStep
{
  int servoIndex;
  int value;
  int delayInStep;
};


RecordedStep recordedSteps[] = {
        {0,30,31}, 
        {1,120,32}, 
        {2,130,33},
        {0,110,34},
        {1,20,34},
        {2,45,33}
        };


/*
int *P[2];
P[0] = recordedSteps[0];
P[1] = recordedSteps[1];
*/


void setup() {

   Serial.begin(115200);



   myservo.attach(servoPin1);  
   myservo5.attach(servoPin5);
   myservo3.attach(servoPin3);


  for (int i = 0; i<3;i++)
        {
        servoPins[i].theservo.attach(servoPins[i].servoPin);
        
        }

   
   for(int i=0; i<(sizeof(recordedSteps)/sizeof(recordedSteps[0]));i++)
        {
                bool inGoal = 0;                          
                RecordedStep &recordedStep = recordedSteps[i];
                while(!inGoal)
                  {
                        int currentPosition = servoPins[(recordedStep.servoIndex)].theservo.read();
                        currentPosition = (
                          currentPosition > recordedStep.value ? 
                          currentPosition - 1 : currentPosition + 1);
                        servoPins[recordedStep.servoIndex].theservo.write(currentPosition);
                        
                        inGoal = (currentPosition == recordedStep.value);

                   delay(10);
                  }
               
        }

  /*
  // to move all the servo in the same time
  int currentPosition[3];
  for (int i = 0; i<3;i++)
        {
        servoPins[i].theservo.attach(servoPins[i].servoPin);
        currentPosition[i] = servoPins[i].theservo.read();  
        }
  bool inGoal[3] = {0, 0, 0};
  while (!inGoal[0] || !inGoal[1] || !inGoal[2])
        {
                // to delay time for each step 
                for(int i=0; i<3;i++)
                {
                  if (!inGoal[i])
                  {
                        currentPosition[i] = (
                          currentPosition[i] > servoPins[i].initialPosition ? 
                          currentPosition[i] - 1 : currentPosition[i] + 1);
                        servoPins[i].theservo.write(currentPosition[i]);
                        
                        inGoal[i] = (currentPosition[i] == servoPins[i].initialPosition);
                  }

                  //check position
                  //Serial.println(currentPosition[i]);

                }
                delay(50);
        }
    */
    //Serial.println(recordedStep.servoIndex);
    //Serial.println(recordedStep.value);
    //Serial.println(recordedStep.delayInStep);
    //Serial.println(recordedSteps[0][1]);
    //Serial.println(currentPosition[0]);
    
    
}

void loop() {
    
    //Sets the delay in microseconds between the steps
    myservo.setDelayMicros(500);
    
    if ((angle==myservo.getMinAngleMicros()) && (myservo.isInGoal())) {
      angle = myservo.getMaxAngleMicros();
      myservo.setGoalAngleMicros(angle);
      //myservo5.setGoalAngleMicros(angle); 
    }
    if ((angle==myservo.getMaxAngleMicros()) && (myservo.isInGoal())) {
      angle = myservo.getMinAngleMicros();
      myservo.setGoalAngleMicros(angle);
      //myservo5.setGoalAngleMicros(angle);
    }    
    //Call this method to execute the movement   
    myservo.doStep();
    
    

    //for test servo
    myservo5.setDelayMicros(2000);
    myservo3.setDelayMicros(1000);
    myservo5.setGoalAngle(180);
    myservo5.doStep();
    delay(0);

    myservo3.setGoalAngle(180);  
    //myservo5.setGoalAngleMicros(544);
    //myservo3.setGoalAngleMicros(2400);
    myservo3.doStep();


    
}