#include <Arduino.h>
#include <SPI.h>
#include <RPC.h>
#include <constants.h>

using namespace rtos;

Thread sensorThread;

#define x1_axis 0
#define y0_axis 1
#define x0_axis 2

#define RIGHT false
#define LEFT true
#define STOP 0
#define ZERO 1
#define CONTINUOUS 2
#define ANGLE_MODE 3


// Dispositivo 360
int x0Degrees = 0;
unsigned long x0Duration = 10000;
int x1Degrees = 0;
unsigned long x1Duration = 10000;
int y0Degrees = 0;
unsigned long y0Duration = 10000;
bool syncx0WithInterval360 = false;
bool syncx1WithInterval360 = false;
bool syncy0WithInterval360 = false;
bool isAction[3] = {false, false, false};
int mode = STOP;


#if defined(ARDUINO_GIGA_M7)
      #define debug Serial 
#elif defined(ARDUINO_GIGA_M4) 
      #define debug RPC 
#endif

#define TIME_PULSE_XY 2500
#define TIME_PULSE_A 500

bool direction[3] = {RIGHT, RIGHT, RIGHT};

unsigned long time_fc_zero[3] = {millis(), millis(), millis()};
bool enable_zero[3] = {true, true, true};

void search_zero()
  {
    time_fc_zero[x1_axis] = millis();
    while(digitalRead(FC[x1_axis])||((millis()-time_fc_zero[x1_axis])<10))
      {
        digitalWrite(PUL[x1_axis], HIGH);
        delayMicroseconds(2000);
        digitalWrite(PUL[x1_axis], LOW);
        delayMicroseconds(2000);
        if(digitalRead(FC[x1_axis])) time_fc_zero[x1_axis] = millis();
      }
    time_fc_zero[y0_axis] = millis();
    while(digitalRead(FC[y0_axis])||((millis()-time_fc_zero[y0_axis])<10))
      {
        digitalWrite(PUL[y0_axis], HIGH);
        delayMicroseconds(2000);
        digitalWrite(PUL[y0_axis], LOW);
        delayMicroseconds(2000);
        if(digitalRead(FC[y0_axis])) time_fc_zero[y0_axis] = millis();
      };
    time_fc_zero[x0_axis] = millis();
    while(digitalRead(FC[x0_axis])||((millis()-time_fc_zero[x0_axis])<10))
      {
        digitalWrite(PUL[x0_axis], HIGH);
        delayMicroseconds(250);
        digitalWrite(PUL[x0_axis], LOW);
        delayMicroseconds(250);
        if(digitalRead(FC[x0_axis])) time_fc_zero[x0_axis] = millis();
      }
  }

int angle_pulses[3] = {0, 0, 0}; //Varaible temporal de pulsos 
float axis_angle[3] = {0, 0, 0}; //Angulo actual

float angleRead(int axis)
  {
    int sensorReading = angle_pulses[axis];
    float reduction = REDUCTION_XY;
    if (axis==2) reduction = REDUCTION_A;
    return (360*sensorReading/(reduction*PULSE_REV));
  }

int axis_pulses[3] = {0, 0, 0}; //Pulsos necesarios para llegar al angulo indicado
int axis_direction[3] = {1, 1, 0}; //Direccion de giro
int angle_pulses_MAX[3] = {0, 0, 0}; //Pulsos necesarios para dar una vuelta completa
int angle_pulses_ant[3] = {0, 0, 0}; //Varaible temporal de pulsos 
bool flag_search[3] = {false, false, false};
bool flag_equal[3] = {true, true, true};
bool flag_refresh[3] = {false, false, false};

unsigned long time_axis[3] = { millis(), millis(), millis()};
unsigned long time_parts[3] = {10, 10, 10};
unsigned long time_pulse[3] = {TIME_PULSE_XY, TIME_PULSE_XY, TIME_PULSE_A};
int ant_angle[3] = {0, 0, 0};

