// INCLUDE LIBRARIES
//#include <Wire.h>
//#include <Adafruit_PWMServoDriver.h>
#include <Servo.h>

int pot_val[][5] = { {0, 255, 255, -255, 0},
                            {0, 0, 0, 0, 0},
                            {0, -255, -255, 255, 0},
                            {0, 0, 0, 0, 0} };
int trim_pot_val[4] = {0, 255, 0, -255};

int rrow = 0;   

uint8_t Status = 0; // 1 bit per servo or motor 0 = finished 

// Servo definitions
#define numservo 5

#define servo1_Pin A0
#define servo2_Pin A1
#define servo3_Pin A2
#define servo4_Pin A3
#define servo5_Pin A4

#define servo1 0
#define servo2 1
#define servo3 2
#define servo4 3
#define servo5 4


// Servo parameters
#define trimFaktor 10

int servo_Pins[numservo] = {servo1_Pin, servo2_Pin, servo3_Pin, servo4_Pin};
                    
const int SERVOMIN = 100;  // This is the 'minimum' pulse length count (out of 4096) in msec
const int SERVOMAX = 480;  // This is the 'maximum' pulse length count (out of 4096) in msec

int anglhome[numservo] ={90, 90, 90, 90, 90}; //degrees  
int home[numservo];

int MinValue[numservo] = {100, 180, 100, 180, 100}; // msec-values!
int MaxValue[numservo] = {480, 400, 480, 400, 480}; // msec-values!

int StepSize[numservo] = {5, 5, 5, 5, 5}; // msec-values!

int DelayTime[][numservo] = { {5, 5, 5, 5, 5},      // fast
                              {15, 15, 15, 15, 15} }; // slow


// Servo variables
uint8_t mapped_servos = 4;

uint8_t servo_speed_val = 1;

int y_leftTrim;

int oldAngle[numservo];
int newAngle[numservo];

int CurrentPosition[numservo]; // msec-values!

unsigned long previousMillis[numservo];



//SET PCA9685 BOARD ADDRESS
//setup the board address - defaults to 0x40 if not specified
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); 

Servo myservo[numservo];

// -----------------------

int newmap(long x, long in_min, long in_max, long out_min, long out_max)

{
	if (x == in_max)
		return out_max;
	else if (out_min < out_max)
		return (x - in_min) * (out_max - out_min + 1) / (in_max - in_min) + out_min;
	else
		return (x - in_min) * (out_max - out_min - 1) / (in_max - in_min) + out_min;
}


// -----------------------

void findHome(uint8_t servonum) // set home-position in msec

{
  home[servonum] = newmap(anglhome[servonum], 0, 180, SERVOMIN, SERVOMAX);
  oldAngle[servonum] = home[servonum];
  CurrentPosition[servonum] = oldAngle[servonum];
  return;
}


// -----------------------

void convert2Angle(int row) // Servotargets setzen

{
//  Serial.println();
	for (uint8_t i = 0; i < mapped_servos; i = i + 1)
	{
    if (i == 0)
    {
      y_leftTrim = trim_pot_val[row] / trimFaktor;
    }
    else
    {
      y_leftTrim = 0;      
    }

		if (pot_val[row][i] < 0) // dreht nach links - Joystick links oder unten
		{
			newAngle[i] = newmap(abs(pot_val[row][i]), 1, 255, home[i]+ y_leftTrim+1, MaxValue[i]);
		}
		else if (pot_val[i] > 0) // dreht nach rechts - Joystick rechts oder oben
		{
			newAngle[i] = newmap(pot_val[row][i], 1, 255, home[i]+ y_leftTrim-1, MinValue[i]);
		}
		else
		{
			newAngle[i] = home[i];
		}
    bitSet(Status, i);
    previousMillis[i] = 0;
	}
//  Serial.println();
//  Serial.print("Row ");
//  Serial.print(row);
//  Serial.println(" digested");
	return;
}


