/* Includes ------------------------------------------------------------------*/
#include <Servo.h>    //to define and control servos

/* Definitions --------------------------------------------------------------------*/
const long SerialCommSpeed      = 115200;     // Defining Serial Port Communication Speed
const int legCount              = 4;          // Number of Legs
const int nodeCount             = 3;          // Number of Nodes per Leg
const int ServoStartAngle       = 90;         // Default Position of the Servos on Start
const int ServoUpdateFrameRate  = 20;         // Default Servo Update Frame Rate in Milliseconds

// Instantiating Servo Library
Servo servo[legCount][nodeCount];

// Defining initial position as 0
int servoCurrPosition[legCount][nodeCount] = {
 {0, 0, 0},
 {0, 0, 0},
 {0, 0, 0},
 {0, 0, 0}
};

// Assigning GPIO Servo Ports
const int servo_pin[legCount][nodeCount] = {
 {13, 12, 11},    // Back Right Leg
 {10, 9, 8},      // Back Left Leg
 {7, 6, 5},       // Front Right Leg
 {4, 3, 2}        // Front Left Leg
};

// Misc Definitions
int servoMaxValue = 180;
int servoMinValue = 0;

void setup() {
  // Starting Serial Ports
  Serial.begin(SerialCommSpeed);
  Serial.println("[OTTO]: Serial started at " + String(SerialCommSpeed) + "kbps");

  //initialize servos
  servo_attach();
  Serial.println("[OTTO]: Initialization Complete");
}


/* ---------------------------------------------------------------------------
  - Attach the servo library ( methods /etc ) to the Servo Array
---------------------------------------------------------------------------*/
void servo_attach(void) {
  for (int i = 0; i < legCount; i++) {
    for (int j = 0; j < nodeCount; j++) {
      // Attach the Servos
      servo[i][j].attach(servo_pin[i][j]);
      Serial.println("[OTTO]: Leg: " + String(i) + " - Servo: " + String(j) + " - Successfuly Attached!");

      // Move the Servos to Default Start Angle
      servo[i][j].write(ServoStartAngle);
      Serial.println("[OTTO]: Leg: " + String(i) + " - Servo: " + String(j) + " - Set to: " + String(ServoStartAngle));

      // Updating the Current Servo Positions
      servoCurrPosition[i][j] = ServoStartAngle;
    }
  }
}

/*---------------------------------------------------------------------------
  - loop function
---------------------------------------------------------------------------*/
void loop() {
  Serial.println("---------------------------------------------------------");
  Serial.println(" MAIN LOOP STARTED ");
  Serial.println("---------------------------------------------------------");

  int newServoPos[4][3] = {
    { 130, 180, 90 },
    { 90, 90, 90 },
    { 90, 90, 90 },
    { 90, 90, 90 }
  };

  updateServoPos( newServoPos, 1000 );
  Serial.println("---------------------------------------------------------");
  Serial.println(" MAIN LOOP FINISHED... DELAYING ");
  Serial.println("---------------------------------------------------------");
  delay(10000);
  Serial.println(" ");
}


void debugMap (int mapArray[], int arraySize) {
  int total = 0;

  Serial.println("---------------------------------------------------------");
  Serial.println(" Map Debbuger ");
  Serial.println("---------------------------------------------------------");
  for (int x=0; x < arraySize; x += 1) {
    Serial.print(" At position ");
    Serial.print(x);
    Serial.print(" value is ");
    Serial.println(mapArray[x]);
    total += (mapArray[x]);
  }

  Serial.println("---------------------------------------------------------");
  Serial.print("TOTAL IS ");
  Serial.println(total);
  Serial.println("---------------------------------------------------------");
}

/* ---------------------------------------------------------------------------
 - generateServoLoopValues - This function will calculate how much the servo
 has to move per loop iteration
 --------------------------------------------------------------------------- */
int generateServoMovementMap (int totalDistanceToMove, int loopTimes, int currLoopIteration ) {

  float accumulated = 0;
  float movementPerLoop = ((float)totalDistanceToMove / (float)loopTimes);
  int movementMap[loopTimes];

  for (int loopNo = 0; loopNo < loopTimes; loopNo += 1) {

    // Check if the movementPerLoop is bigger than 1
    if (movementPerLoop > 1) {
        // Converting the float movementPerLoop into an integer since
        // the servo cannot move in float angles.
        int realDistanceToMove  = (int)movementPerLoop;

        // Check if there's any left over to be saved
        float leftOver = (movementPerLoop - (realDistanceToMove));

        // If there's any left over, we should accumulate it
        if (leftOver != 0) {
          accumulated += leftOver;
        }

        movementMap[loopNo] = realDistanceToMove;

    // Check if the movementPerLoop is smaller than 1
    } else if (movementPerLoop < -1) {
        // Converting the float movementPerLoop into an integer since
        // the servo cannot move in float angles.
        int realDistanceToMove  = (int)movementPerLoop;

        // Check if there's any left over to be saved
        float leftOver = (movementPerLoop - (realDistanceToMove));

        // If there's any left over, we should accumulate it
        if (leftOver != 0) {
          accumulated += leftOver;
        }

        movementMap[loopNo] = realDistanceToMove;

    // movementPerLoop is to small to move, we should simple store
    } else {
      accumulated += movementPerLoop;
      movementMap[loopNo] = 0;
    }

    // Check if there is any value bigger than 1 in accumulated
    if (round(accumulated*10)/10 >= 1) {
      accumulated -= 1;
      movementMap[loopNo] += 1;

    // Check if there is any value smaller than -1 in accumulated
    } else if (round(accumulated*10)/10 <= -1) {
      accumulated += 1;
      movementMap[loopNo] += -1;
    }

  }

//  debugMap(movementMap, loopTimes);
  return movementMap[currLoopIteration];
}

/* ---------------------------------------------------------------------------
 - UpdateServoPos
 --------------------------------------------------------------------------- */
void updateServoPos( int servoDesiredPosition[legCount][nodeCount], int executionTime ) {

  // Calculating how many loops will happen
  const int loopTimes = abs( executionTime / ServoUpdateFrameRate );

  // Executing servo movements
  for (int loopNo = 0; loopNo < loopTimes; loopNo += 1) {

    for (int leg = 0; leg < legCount; leg += 1) {
      for (int node = 0; node < nodeCount; node += 1) {

        if (servoDesiredPosition[leg][node] != servoCurrPosition[leg][node]) {
          // Servo needs to move, let's calculate how much to move
          int totalDistanceToMove = servoDesiredPosition[leg][node] - servoCurrPosition[leg][node];

          // Ensuring that the totalDistanceToMove doesn't exceed servoMaxValue
          if ((servoCurrPosition[leg][node] + totalDistanceToMove) > servoMaxValue) {
            totalDistanceToMove = (servoMaxValue - servoCurrPosition[leg][node]);

          // Ensuring that the totalDistanceTomove doesn't exceed servoMinValue
          } else if ((servoCurrPosition[leg][node] + totalDistanceToMove) < servoMinValue) {
            totalDistanceToMove = -(servoMinValue + servoCurrPosition[leg][node]);
          }

          // Getting the amount of the servo should move on this loop iteration
          int movement = generateServoMovementMap( totalDistanceToMove, loopTimes, loopNo );

          // Checking if it's necessary to move the servo
          if (movement != 0) {
            Serial.print("Moving servo [");
            Serial.print(leg);
            Serial.print("][");
            Serial.print(node);
            Serial.print("] from ");
            Serial.print(servoCurrPosition[leg][node]);
            Serial.print(" to ");
            Serial.println(movement);
          }
        }

      }
    }

  }

  return;
}