void stop_all()
  {
    for(int i=0; i<3; i++) flag_refresh[i] = false;
  }

#define RESOLUTION_ANGLE 0.25
float axis_angle_ant[3] = {0,0,0};

void search_angle(int axis, int angle, unsigned long time_initial, unsigned long time_final)
  {
    if(isAction[axis])
      {
        axis_angle[axis] = angleRead(axis);
        if((axis_angle[axis]>((float)angle+RESOLUTION_ANGLE))||(axis_angle[axis]<((float)angle-RESOLUTION_ANGLE)))
          {
            if (axis_angle_ant[axis]!=axis_angle[axis])
              {
                #if DEBUG_M4
                  RPC.print(axis_angle[axis]);
                  RPC.print(" ");
                  RPC.println(angle);
                #endif
                axis_angle_ant[axis] = axis_angle[axis];
              }
            if (flag_search[axis])
              {
                flag_search[axis] = false;
                #if DEBUG_M4
                  RPC.print(axis_angle[axis]);
                  RPC.print(" ");
                  RPC.print(angle);
                #endif
                if (axis_angle[axis]<(angle + RESOLUTION_ANGLE)) direction[axis] = RIGHT;
                else direction[axis] = LEFT;
                if ((abs(axis_angle[axis])>360)||(abs(angle)>360))enable_zero[axis] = false;
                else enable_zero[axis] = true;
                if((axis==y0_axis)||(axis==x0_axis)) direction[axis]=!direction[axis];
                digitalWrite(DIRP[axis], direction[axis]);
                if (axis!=x0_axis) axis_pulses[axis] = map(abs(angle-axis_angle[axis]), 0, 359, 0, PULSE_REV*REDUCTION_XY);
                else axis_pulses[axis] = map(abs(angle-axis_angle[axis]), 0, 359, 0, PULSE_REV*REDUCTION_A);
                time_pulse[axis] = ((1000/2)*(time_final-time_initial)/axis_pulses[axis]); //1000 es conversion de milisegundos a microsegundos entre dos porque es tiempo de 0 a 1
                if((time_pulse[axis]<TIME_PULSE_XY)&&(axis<=y0_axis)) time_pulse[axis]=TIME_PULSE_XY;
                else if((time_pulse[axis]<TIME_PULSE_A)&&(axis==x0_axis)) time_pulse[axis]=TIME_PULSE_A;
                #if DEBUG_M4
                  RPC.print(" ");
                  RPC.print(axis_pulses[axis]);
                  RPC.print(" ");
                  RPC.println(time_pulse[axis]);
                #endif
                flag_refresh[axis] = true;
              }
          }
        else if((axis_angle[axis]<=((float)angle+RESOLUTION_ANGLE))&&(axis_angle[axis]>=((float)angle-RESOLUTION_ANGLE))) flag_refresh[axis] = false;
      }
  }

unsigned long time_refresh_x = micros();
unsigned long time_refresh_y = micros();
unsigned long time_refresh_a = micros();
bool state_x = false;
bool state_y = false;
bool state_a = false;
void refresh_steppers()
  {
    if(((micros()-time_refresh_x)>=time_pulse[x1_axis])&&(flag_refresh[x1_axis]))
      {
        time_refresh_x = micros();
        digitalWrite(PUL[x1_axis], state_x);
        if(state_x) 
          {
            if (direction[x1_axis]==RIGHT) angle_pulses[x1_axis]++;
            else angle_pulses[x1_axis]--;
          }
        state_x = !state_x;      
      }
    if(((micros()-time_refresh_y)>=time_pulse[y0_axis])&&(flag_refresh[y0_axis]))
      {
        time_refresh_y = micros();
        digitalWrite(PUL[y0_axis], state_y);
        if(state_y) 
          {
            if (direction[y0_axis]==LEFT) angle_pulses[y0_axis]++;
            else angle_pulses[y0_axis]--;
          }
        state_y = !state_y;
      }
    if(((micros()-time_refresh_a)>=time_pulse[x0_axis])&&(flag_refresh[x0_axis]))
      {
        time_refresh_a = micros();
        digitalWrite(PUL[x0_axis], state_a);
        if(state_a)
          {
            if (direction[x0_axis]==LEFT) angle_pulses[x0_axis]++;
            else angle_pulses[x0_axis]--;
          } 
        state_a = !state_a;
      }
  }


