// https://forum.arduino.cc/t/hc-sr04-wont-continue-working-after-one-loop/1093489/55

/* Notesto self
   MoveToPosition(posX, 100) controls speed (20-200)
   M1=base degrees. Allowed values from 0 to 180 degrees
   M2=shoulder degrees. Allowed values from 15 to 165 degrees
   M3=elbow degrees. Allowed values from 0 to 180 degrees
   M4=wrist degrees. Allowed values from 0 to 180 degrees
   M5=wrist rotation degrees. Allowed values from 0 to 180 degrees
   M6=gripper degrees. Allowed values from 10 to 73 degrees. 10: fingers open, 73: fingers closed.
*/
// defines pins numbers
const int trigPin = 7;
const int echoPin = 8;
const int ledPin = 13;

// pre-defined motor pins
// base = 11;
// shoulder = 10;
// elbow = 9;
// wrist_rot = 5;
// wrist_ver = 6;
// gripper = 3;

// defines variables
long DUR;
int DIS;

//includes libraries needed
#include <BraccioRobot.h>
#include <Servo.h>
//
Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_rot;
Servo wrist_ver;
Servo gripper;

bool block1_flag = false;
bool block2_flag = false;
bool block3_flag = false;

//setup code to runs once, initialises the robot
void setup() {
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  pinMode(ledPin, OUTPUT); //sets led pin as an output
  Serial.begin(9600); // Starts the serial communication
  BraccioRobot.init();
}

void loop()
{
  // Clears the trigPin, 2 micro seconds
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  // Sets the trigPin on HIGH state, 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Reads the echoPin
  DUR = pulseIn(echoPin, HIGH);

  // Calculating the distance in cm
  DIS = DUR * 0.034 / 2;
  //Serial.print( "duration ");
  //Serial.print( DUR );

  Serial.print("distance: ");
  Serial.print(DIS);
  Serial.println(" cm");

  if (DIS <= 8) {
    digitalWrite(ledPin, HIGH);
    if (!block1_flag) {
      MoveBlock1();
      block1_flag = true;
    }
    else if (!block2_flag) {
      MoveBlock2();
      block2_flag = true;
    }
    else if (!block3_flag) {
      MoveBlock3();
      block3_flag = true;
    }
  } else {
    digitalWrite(ledPin, LOW);
    SafetyPos();
    // reset block flags if object is no longer detected
    block1_flag = false;
    block2_flag = false;
    block3_flag = false;
  }
  //Serial.print( "duration ");
  //Serial.print( DUR );

  Serial.print( " distance ");
  Serial.print( DIS );
  Serial.println();

}

void MoveBlock1() {
  Position Block1;
  //         (M1, M2, M3, M4, M5, M6)
  Block1.set(190, 120, 20, 10, 90, 20);

  //Block1
  BraccioRobot.moveToPosition(Block1, 50);
  BraccioRobot.moveToPosition(Block1.set(180, 84, 15, 3, 90, 20), 25);
  BraccioRobot.moveToPosition(Block1.set(180, 84, 15, 3, 90, 70), 25);
  BraccioRobot.moveToPosition(Block1.set(180, 90, 45, 45, 90, 70), 50);
  BraccioRobot.moveToPosition(Block1.set(90, 90, 45, 45, 90, 70), 50);
  BraccioRobot.moveToPosition(Block1.set(90, 85, 10, 10, 90, 70), 25);
  BraccioRobot.moveToPosition(Block1.set(90, 85, 10, 10, 90, 20), 50);
  BraccioRobot.moveToPosition(Block1.set(90, 90, 45, 45, 90, 20), 50);
}

void MoveBlock2() {
  Position Block2;
  Block2.set(190, 80, 10, 10, 5, 20);

  //Block2
  BraccioRobot.moveToPosition(Block2, 50);
  BraccioRobot.moveToPosition(Block2.set(180, 80, 10, 10, 5, 70), 25);
  BraccioRobot.moveToPosition(Block2.set(180, 90, 45, 45, 5, 70), 50);
  BraccioRobot.moveToPosition(Block2.set(90, 90, 45, 45, 90, 70), 50);
  BraccioRobot.moveToPosition(Block2.set(90, 85, 10, 10, 90, 70), 25);
  BraccioRobot.moveToPosition(Block2.set(90, 85, 10, 10, 90, 20), 50);
  BraccioRobot.moveToPosition(Block2.set(90, 90, 45, 45, 90, 20), 50);
}

void MoveBlock3() {
  Position Block3;
  Block3.set(170, 80, 45, 45, 178, 20);

  //Block3
  BraccioRobot.moveToPosition(Block3, 50);
  BraccioRobot.moveToPosition(Block3.set(170, 120, 10, 10, 178, 20), 25);
  BraccioRobot.moveToPosition(Block3.set(170, 80, 10, 10, 178, 20), 25);
  BraccioRobot.moveToPosition(Block3.set(170, 80, 10, 10, 178, 70), 50);
  BraccioRobot.moveToPosition(Block3.set(170, 90, 45, 45, 178, 70), 50);
  BraccioRobot.moveToPosition(Block3.set(90, 90, 45, 45, 90, 70), 50);
  BraccioRobot.moveToPosition(Block3.set(90, 85, 10, 10, 90, 70), 25);
  BraccioRobot.moveToPosition(Block3.set(90, 85, 10, 10, 90, 20), 50);
  BraccioRobot.moveToPosition(Block3.set(90, 90, 45, 45, 90, 20), 50);
}

void SafetyPos() {
  Position safetyPos;
  safetyPos.set(90, 90, 90, 90, 90, 20);
  BraccioRobot.moveToPosition(safetyPos, 25);


}