#include <AccelStepper.h>

// robot geometry constants
const float e = 30; // side of end effector triangle
const float f = 240; // side of fixed triangle
const float re = 324; // length of parallelogram joint
const float rf = 182.0; // length of upper joint
const float baseZ = 170.0; // base height

// stepper motor pin assignments
const int MotorStep[] = {3, 5, 7};
const int MotorDir[] = {2, 4, 6};

// stepper motor speed and acceleration
const float maxSpeed = 2000.0; // steps per second
const float acceleration = 2000.0; // steps per second per second

// create AccelStepper objects for each motor
AccelStepper stepper1(AccelStepper::DRIVER, MotorStep[0], MotorDir[0]);
AccelStepper stepper2(AccelStepper::DRIVER, MotorStep[1], MotorDir[1]);
AccelStepper stepper3(AccelStepper::DRIVER, MotorStep[2], MotorDir[2]);

//Button pins
  int button1 = 22;
  int button2 = 23;
  int button3 = 24;
  int button4 = 25;
  int button5 = 26;
  int button6 = 27;
  int button7 = 28;
  int statebutton1 = 48;
  int statebutton2 = 49;
  int statebutton3 = 50;

  //Setup af states
  int buttonstate1;
  int buttonstate2;
  int buttonstate3; 
   
  //Setup til state 
  int state; 
  int stepState = LOW;

  //Setup af home position
  const int home_switch1 = 9;
  const int home_switch2 = 10;
  const int home_switch3 = 11;
  const float home_speed = 10;
  const float home_acceleration = 10;
  int initial_homing = 1;

void setup() {
  // set motor speeds and acceleration
  stepper1.setMaxSpeed(home_speed);
  stepper2.setMaxSpeed(home_speed);
  stepper3.setMaxSpeed(home_speed);
  stepper1.setAcceleration(home_acceleration);
  stepper2.setAcceleration(home_acceleration);
  stepper3.setAcceleration(home_acceleration);

  // set initial motor positions
  //stepper1.setCurrentPosition(0);
  //stepper2.setCurrentPosition(0);
  //stepper3.setCurrentPosition(0);
  pinMode(button1, INPUT_PULLUP);
  pinMode(button2, INPUT_PULLUP);
  pinMode(button3, INPUT_PULLUP);
  pinMode(button4, INPUT_PULLUP);
  pinMode(button5, INPUT_PULLUP);
  pinMode(button6, INPUT_PULLUP);
  pinMode(button7, INPUT_PULLUP);
  pinMode(statebutton1, INPUT_PULLUP);
  pinMode(statebutton2, INPUT_PULLUP);
  pinMode(statebutton3, INPUT_PULLUP);
  pinMode(home_switch1, INPUT_PULLUP);
  pinMode(home_switch2, INPUT_PULLUP);
  pinMode(home_switch3, INPUT_PULLUP);
  Serial.begin(9600);
  
  //Homing position to limswitches
  Serial.print("Stepper is in homing position...\n");
  while(digitalRead(home_switch1) == LOW || digitalRead(home_switch2)== LOW || digitalRead(home_switch3)== LOW) //Moves stepper motors to home
  {
    if(digitalRead(home_switch1)==LOW)
    {
      stepper1.moveTo(initial_homing);
      initial_homing--;
      stepper1.run();
    }
    if(digitalRead(home_switch2)==LOW)
    {
      stepper2.moveTo(initial_homing);
      initial_homing--;
      stepper2.run();
    }
    if(digitalRead(home_switch3)==LOW)
    {
      stepper3.moveTo(initial_homing);
      initial_homing--;
      stepper3.run();
    }
  }

  //Make steps from limswitches until not HIGH
  while(digitalRead(home_switch1)==HIGH && digitalRead(home_switch2)==HIGH && digitalRead(home_switch3)==HIGH) //Moves stepper motors to home
  {
    stepper1.moveTo(abs(initial_homing++));
    stepper2.moveTo(abs(initial_homing++));
    stepper3.moveTo(abs(initial_homing++));
    stepper1.run();
    stepper2.run();
    stepper3.run();
    delay(100);
  }  

  stepper1.setCurrentPosition(0);
  stepper2.setCurrentPosition(0);
  stepper3.setCurrentPosition(0);
  Serial.print("Homing complete \n");
  delay(1000);

  stepper1.setMaxSpeed(maxSpeed);
  stepper2.setMaxSpeed(maxSpeed);
  stepper3.setMaxSpeed(maxSpeed);
  stepper1.setAcceleration(acceleration);
  stepper2.setAcceleration(acceleration);
  stepper3.setAcceleration(acceleration);

}