long startTimex = 0;
long startTimey = 0;
long startTimea = 0;
const int timeThreshold = 500;

void f_x1_axis(){ //Funcion de paso por cero de la interrupcion del eje x
   if (((millis() - startTimex) > timeThreshold)&&(enable_zero[x1_axis]))
    {
      if(angle_pulses[x1_axis]>0) angle_pulses_MAX[x1_axis] = angle_pulses[x1_axis]; //Varaible de pulsos maximos por vuelta
      angle_pulses[x1_axis] = 0; //Varaible temporal de pulsos 
      startTimex = millis();
    }
  }

void f_y0_axis(){ //Funcion de paso por cero de la interrupcion del eje y
   if (((millis() - startTimey) > timeThreshold)&&(enable_zero[y0_axis]))
    {
      if(angle_pulses[y0_axis]>0) angle_pulses_MAX[y0_axis] = angle_pulses[y0_axis]; //Varaible de pulsos maximos por vuelta
      angle_pulses[y0_axis] = 0; //Varaible temporal de pulsos 
      startTimey = millis();
    }
  }

void f_x0_axis(){ //Funcion de paso por cero de la interrupcion del eje a
   if (((millis() - startTimea) > timeThreshold)&&(enable_zero[x0_axis]))
    {
      if(angle_pulses[x0_axis]>0) angle_pulses_MAX[x0_axis] = angle_pulses[x0_axis]; //Varaible de pulsos maximos por vuelta
      angle_pulses[x0_axis] = 0; //Varaible temporal de pulsos 
      startTimea = millis();
    }
  } 


/*
Functions on the M4 that returns axis
*/
int AxisA_Read() {
  int result = angle_pulses[x0_axis];
  return result;
}

int AxisX_Read() {
  int result = angle_pulses[x1_axis];
  return result;
}

int AxisY_Read() {
  int result = angle_pulses[y0_axis];
  return result;
}

int read_value[10];
int* decode_values(String inputString_rpc, int num_dec)
  {
    String str = inputString_rpc.substring(inputString_rpc.lastIndexOf(":") + 1);
    read_value[0] = str.substring(0, str.indexOf(',')).toInt();
    for(int i=1; i<num_dec; i++)
      {
        str = str.substring(str.indexOf(',')+1);
        read_value[i] = str.substring(0, str.indexOf(',')).toInt();
      }
    return read_value;
  }