// -----------------------

void moveServo(uint8_t servoNum)
{
  if (newAngle[servoNum] == oldAngle[servoNum] || bitRead(Status, servoNum) == 0)
  {
    bitClear(Status, servoNum); // just to be sure
//    Serial.print("Servo ");
//    Serial.print(servoNum);
//    Serial.println(" no move planned");
    return;
  }

  unsigned long currentMillis = millis();

  if ((currentMillis - previousMillis[servoNum]) >= DelayTime[servo_speed_val][servoNum])
  {

//    if (servoNum == 1)
//    {
//    Serial.println();
//    Serial.print("Servo ");
//    Serial.println(servoNum);
//    Serial.print("Old: ");
//    Serial.print(oldAngle[servoNum]);
//    Serial.print(" new: ");
//    Serial.print(newAngle[servoNum]);
//    Serial.print(" CurPos: ");
//    Serial.print(CurrentPosition[servoNum]);
//    Serial.print(" Step: ");
//    Serial.print(StepSize[servoNum]);
//    }

    previousMillis[servoNum] = currentMillis;

    if (CurrentPosition[servoNum] < newAngle[servoNum])
    {
      CurrentPosition[servoNum] = CurrentPosition[servoNum] + StepSize[servoNum];
      if (CurrentPosition[servoNum] > newAngle[servoNum])
      {
        CurrentPosition[servoNum] = newAngle[servoNum];
      }
//      Serial.print(" newCur: ");
//      Serial.println(CurrentPosition[servoNum]);
//        pwm.writeMicroseconds(servoNum, CurrentPosition[servoNum]);
      myservo[servoNum].write(newmap(CurrentPosition[servoNum], SERVOMIN, SERVOMAX, 0, 180));
    }
    else if (CurrentPosition[servoNum] > newAngle[servoNum])
    {
      CurrentPosition[servoNum] = CurrentPosition[servoNum] - StepSize[servoNum];
      if (CurrentPosition[servoNum] < newAngle[servoNum])
      {
        CurrentPosition[servoNum] = newAngle[servoNum];
      }
//      Serial.print(" newCur: ");
//      Serial.println(CurrentPosition[servoNum]);
//        pwm.writeMicroseconds(servoNum, CurrentPosition[servoNum]);
      myservo[servoNum].write(newmap(CurrentPosition[servoNum], SERVOMIN, SERVOMAX, 0, 180));
    }
    else
    {
      oldAngle[servoNum] = newAngle[servoNum];
      CurrentPosition[servoNum] = oldAngle[servoNum];
//      Serial.print(" finCur: ");
//      Serial.println(CurrentPosition[servoNum]);
      bitClear(Status, servoNum);
//      Serial.print("Servo ");
//      Serial.print(servoNum);
//      Serial.println(" done");
      return;
    }   
  }
}

void setup()
{
//  Serial.begin(115200);

  for (uint8_t i = 0; i < numservo; i++)
  {
    myservo[i].attach(servo_Pins[i]);
    findHome(i);
  }

/*
  pwm.begin();
  pwm.setPWMFreq(50);

  for (int i = 0; i < 4; i=i+1)
  {
    findHome(i);
    pwm.writeMicroseconds(servoNum, CurrentPosition[servoNum]);
  }

*/
}


void loop()
{
  Status = 0;

  if (rrow == 0 || rrow == 2)
  { 
    servo_speed_val = 0;
  }
  else
  {
    servo_speed_val = 1;
  }

  convert2Angle(rrow);

  do 
  {
//    Serial.println();
//    Serial.print("Status: ");
//    Serial.println(Status);
  
    moveServo(servo1);
    moveServo(servo2);
    moveServo(servo3);
    moveServo(servo4);
    moveServo(servo5);

  }
  while (Status > 0);

  rrow = rrow + 1;
  if (rrow == 4)
  {
    rrow = 0;
  }
}