void loop() 
{
  
//Anvendelse af switch case
  int button1 = digitalRead(button1);
  int button2 = digitalRead(button2);
  int button3 = digitalRead(button3);
  int button4 = digitalRead(button4);
  int button5 = digitalRead(button5);
  int button6 = digitalRead(button6);
  int stateShifter = digitalRead(button7);
  
  buttonstate1 = digitalRead(statebutton1);
  buttonstate2 = digitalRead(statebutton2);
  buttonstate3 = digitalRead(statebutton3);
 

	switch(state)
  {
      case 0:
      Serial.write("Idle mode");
      Serial.write("\n");
      if(buttonstate1==LOW)
        {
          state = 1;
          delay(1000);
        }
      else if (buttonstate2==LOW)
      {
        state=2;
        delay(1000);
      }
      else if (buttonstate3==LOW)
      {
        state=3;
        delay(1000);
      }
      break;

      case 1:
      Serial.write("Case 1");
      Serial.write("\n");
      
      //-----Styring af motorer-----//
      //-----Motor(speed,acceleration)-----//
      Motor1(100,100);
      Motor2(100,1);
      Motor3(5000000,10000000);
      
      if(buttonstate1==LOW) 
      	{
			    state = 1;
        	delay(1000);
        } 
      else if(buttonstate2==LOW)
        {
          state = 2;
          delay(1000);
        }
      else if(buttonstate3==LOW)
        {
        state = 3;
        delay(1000);
        }
      break;

  	  case 2:
  		//Styring af counter og clockwise
      	Serial.write("Du er i stage 2.");
      	Serial.write("\n");
         // find tool position when the arms are parallel to the floor
         // set the desired coordinates
        float x = 0; // Desired x-coordinate in mm
        float y = f - e; // Desired y-coordinate in mm
        float z = (re/2) + baseZ; // Desired z-coordinate in mm
        GoToCartesian(x, y, z);
        delay(1000);

        // find the middle of the envelope relative to the base (0,0,0)
        x = 0;
        y = 0;
        z = 0;
        GoToCartesian(x, y, z);
        GoToCartesian(270,0,0);
        Serial.write("Heloooo");
        GoToCartesian(-270,0,0);
  		  break;

        case 3:
        Serial.print("Case 3");
        break;
  }
}