char c_rpc;
String inputString_rpc;
void RPCRead()
  {
     ////////////////////////////////////////////////////////////
     /////// RUTINA TRATAMIENTO DE STRINGS DEL UART   ///////////
     ////////////////////////////////////////////////////////////
     if (RPC.available())
       {
         c_rpc = RPC.read();
         if ((c_rpc == '\r') || (c_rpc == '\n'))
         {
          digitalWrite(LED_BUILTIN, LOW);
          if (inputString_rpc.startsWith("/axisA:")) //Interval Motor
            {
              int* value = decode_values(inputString_rpc, 4);
              #if DEBUG_M4
                RPC.print("Recibido axis M4: ");
                RPC.println(inputString_rpc);
                for(int i=0; i<4; i++) RPC.println(value[i]);
              #endif
              x0Degrees = value[0];
              x0Duration = value[1]*1000;
              syncx0WithInterval360 = value[2];
              mode = ANGLE_MODE;
              if(!value[3]) //Save or test
                {
                  isAction[x0_axis] = true;
                  flag_search[x0_axis] = true;
                  time_axis[x0_axis] = millis();
                }
            }
          else if (inputString_rpc.startsWith("/axisX:")) //Interval Motor
            {
              int* value = decode_values(inputString_rpc, 4);
              #if DEBUG_M4
                RPC.print("Recibido axis M4: ");
                RPC.println(inputString_rpc);
                for(int i=0; i<4; i++) RPC.println(value[i]);
              #endif
              x1Degrees = value[0];
              x1Duration = value[1]*1000;
              syncx1WithInterval360 = value[2];
              mode = ANGLE_MODE;
              if(!value[3]) //Save or test
                {
                  isAction[x1_axis] = true;
                  flag_search[x1_axis] = true;
                  time_axis[x1_axis] = millis();
                }
            }
          else if (inputString_rpc.startsWith("/axisY:")) //Interval Motor
            {
              int* value = decode_values(inputString_rpc, 4);
              #if DEBUG_M4
                RPC.print("Recibido axis M4: ");
                RPC.println(inputString_rpc);
                for(int i=0; i<4; i++) RPC.println(value[i]);
              #endif
              y0Degrees = value[0];
              y0Duration = value[1]*1000;
              syncy0WithInterval360 = value[2];
              mode = ANGLE_MODE;
              if(!value[3]) //Save or test
                {
                  isAction[y0_axis] = true;
                  flag_search[y0_axis] = true;
                  time_axis[y0_axis] = millis();
                }
            }
          else if (inputString_rpc.startsWith("/action:")) //Interval Motor
            {
              int* value = decode_values(inputString_rpc, 1);
              #if DEBUG_M4
                RPC.print("Recibido action M4: ");
                RPC.println(inputString_rpc);
                for(int i=0; i<1; i++) RPC.println(value[i]);
              #endif
              if (value[0])
              {
                for(int i=0; i<3; i++)
                {
                  isAction[i] = true;
                  flag_search[i] = true;
                  time_axis[i] = millis();
                }
              }
              else
                for(int i=0; i<3; i++) 
                  {
                    isAction[i] = false;
                    flag_refresh[i] = false;
                  }
            }
          inputString_rpc = String();
          digitalWrite(LED_BUILTIN, HIGH);
         }
         else
           inputString_rpc += c_rpc;
       }
  }

void requestReading() {
  while (true) {
    //delay(100);
    RPCRead();
  }
}

void setup() {
  RPC.begin();
  for(int i=0; i<3; i++)
    {
      pinMode(EN[i], OUTPUT);
      pinMode(DIRP[i], OUTPUT);
      pinMode(PUL[i], OUTPUT);
      digitalWrite(EN[i], LOW);
      digitalWrite(DIRP[i], HIGH);
      digitalWrite(PUL[i], HIGH);
    }
  for(int i=0; i<3; i++) pinMode(FC[i], INPUT_PULLUP);
  search_zero();
  //delay(10000);
  attachInterrupt(digitalPinToInterrupt(FC[x1_axis]), f_x1_axis, FALLING);
  attachInterrupt(digitalPinToInterrupt(FC[y0_axis]), f_y0_axis, FALLING);
  attachInterrupt(digitalPinToInterrupt(FC[x0_axis]), f_x0_axis, FALLING);

  flag_refresh[y0_axis]=false;
  flag_refresh[x1_axis]=false;
  flag_refresh[x0_axis]=false;

  //Bind the sensorRead() function on the M4
  RPC.bind("AxisA", AxisA_Read);
  RPC.bind("AxisX", AxisX_Read);
  RPC.bind("AxisY", AxisY_Read);
  /*
  Starts a new thread that loops the requestReading() function
  */
  //sensorThread.start(requestReading);
}

void loop() {
  search_angle(x0_axis, x0Degrees, 0, x0Duration);
  search_angle(x1_axis, x1Degrees, 0, x1Duration);
  search_angle(y0_axis, y0Degrees, 0, y0Duration);
  refresh_steppers();
  RPCRead();
}