/*
  this code is written by Bob van der Weide
  The code is made to run multiple servos with it.
  The servos can be coupled to eachother, the coupled servos can not move while the other is active.
  The coupled servos can also force the other servo not to move while the servo is at a certain location.
*/

//===================================DEFINING VARIABLES==================================

// checks wich board is connected
#ifdef ESP32
#include <ESP32Servo.h>
#elif defined(ARDUINO_AVR_UNO)
#include <Servo.h>
#else
#error board not supported
#endif

const int amountServos = 4;

// place where the initial values of the servos will be saved
typedef struct servo_values
{
  int id; // this is the place where the number of the servo has to come, starting from 0
  int servoPin; // this is for the pin where the servo gets it signals from
  int analogPin; // this is for the pin where will be detected if the servo is stalling
  int couple; // this is where you declare what the ID is from the coupled servo
  int minWorkingLocation; // this is where you set the minimum degrees where the servo can go to
  int maxWorkingLocation; // this is where you set the maximum degrees where the servo can go to
  int maxCoupleLocation; // this is the maximum location where the coupled servo can be for this servo to work
  int minCoupleLocation; // this is the minimum location where the coupled servo can be for this servo to work
  int minStepSize = 10; // this is the minimum degrees each step will be
  int maxStepSize = 40; // this is the maximum degrees each step will be
  int startLocation = 0; // this is the location where the servo will go to when the program starts 
  int minStopLocation = 180;
  int maxStopLocation = 0;
  int stopValue = 560; // if this value gets reached than there something is stalling the servo
} servo_value_storage;

// sets how many servos there will be
servo_value_storage servo[amountServos];
Servo servoNumber[amountServos];

// keeps track of the time (internal clock)
unsigned long startMillis;
unsigned long currentMillis;



//===================================SERVO INFORMATION===================================

// location where you have to put in the values for the servos
void define_servos(void)
{
  servo[0].id = 0;
  servo[0].servoPin = 21;
  servo[0].analogPin = 4;
  servo[0].minWorkingLocation = 0;
  servo[0].maxWorkingLocation = 180;
  servo[0].minCoupleLocation = 100;
  servo[0].maxCoupleLocation = 180;
  servo[0].minStepSize = 4;
  servo[0].maxStepSize = 4;
  servo[0].startLocation = 0;
  servo[0].minStopLocation = 40;
  servo[0].maxStopLocation = 80;
  servo[0].stopValue = 20;

  servo[1].id = 1;
  servo[1].servoPin = 19;
  servo[1].analogPin = 2;
  servo[1].minWorkingLocation = 0;
  servo[1].maxWorkingLocation = 180;
  servo[1].minCoupleLocation = 0;
  servo[1].maxCoupleLocation = 180;
  servo[1].minStepSize = 20;
  servo[1].maxStepSize = 40;
  servo[1].startLocation = 0;
  servo[1].minStopLocation = 20;
  servo[1].maxStopLocation = 30;
  servo[1].stopValue = 20;

  servo[2].id = 2;
  servo[2].servoPin = 23;
  servo[2].analogPin = 12;
  servo[2].minWorkingLocation = 0;
  servo[2].maxWorkingLocation = 180;
  servo[2].minCoupleLocation = 100;
  servo[2].maxCoupleLocation = 180;
  servo[2].minStepSize = 4;
  servo[2].maxStepSize = 40;
  servo[2].startLocation = 0;
  servo[2].minStopLocation = 40;
  servo[2].maxStopLocation = 80;
  servo[2].stopValue = 20;

  servo[3].id = 3;
  servo[3].servoPin = 22;
  servo[3].analogPin = 13;
  servo[3].minWorkingLocation = 0;
  servo[3].maxWorkingLocation = 180;
  servo[3].minCoupleLocation = 0;
  servo[3].maxCoupleLocation = 180;
  servo[3].minStepSize = 20;
  servo[3].maxStepSize = 40;
  servo[3].startLocation = 0;
  servo[3].minStopLocation = 20;
  servo[3].maxStopLocation = 30;
  servo[3].stopValue = 20;
}

// location where you have to couple the servos to eachother
void define_couples(void)
{
  servo[0].couple = servo[1].id;
  servo[1].couple = servo[0].id;
  servo[2].couple = servo[3].id;
  servo[3].couple = servo[2].id;
}

//======================================SERVO CLASS======================================

class servo_functions
{
  private: // this is where all the variables are kept for the function, they can only be accessed via the class itself
    Servo _servo;
    int _thisServo;
    int _pin;
    int _coupledLocation;
    bool _active;
    unsigned long _time;
    int _loc;
    int _coupled;
    int _minCoupledLocation;
    int _maxCoupledLocation;
    int _minLocation;
    int _maxLocation;
    int _randomMinValue;
    int _randomMaxValue;
    bool _coupledActive;
    int _minStopLoc;
    int _maxStopLoc;
    int _choosenMove;
    int _currentPosition;
    int _stopValue;
    bool _blocked;
    int _blockadeTime;
    bool _coupledBlocked;

  public: // this is where the functions and variables are that can be accessed from outside the class
    servo_functions(){}
    void set_servo(Servo &s) {_servo = s;}
    void initialize(int pin,
                    int coupled,
                    int minCoupledLocation,
                    int maxCoupledLocation,
                    int ownMinLocation,
                    int ownMaxLocation,
                    int minStepSize,
                    int maxStepSize,
                    int minStopLoc,
                    int maxStopLoc,
                    int stopValue
                   );
    void set_coupled(servo_functions coupled[]);
    void start(int servoPin, int startPosition);
    void can_move(void);
    void random_time(void);
    void random_loc(void);
    void new_pos(void);
    int locations(void);
    void blocked(void);
    int get_location(void);
    int get_active(void);
    int get_blocked(void);
};

