const int T1_YELLOW = A3;
const int T2_RED = A2;
const int T3_BLACK = A1;
const int T4_BLUE = A0;
const int LED_PIN = PD3;
const int MAX_STEPS = 48;

int currentStep = 0; // Indicates in which step I am (0,47)-> your motor do 48 steps in one rotation
int currentAngle = 0; // Indicates in which angle I am (0 to 360)
int numOfSteps = 0;

#define STEP_DELAY 100

void setup()
{
  pinMode(LED_PIN, OUTPUT);
  pinMode(T4_BLUE, OUTPUT);
  pinMode(T3_BLACK, OUTPUT);
  pinMode(T2_RED, OUTPUT);
  pinMode(T1_YELLOW, OUTPUT);
  Serial.begin(9600);

  currentStep = 0;
  currentAngle = 0;
}



void stepMotor(int stepType)
{
  switch (stepType)
  {
    case 0:
      digitalWrite(T1_YELLOW, LOW);
      digitalWrite(T2_RED, HIGH);
      digitalWrite(T3_BLACK, HIGH);
      digitalWrite(T4_BLUE, LOW);
      break;

    case 1:
      digitalWrite(T1_YELLOW, LOW);
      digitalWrite(T2_RED, LOW);
      digitalWrite(T3_BLACK, HIGH);
      digitalWrite(T4_BLUE, HIGH);
      break;

    case 2:
      digitalWrite(T1_YELLOW, HIGH);
      digitalWrite(T2_RED, LOW);
      digitalWrite(T3_BLACK, LOW);
      digitalWrite(T4_BLUE, HIGH);
      break;

    case 3:
      digitalWrite(T1_YELLOW, HIGH);
      digitalWrite(T2_RED, HIGH);
      digitalWrite(T3_BLACK, LOW);
      digitalWrite(T4_BLUE, LOW);
      break;
  }
}

// Move x steps (negative will move backwards)
void step(int stepsToMove)
{
  int stepsLeft = abs(stepsToMove); // how many steps to take
  int direction = 0;

  // determine direction based on whether stepsToMove is + or -:
  if (stepsToMove > 0)
  {
    direction = 1;
  }

  if (stepsToMove < 0)
  {
    direction = 0;
  }

  // decrement the number of steps, moving one step each time:
  while (stepsLeft > 0)
  {
    // increment or decrement the step number,
    // depending on direction:
    if (direction == 1)
    {
      currentStep++;
      if (currentStep == MAX_STEPS)
      {
        currentStep = 0;
      }
    }
    else // direction==0
    {
      if (currentStep == 0)
      {
        currentStep = MAX_STEPS;
      }
      currentStep--;
    }
    // decrement the steps left:
    stepsLeft--;

    // step the motor to step number 0, 1, ..., {3 or 10}
    stepMotor(currentStep % 4);
    delay(STEP_DELAY);
  }
}

// Obtain the number of steps required for moving x degrees from the current position
int angleToStep(int angle) {
  return (angle * 48) / 360;
}

// Moves the stepper motor to a specific Angle
void moveToAngle(int angle)
{
  // Find the shortest path (meaning, which direction requires the least number of steps)
  int difference = angle - currentAngle;
  if (abs(difference) > 180) {
    int conjugateAngle = 360 - abs(difference);
    numOfSteps = -(difference >= 0 ? -angleToStep(conjugateAngle) : angleToStep(conjugateAngle));
  } else {
    numOfSteps = -(angleToStep(difference));
  }
  currentAngle = angle;
  // Move the stepper motor
  step(numOfSteps);
}


void loop()
{

  if (Serial.available())
  {
    // Read the data received via bluetooth, 
    // Obtain the new position (angle) 
    // and call moveToAngle to start moving the stepper motor
    String data = Serial.readString();
    moveToAngle(data.toInt() % 360);
  }
}