#include <Servo.h>
#include <math.h>
#include <ExcelDataReader.h>

///// All global variables /////

// Define pin of light sensor
const int lightSensorPin = A0; 

// Define lightSensorReading threshold
const int lightsen_thres=300;

// Define global variables for the servo pins
const int baseservo_pin = 9;
const int arm1servo_pin = 10;
const int arm2servo_pin = 11;


// Define global variables for the Servo objects
Servo baseservo;
Servo arm1servo;
Servo arm2servo;


// Define the arm length
const double arm_length = 105;


////Define pins for two buttons and two pins for the DC motor
//buttons
const int buttonPin1 = 2; // Button 1 connected to digital pin 2
const int buttonPin2 = 3; // Button 2 connected to digital pin 3
//dc motor pins
const int motorPin1 = 8; // DC motor connected to digital pin 8
const int motorPin2 = 12; // DC motor connected to digital pin 12


///set up for lists

// Define number of points first text1, number of rows imported from excel
const int num_list1=93;
// Define number of points first text2, number of rows imported from excel
const int num_list2=70;

//lists of x,y,z text1
double x_values_text1[num_list1];
double y_values_text1[num_list1];
double z_values_text1[num_list1];

//lists of x,y,z text2
double x_values_text2[num_list2];
double y_values_text2[num_list2];
double z_values_text2[num_list2];

// Define function to return to home position

void homePosition() {
    baseservo.write(90);
    arm1servo.write(90);
    arm2servo.write(90);
}

// Define inverse kinematics function

void inverseKinematics(double x, double y, double z) {

  double b = atan2(y,x) * (180 / 3.1415); // base angle
  double l = sqrt(x*x + y*y); 
  double h = sqrt (l*l + z*z);
  double phi = atan(z/l) * (180 / 3.1415);
  double theta = acos((h/2)/arm_length) * (180 / 3.1415);  
  double a1 = 188 - (phi + theta); // angle for first part of the arm
  double a2 = 101 + (phi - theta); // angle for second part of the arm

    // Move the servos to the calculated angles
    baseservo.write(b);
    arm1servo.write(a1);
    arm2servo.write(a2);

    delay(50);
}


void setup() {

  baseservo.attach(baseservo_pin);
  arm1servo.attach(arm1servo_pin);
  arm2servo.attach(arm2servo_pin);

  pinMode(buttonPin1, INPUT_PULLUP); // Set button 1 as input with internal pull-up resistor
  pinMode(buttonPin2, INPUT_PULLUP); // Set button 2 as input with internal pull-up resistor
  pinMode(motorPin1, OUTPUT); // Set motor pin 1 as output
  pinMode(motorPin2, OUTPUT); // Set motor pin 2 as output

  // text1, read each column from the Excel file for text1 and assing the values to the x_values_text1, y_values_text1 and z_values_text1
  ExcelDataReader excel("coordinates_1.xlsx");
  excel.readColumn(1, x_values_text1, num_list1);
  excel.readColumn(2, y_values_text1, num_list1);
  excel.readColumn(3, z_values_text1, num_list1);

  // text2, read each column from the Excel file for text2 and assing the values to the x_values_text2, y_values_text2 and z_values_text2
  ExcelDataReader excel2("coordinates_2.xlsx");
  excel2.readColumn(1, x_values_text2, num_list2);
  excel2.readColumn(2, y_values_text2, num_list2);
  excel2.readColumn(3, z_values_text2, num_list2);


// step 1: robot is at home position
  homePosition();

  // DC motor direction is defined
  // Rotate the motor in one direction
  digitalWrite(motorPin1, HIGH);
  digitalWrite(motorPin2, LOW);

}


void loop() {

  int lightSensorReading = 0;

  lightSensorReading = analogRead(lightSensorPin); //reads the value from the sensor
    //Serial.print(lightSensorReading);
    
  if(lightSensorReading > lightsen_thres){ //when there is light

    for (int i = 0; i < sizeof(x_values_text1) / sizeof(x_values_text1[0]); i++) {
      inverseKinematics(x_values_text1[i], y_values_text1[i], z_values_text1[i]);
  }

    //gets the robot to home position after completing the writing
    homePosition();

    /////////////START ERASER///////////////

    if (digitalRead(buttonPin2) == LOW) {
    // Reverse the direction of the motor
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, HIGH);
    
    // Wait for the motor to hit button 1
    while (digitalRead(buttonPin1) == HIGH) {
      delay(10);
    }
    
    // Reverse the direction of the motor
    digitalWrite(motorPin1, HIGH);
    digitalWrite(motorPin2, LOW);
    
    // Wait for the motor to hit button 2
    while (digitalRead(buttonPin2) == HIGH) {
      delay(10);
    }
    
    // Reverse the direction of the motor
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, HIGH);
    
    // Wait for 1 second
    delay(500);
    
    // Stop the motor
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
  }
}
/////////////// STOP OF ERASER////////////////


  else{
    //Gets the robot to home position when there is no light.
    homePosition();
    //Robot starts writing text2
    for (int i = 0; i < sizeof(x_values_text2) / sizeof(x_values_text2[0]); i++) {
        inverseKinematics(x_values_text2[i], y_values_text2[i], z_values_text2[i]);
  }
    //gets the robot to home position after completing the writing of text2
    homePosition();

    /////////////START ERASER///////////////

    if (digitalRead(buttonPin2) == LOW) {
    // Reverse the direction of the motor
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, HIGH);
    
    // Wait for the motor to hit button 1
    while (digitalRead(buttonPin1) == HIGH) {
      delay(10);
    }
    
    // Reverse the direction of the motor
    digitalWrite(motorPin1, HIGH);
    digitalWrite(motorPin2, LOW);
    
    // Wait for the motor to hit button 2
    while (digitalRead(buttonPin2) == HIGH) {
      delay(10);
    }
    
    // Reverse the direction of the motor
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, HIGH);
    
    // Wait for 1 second
    delay(500);
    
    // Stop the motor
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
  }
}
/////////////// STOP OF ERASER////////////////

}