//====================================SERVO FUNCTIONS====================================

// place where the servo values will be put in the private section of the class
void servo_functions::initialize(int pin,
                                 int coupled,
                                 int minCoupledLocation,
                                 int maxCoupledLocation,
                                 int ownMinLocation,
                                 int ownMaxLocation,
                                 int minStepSize,
                                 int maxStepSize,
                                 int minStopLoc,
                                 int maxStopLoc,
                                 int stopValue
                                )
{
  _pin = pin;
  _coupled = coupled;
  _minCoupledLocation = minCoupledLocation;
  _maxCoupledLocation = maxCoupledLocation;
  _minLocation = ownMinLocation;
  _maxLocation = ownMaxLocation;
  _randomMinValue = minStepSize;
  _randomMaxValue = maxStepSize;
  _minStopLoc = minStopLoc;
  _maxStopLoc = maxStopLoc;
  _stopValue = stopValue;
}

void servo_functions::start(int servoPin, int startPosition)
{
  _thisServo = servoPin;
  _servo.attach(servoPin);
  pinMode(_pin, INPUT);
  _servo.write(startPosition);
  _currentPosition = startPosition;
  Serial.println("servo pin:" + String(servoPin) + "\t position:" + _currentPosition + "\t stopValue:" + _stopValue + "\t pin:" + _pin);
}

void servo_functions::set_coupled(servo_functions coupled[])
{
  _coupledLocation = coupled[_coupled].get_location();
  _coupledActive = coupled[_coupled].get_active();
  _coupledBlocked = coupled[_coupled].get_blocked();
  //Serial.println("servo pin:" + String(_thisServo) + "\t blocked couple:" + _coupledBlocked);
  can_move();
}

void servo_functions::can_move(void)
{
  if (_blocked == true)
  {
    blocked();
    return;
  }
  if (_coupledBlocked == true) {
    return;
  }
  if (_coupledActive == true) {
    return;
  }
  if ((_coupledLocation < _minCoupledLocation) || (_coupledLocation > _maxCoupledLocation)) {
    return;
  }
  if ((_coupledLocation >= _minStopLoc) && (_coupledLocation <= _maxStopLoc)) {
    return;
  }
  random_time();
}

void servo_functions::random_time(void)
{
  if (_active == true) {
    new_pos();
  }
  if (_time > currentMillis) {
    return;
  }
  _active = true;
  _time = random(1, 10) * 100 + currentMillis;
  char buffer[40];
  sprintf(buffer, "servo pin: %s \t time: %02i:%02i:%02i", String(_thisServo), _time / 1000 / 3600, _time / 1000 % 3600 / 60, _time / 1000 % 3600 % 60);
  Serial.println(buffer);
  random_loc();
}

void servo_functions::random_loc(void)
{
  int newLoc;
  do
  {
    newLoc = locations();
  } while ((_currentPosition - _choosenMove) < newLoc && newLoc < (_currentPosition + _choosenMove));

  _loc = newLoc;
  Serial.println("servo pin: " + String(_thisServo) + "\t new location: " + _loc);
}

int servo_functions::locations(void)
{
  int randVal = random(_randomMinValue, _randomMaxValue);
  _choosenMove = randVal;
  int randLoc = random(0, ((_maxLocation - _minLocation) / randVal)) * randVal + _minLocation;
  return randLoc;
}

void servo_functions::new_pos(void)
{
  //Serial.println("Pin: "+ String(_thisServo) + "\t value: " + analogRead(_pin));
  if (analogRead(_pin) >= _stopValue)
  {
    _blocked = true;
    return;
  }
  if (_loc == _currentPosition)
  {
    _active = false;
    return;
  }
  if (_loc > _currentPosition) {
    _servo.write(_currentPosition++);
  }
  if (_loc < _currentPosition) {
    _servo.write(_currentPosition--);
  }
}

void servo_functions::blocked(void)
{
  if (_blockadeTime > currentMillis) {
    return;
  }
  _blockadeTime = currentMillis + 60000;
  _blocked = false;
  Serial.println("servo pin: " + String(_thisServo) + "\t has an error");
}

int servo_functions::get_location(void)
{
  return _currentPosition;
}

int servo_functions::get_active(void)
{
  return _active;
}

int servo_functions::get_blocked(void)
{
  return _blocked;
}

//======================================SETUP CLASS======================================

servo_functions classes[amountServos];

void setup(void)
{
  Serial.begin(115200);
  define_servos();
  define_couples();
  startMillis = millis();
  
  for (int i = 0; i < (sizeof(servo) / sizeof(servo[0])); i++)
  {;classes[i].set_servo(servoNumber[i]);
    classes[i].initialize(servo[i].analogPin,
                           servo[i].couple,
                           servo[i].minCoupleLocation,
                           servo[i].maxCoupleLocation,
                           servo[i].minWorkingLocation,
                           servo[i].maxWorkingLocation,
                           servo[i].minStepSize,
                           servo[i].maxStepSize,
                           servo[i].minStopLocation,
                           servo[i].maxStopLocation,
                           servo[i].stopValue
                           );
    classes[i].start(servo[i].servoPin, servo[i].startLocation);
  }
  delay(5000);
}

//=======================================SERVO MOVE======================================

void loop(void)
{
  currentMillis = millis();
  for (int i = 0; i < (sizeof(servo) / sizeof(servo[0])); i++)
  {
    classes[i].set_coupled(classes);
  }
}