#include <AccelStepper.h>
#include <math.h>

#define FULLSTEP 1
#define HALFSTEP 2

#define dir1 6
#define step1 7
#define stepPerRound1 2048 // số xung để quay 1 vòng (pulse per round)
#define control_res1 FULLSTEP

#define dir2 8
#define step2 9
#define stepPerRound2 200 // số xung để quay 1 vòng (pulse per round)
#define control_res2 FULLSTEP

// độ dài tay
const int d1 = 12;
const int d2 = 12;
// goc
float alpha = 0;  // goc giua truc x va toa do dich
float theta1 = 0; // goc arm1 so voi truc x
float theta2 = 0; // goc arm2 so voi arm 1
// toa do x, y
float x = 0;
float y = 0;

AccelStepper arm1(AccelStepper::DRIVER, step1, dir1);
AccelStepper arm2(AccelStepper::DRIVER, step2, dir2);

void startup();
void RotateAngle(uint8_t step, float angle);
void setPoint(int x, int y);
void goToPoint(float x, float y);

void setup()
{
  arm1.setMaxSpeed(900);
  arm1.setAcceleration(400);

  arm2.setMaxSpeed(950);
  arm2.setAcceleration(400);
}

void loop()
{
   startup();
   goToPoint(24, 24);
   while (1);

}

void startup()
{
  arm1.setCurrentPosition(0);
  arm2.setCurrentPosition(0);
}

/// @brief dieu khien robot den vi tri x, y
void goToPoint(float x, float y)
{
  setPoint(x, y);
  RotateAngle(1, theta1);
  RotateAngle(2, theta2);
}

/// @brief: quay step x mot goc 'angle' so voi toa do goc ban dau
/// @param x: chon dong co can dieu khien
/// @param angle: goc can quay
void RotateAngle(uint8_t x, float angle)
{
  int pulse = 0;
  if (x == 1)
  {
    pulse = (int)angle * (stepPerRound1 * control_res1) / 360;
    arm1.moveTo(pulse);
    arm1.runToPosition();
  }
  if (x == 2)
  {
    pulse = (int)angle * (stepPerRound2 * control_res2) / 360;
    arm2.moveTo(pulse);
    arm2.runToPosition();
  }
}

/// @brief Nhap vao toa do x, y tra ve goc giua cac khau
void setPoint(float x, float y)
{
  float l = sqrt(x * x + y * y);
  alpha = atanf(y / x) * 180 / PI;
  float a = acosf((d1 * d1 + l * l - d2 * d2) / 2 * d1 * l);
  theta1 = (alpha - a) * 180 / PI;
  theta2 = (acosf((d1 * d1 + d2 * d2 - l * l) / (2 * d1 * d2))) * 180 / PI;
}