void GoToCartesian(float x, float y, float z) 
{
  // calculate intermediate values
  float r1 = sqrt(pow(x,2)+pow(y,2))+e;
  float z1 = z-baseZ;
  float r2 = sqrt(pow(r1,2)+pow(z1,2));
  float theta1 = atan2(y,x);
  float theta2 = atan2(z1,r1);
  float theta3 = atan2(sqrt(pow(re,2)+pow(rf,2)-2*re*rf*cos(theta2)),rf*sin(theta2));
  float phi1 = atan2(z1,r1);
  float phi2 = atan2(re*sin(theta2),rf-re*cos(theta2));
  float phi3 = atan2(sqrt(pow(re,2)+pow(rf,2)-2*re*rf*cos(theta2)),-re*cos(theta2)+rf);

  // convert angles to steps
  float steps1 = theta1 * 200.0 / PI;
  float steps2 = (phi1 + phi2 - PI/2.0) * 200.0 / PI;
  float steps3 = (phi3 - phi2 + PI/2.0 - theta3) * 200.0 / PI;

  // calculate distances for each motor
  float distance1 = abs(steps1 - stepper1.currentPosition());
  float distance2 = abs(steps2 - stepper2.currentPosition());
  float distance3 = abs(steps3 - stepper3.currentPosition());

  // calculate maximum distance and adjust motor speeds accordingly
  float maxDistance = max(max(distance1, distance2), distance3);
  float speed1 = (maxDistance-distance1) / distance1 * maxSpeed;
  float speed2 = (maxDistance-distance2) / distance2 * maxSpeed;
  float speed3 = (maxDistance-distance3) / distance3 * maxSpeed;

  // set acceleration and deceleration
  stepper1.setAcceleration(acceleration);
  stepper2.setAcceleration(acceleration);
  stepper3.setAcceleration(acceleration);
  stepper1.setSpeed(speed1);
  stepper2.setSpeed(speed2);
  stepper3.setSpeed(speed3);

  // move motors to calculated positions
  stepper1.moveTo(steps1);
  stepper2.moveTo(steps2);
  stepper3.moveTo(steps3);

  // wait for motors to finish moving
  while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0 || stepper3.distanceToGo() != 0) {
    stepper1.run();
    stepper2.run();
    stepper3.run();
  }

  // set deceleration to zero
  stepper1.setAcceleration(1000);
  stepper2.setAcceleration(1000);
  stepper3.setAcceleration(1000);

  // delay to ensure all motors have finished moving
  delay(1000);
}



void printPosition() 
{
  float steps1 = stepper1.currentPosition();
  float steps2 = stepper2.currentPosition();
  float steps3 = stepper3.currentPosition();
  float theta1 = steps1 * PI / 200.0;
  float phi1 = steps2 * PI / 200.0 - PI/2.0;
  float phi2 = steps3 * PI / 200.0 + phi1 - PI/2.0;
  float r1 = sqrt(pow(re,2)+pow(rf,2)-2*re*rf*cos(phi2));
  float z1 = sin(phi2)*rf;
  float z = z1+baseZ;
  float r2 = r1-e;
  float x = r2*cos(theta1);
  float y = r2*sin(theta1);

  Serial.print("Current Position: (x=");
  Serial.print(x);
  Serial.print(", y=");
  Serial.print(y);
  Serial.print(", z=");
  Serial.print(z);
  Serial.println(")");
}





void Motor1(int setSpeed, int setAcceleration)
{
   int button1 = digitalRead(22);
   int button2 = digitalRead(23);
   if (button1 == LOW)
  {
    // Change the direction of the motor
    stepper1.setSpeed(setSpeed);
    stepper1.setAcceleration(setAcceleration);
    stepper1.runSpeed();
  }

  // Stop the motor when button2 is pressed
  if (button2 == LOW)
  {

    stepper1.setSpeed(-setSpeed);
    stepper1.setAcceleration(setAcceleration);
    stepper1.runSpeed();
  }
}

void Motor2(int setSpeed, int setAcceleration)
{
   int button3 = digitalRead(24);
   int button4 = digitalRead(25);
   if (button3 == LOW)
  {
    // Change the direction of the motor
    stepper2.setSpeed(setSpeed);
    stepper2.setAcceleration(setAcceleration);
    stepper2.runSpeed();
  }

  // Stop the motor when button2 is pressed
  if (button4 == LOW)
  {

    stepper2.setSpeed(-setSpeed);
    stepper2.setAcceleration(setAcceleration);
    stepper2.runSpeed();
  }
}

void Motor3(int setSpeed, int setAcceleration)
{
   int button5 = digitalRead(26);
   int button6 = digitalRead(27);
   if (button5 == LOW)
  {
    // Change the direction of the motor
    stepper3.setSpeed(setSpeed);
    stepper3.setAcceleration(setAcceleration);
    stepper3.runSpeed();
  }

  // Stop the motor when button2 is pressed
  if (button6 == LOW)
  {

    stepper3.setSpeed(-setSpeed);
    stepper3.setAcceleration(setAcceleration);
    stepper3.runSpeed();
  }
}