/*
  robot.ino
  https://www.youtube.com/watch?v=-T0ylQ7CmUU
  https://forum.arduino.cc/t/sunfounder-sloth-wont-calibrate-and-then-falls-over/1088654/
  https://github.com/sunfounder/SunFounder_Crawling_Quadruped_Robot_Kit_for_Arduino/tree/master/CD
  
  - Sunfounder Sloth robot
  - plug the robot in
  - upload calibration code
  - turn it on
  
** feet instantly rotate in one direction and it falls over
** legs work fine if at 0
** no changes to the code or the angle of rotation have fixed this
** won't recognize ultrasonic sensor
**  TOP LEFT servo does nothing
**  TOP RIGHT servo does nothing

*/

// Remove comments where needed.
// #define INSTALL      // All servos seek 0
// #define CALIBRATION  // All servos seek 90
#define RUN          // All servos seek 90. Adjusting sonar makes feet move (see Serial ouput)

// original VarSpeedServo.h
// #include "VarSpeedServo.h"  //include the VarSpeedServo library
#include <NewPing.h>           //include the NewPing library
NewPing sonar(4, 3, 200); // trigger pin, echo pin, max distance

// re-write for Servo.h
#include <Servo.h>
Servo RU;
Servo RL;
Servo LU;
Servo LL;

// original VarSpeedServo.h
// VarSpeedServo RU;  //Right Upper
// VarSpeedServo RL;  //Right Lower
// VarSpeedServo LU;  //Left Upper
// VarSpeedServo LL;  //Left Lower

//vel(min), delay_Forward(max) = (5, 2000)
const int vel = 20, vel_Back = 10;               //vel(mid), delay_Forward(mid) = (20, 750)
const int delay_Forward = 750, delay_Back = 1000;//vel(max), delay_Forward(min)= (256, 50)
//wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500)
const int array_cal[4] = {0, 0, 0, 0};
int RU_Degree = 0, LU_Degree = array_cal[2] + 5;

const int num1 = 6;
const int array_forward[num1][4] =
{
  {0, -40, 0, -20},
  {30, -40, 30, -20},
  {30, 0, 30, 0},
  {0, 20, 0, 40},
  {-30, 20, -30, 40},
  {-30, 0, -30, 0},
};

const int num2 = 5;
const int array_turn[num2][4] =
{
  {-40, 0, -20, 0},
  {-40, 30, -20, 30},
  {0, 30, 0, 30},
  {30, 0, 30, 0},
  {0, 0, 0, 0},
};

// #define INSTALL
// #define CALIBRATION
// #define RUN

void Servo_Init()
{
  RU.attach(9);   // Connect the signal wire of the upper-right servo to pin 9
  RL.attach(10);   // Connect the signal wire of the lower-right servo to pin 10
  LU.attach(11);   // Connect the signal wire of the upper-left  servo to pin 11
  LL.attach(12);   // Connect the signal wire of the lower-left  servo to pin 12
}

void Adjust()        // Avoid the servo's fast spinning in initialization
{ // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees
  for (RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) {
    RU.write(RU_Degree);         // in steps of 1 degree
    LU.write(LU_Degree--);       // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree'
    delay(15);   // waits 15ms for the servo to reach the RU_Degreeition
  }
}

bool TooClose()
{
  int tooclose = 0;
  for (int a = 0; a < 5; a++) {
    delay(50);
    int din = sonar.ping_in();
    if (din < 7 && din > 0) {
      tooclose++;
      Serial.println("TooClose");
    }
  }
  if (tooclose < 5)
    return 1;
  return 0;
}

void Forward()
{
  Serial.println("Forward");
  for (int x = 0; x < num1; x++) {

    // re-write for Servo.h
    RU.write (array_cal[0] + array_forward[x][0]);
    RL.write (array_cal[1] + array_forward[x][1]);
    LU.write (array_cal[2] + array_forward[x][2]);
    LL.write (array_cal[3] + array_forward[x][3]);

    // original VarSpeedServo.h
    // RU.slowmove (array_cal[0] + array_forward[x][0] , vel);
    // RL.slowmove (array_cal[1] + array_forward[x][1] , vel);
    // LU.slowmove (array_cal[2] + array_forward[x][2] , vel);
    // LL.slowmove (array_cal[3] + array_forward[x][3] , vel);
    delay(delay_Forward);
  }
}

void Backward()
{
  Serial.println("Backward");
  for (int z = 0; z < 4; z++) {
    for (int y = 0; y < num2; y++) {

      // re-write for Servo.h
      RU.write (array_cal[0] + array_turn[y][0]);
      RL.write (array_cal[1] + array_turn[y][1]);
      LU.write (array_cal[2] + array_turn[y][2]);
      LL.write (array_cal[3] + array_turn[y][3]);

      // original VarSpeedServo.h
      // RU.slowmove (array_cal[0] + array_turn[y][0] , vel_Back);
      // RL.slowmove (array_cal[1] + array_turn[y][1] , vel_Back);
      // LU.slowmove (array_cal[2] + array_turn[y][2] , vel_Back);
      // LL.slowmove (array_cal[3] + array_turn[y][3] , vel_Back);
      delay(delay_Back);
    }
  }
}

void setup()
{
  Serial.begin(115200);
  Serial.println("setup");
#ifdef INSTALL
  Servo_Init();

  // re-write for Servo.h
  RU.write (90);
  RL.write (90);
  LU.write (90);
  LL.write (90);

  // original VarSpeedServo.h
  // RU.slowmove (90 , vel);
  // RL.slowmove (90 , vel);
  // LU.slowmove (90 , vel);
  // LL.slowmove (90 , vel);

  while (1);
#endif

#ifdef CALIBRATION
  Servo_Init();
  Adjust();

  // re-write for Servo.h
  RL.write (array_cal[1]);
  LL.write (array_cal[3]);

  // original VarSpeedServo.h
  // RL.slowmove (array_cal[1] , vel);
  // LL.slowmove (array_cal[3] , vel);

  delay(2000);
  while (1);
#endif

#ifdef RUN
  Servo_Init();
  Adjust();

  // re-write for Servo.h
  RL.write (array_cal[1]);
  LL.write (array_cal[3]);

  // original VarSpeedServo.h
  // RL.slowmove (array_cal[1] , vel);
  // LL.slowmove (array_cal[3] , vel);
  delay(2000);
#endif
}

void loop()
{
  while (TooClose())
    Forward();
  Backward();
}