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

#define DIR_AZ 18
#define STEP_AZ 10
#define DIR_EL 6
#define STEP_EL 7

#define MS1 9
#define EN 8

#define SPR 200
#define RATIO 54
#define T_DELAY 60000

#define HOME_AZ 4
#define HOME_EL 5

#define ANGLE_SCANNING_MULT 180
#define MAX_AZ_ANGLE 360
#define MAX_EL_ANGLE 360

#define HOME_DELAY 6000

unsigned long t_DIS = 0;

AccelStepper AZstepper(1, STEP_AZ, DIR_AZ);
AccelStepper ELstepper(1, STEP_EL, DIR_EL);

void setup()
{
  AZstepper.setMaxSpeed(150);
  AZstepper.setAcceleration(50);

  ELstepper.setMaxSpeed(150);
  ELstepper.setAcceleration(50);

  pinMode(EN, OUTPUT);
  digitalWrite(EN, LOW);

  pinMode(MS1, OUTPUT);
  digitalWrite(MS1, LOW);

  pinMode(HOME_AZ, INPUT);
  pinMode(HOME_EL, INPUT);

  Serial.begin(19200);

  Homing(deg2step(-ANGLE_SCANNING_MULT), deg2step(-ANGLE_SCANNING_MULT));
}

void loop()
{
  static long AZstep = 0;
  static long ELstep = 0;

  if (t_DIS == 0)
    t_DIS = millis();

  if (!AZstepper.isRunning() && !ELstepper.isRunning() && millis() - t_DIS > T_DELAY)
  {
    digitalWrite(EN, HIGH);
  }
  else
  {
    digitalWrite(EN, LOW);
  }

  cmd_proc(AZstep, ELstep);
  stepper_move(AZstep, ELstep);
}

/* ================= HOMING (INALTERATO) ================= */

void Homing(long AZsteps, long ELsteps)
{
  int value_Home_AZ = HIGH;
  int value_Home_EL = HIGH;
  int n_AZ = 1;
  int n_EL = 1;
  boolean isHome_AZ = false;
  boolean isHome_EL = false;

  AZstepper.moveTo(AZsteps);
  ELstepper.moveTo(ELsteps);

  while (isHome_AZ == false || isHome_EL == false)
  {
    value_Home_AZ = digitalRead(HOME_AZ);
    value_Home_EL = digitalRead(HOME_EL);

    if (value_Home_AZ == HIGH)
    {
      AZstepper.moveTo(AZstepper.currentPosition());
      isHome_AZ = true;
    }

    if (value_Home_EL == HIGH)
    {
      ELstepper.moveTo(ELstepper.currentPosition());
      isHome_EL = true;
    }

    if (AZstepper.distanceToGo() == 0 && !isHome_AZ)
    {
      n_AZ++;
      AZsteps = deg2step(pow(-1, n_AZ) * n_AZ * ANGLE_SCANNING_MULT);
      if (abs(n_AZ * ANGLE_SCANNING_MULT) > MAX_AZ_ANGLE)
      {
        error(0);
        break;
      }
      AZstepper.moveTo(AZsteps);
    }

    if (ELstepper.distanceToGo() == 0 && !isHome_EL)
    {
      n_EL++;
      ELsteps = deg2step(pow(-1, n_EL) * n_EL * ANGLE_SCANNING_MULT);
      if (abs(n_EL * ANGLE_SCANNING_MULT) > MAX_EL_ANGLE)
      {
        error(1);
        break;
      }
      ELstepper.moveTo(ELsteps);
    }

    AZstepper.run();
    ELstepper.run();
  }

  long time = millis();
  while (millis() - time < HOME_DELAY)
  {
    AZstepper.run();
    ELstepper.run();
  }

  AZstepper.setCurrentPosition(0);
  ELstepper.setCurrentPosition(0);
}

/* ================= EASYCOMM 2 ================= */

void cmd_proc(long &stepAz, long &stepEl)
{
  char buffer[256];
  char incomingByte;
  static int counter = 0;
  char *p;
  char *str;
  char data[100];

  double angleAz, angleEl;

  while (Serial.available() > 0)
  {
    incomingByte = Serial.read();

    if (incomingByte == '!')
    {
      Serial.print("TM1 AZ");
      Serial.print(10 * step2deg(AZstepper.currentPosition()), 1);
      Serial.print(" EL");
      Serial.println(10 * step2deg(ELstepper.currentPosition()), 1);
    }

    else if (incomingByte == '\n')
    {
      buffer[counter] = 0;
      p = buffer;

      if (buffer[0] == 'A' && buffer[1] == 'Z')
      {
        if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L')
        {
          Serial.print("AZ");
          Serial.print(step2deg(AZstepper.currentPosition()), 1);
          Serial.print(" EL");
          Serial.println(step2deg(ELstepper.currentPosition()), 1);
        }
        else
        {
          str = strtok_r(p, " ", &p);
          strncpy(data, str + 2, 10);
          angleAz = atof(data);

          /* ===== Improved Meridian Flip ===== */

          double currentAz = step2deg(AZstepper.currentPosition());
          double targetAz = angleAz;

          currentAz = fmod(currentAz, 360.0);
          if (currentAz < 0) currentAz += 360.0;

          targetAz = fmod(targetAz, 360.0);
          if (targetAz < 0) targetAz += 360.0;

          double delta = targetAz - currentAz;
          delta = fmod(delta + 540.0, 360.0) - 180.0;

          stepAz = AZstepper.currentPosition() + deg2step(delta);

          /* ===== Elevation ===== */

          str = strtok_r(p, " ", &p);
          if (str[0] == 'E' && str[1] == 'L')
          {
            strncpy(data, str + 2, 10);
            angleEl = atof(data);
            stepEl = deg2step(angleEl);
          }
        }
      }

      else if (strncmp(buffer, "SA SE", 5) == 0)
      {
        Serial.print("AZ");
        Serial.print(step2deg(AZstepper.currentPosition()), 1);
        Serial.print(" EL");
        Serial.println(step2deg(ELstepper.currentPosition()), 1);

        stepAz = AZstepper.currentPosition();
        stepEl = ELstepper.currentPosition();
      }

      else if (strncmp(buffer, "RESET", 5) == 0)
      {
        Serial.print("AZ");
        Serial.print(step2deg(AZstepper.currentPosition()), 1);
        Serial.print(" EL");
        Serial.println(step2deg(ELstepper.currentPosition()), 1);

        Homing(0, 0);
        stepAz = 0;
        stepEl = 0;
      }

      counter = 0;
      t_DIS = 0;
    }

    else
    {
      if (counter < sizeof(buffer) - 1)
      {
        buffer[counter++] = incomingByte;
      }
    }
  }
}

/* ================= ERROR ================= */

void error(int num_error)
{
  switch (num_error)
  {
    case 0:
      while (1)
      {
        Serial.println("AL001");
        delay(100);
      }

    case 1:
      while (1)
      {
        Serial.println("AL002");
        delay(100);
      }

    default:
      while (1)
      {
        Serial.println("AL000");
        delay(100);
      }
  }
}

/* ================= STEPPER ================= */

void stepper_move(long stepAz, long stepEl)
{
  AZstepper.moveTo(stepAz);
  ELstepper.moveTo(stepEl);

  AZstepper.run();
  ELstepper.run();
}

/* ================= CONVERSIONI ================= */

long deg2step(double deg)
{
  return (long)(RATIO * SPR * deg / 360.0);
}

double step2deg(long Step)
{
  return 360.0 * (double)Step / (SPR * RATIO);
}