#include "DualG2HighPowerMotorShield.h"
// https://www.pololu.com/docs/0J72
// https://github.com/pololu/dual-g2-high-power-motor-shield
#include <TimerThree.h>

// Uncomment the version corresponding with the version of your shield.
DualG2HighPowerMotorShield24v14 md;
// DualG2HighPowerMotorShield18v18 md;
// DualG2HighPowerMotorShield24v18 md;
// DualG2HighPowerMotorShield18v22 md;

//Variables bus RS485
const int EnTxPin =  49;  // HIGH:TX y LOW:RX
const int mydireccion = 108; //Direccion del esclavo
#define rs485 Serial2

#define fc1 18
#define fc2 19
#define DEBUG false
#define MAXSPEED 200
#define TIMERAMP 10

#define N 3                                  //Numero de vueltas por vuelta
int pulses = 0;                              //Output pulses.
uint8_t port = 0;
float pulses_max = 2400 * N;

volatile float angle_final = 0;
volatile bool search_angle = false;
volatile int  speed_actual = 0;

void A_CHANGE1() {
  port = PIND & 0x03; //B00000011;
  if (port == 0x00) pulses--;
  else if (port == 0x01) pulses++;
  else if (port == 0x02) pulses++;
  else if (port == 0x03) pulses--;
}

void B_CHANGE1() {
  port = PIND & 0x03; //B00000011;
  if (port == 0x00) pulses++;
  else if (port == 0x02) pulses--;
  else if (port == 0x01) pulses--;
  else if (port == 0x03) pulses++;
}


volatile boolean stop_right = false;
volatile boolean flag_right = false;

void FC1_CHANGE() { //PD3
  port = PIND & 0x0C; //B00001100;
  if ((port == 0x00) || (port == 0x04))
  {
    if (flag_right)
    {
      md.disableDrivers();
      md.setM1Speed(0);
      flag_right = false;
    }
    stop_right = true;
    pulses = 0;
  }
  else if ((port == 0x0C) || (port == 0x08))
  {
    md.enableM1Driver();
    stop_right = false;
  }
}

volatile boolean stop_left = false;
volatile boolean flag_left = false;

void FC2_CHANGE() { //PD2
  port = PIND & 0x0C; //B00001100;
  if ((port == 0x00) || (port == 0x08))
  {
    if (flag_left)
    {
      md.disableDrivers();
      md.setM1Speed(0);
      flag_left = false;
    }
    stop_left = true;
    pulses_max = pulses;
  }
  else if ((port == 0x04) || (port == 0x0C))
  {
    md.enableM1Driver();
    stop_left = false;
  }

}

void setup()
{
  Serial.begin(115200);
#if DEBUG
  Serial.println("Dual G2 High Power Motor Shield");
#endif

  //Inicializacion bus RS485
  rs485.begin(115200);
  rs485.setTimeout(100);  //establecemos un tiempo de espera de 100ms
  pinMode(EnTxPin, OUTPUT);
  digitalWrite(EnTxPin, LOW); //RS485 como receptor

  //Inicializacion de los finales de carrera y parada
  pinMode(fc1, INPUT);
  pinMode(fc2, INPUT);
  if (!digitalRead(fc1)) stop_right = true;
  if (!digitalRead(fc2)) stop_left  = true;

  //Para subir la frecuencia del PWM pin 9 del Mega 2560 a 31333 Hz
  //TCCR2B = (TCCR2B & 0xF8) | 1 ;

  /*  TIMER 2       (Pin 9, 10)
    Value                             Divisor                      Frequency
    0x01                                  1                                31.374 KHz
    0x02                                  8                                3.921 KHz
    0x03                                 32                             980.3 Hz
    0x04                                 64                             490.1 Hz               // default
    0x05                                 128                           245 hz
    0x06                                 256                           122.5 hz
    0x07                                 1024                           30.63 hz*/

  attachInterrupt(3, A_CHANGE1, CHANGE);//PIN20 PD1 Encoder revisar
  attachInterrupt(2, B_CHANGE1, CHANGE);//PIN21 PD0 Encoder revisar
  attachInterrupt(5, FC1_CHANGE, CHANGE);//PIN18 PD3 FC1
  attachInterrupt(4, FC2_CHANGE, CHANGE);//PIN19 PD2 FC2

  md.init();
  md.calibrateM1CurrentOffset();
  delay(10);

  Timer3.initialize(10000);
  Timer3.attachInterrupt(timerRead); // blinkLED to run every 0.15 seconds



  //calibrate();

}


void loop()
{

  int temp_ang = actual_angle();
  if ((temp_ang != angle_final) && (!search_angle))
  {
    delay(100);
    move_motor(angle_final, 100);
  }

  if (rs485.available())
  {
    if (rs485.read() == 'I') //Si recibimos el inicio de trama
    {
      int direccion = rs485.parseInt(); //recibimos la direccion
      if (direccion == mydireccion) //Si direccion es la nuestra
      {
        char funcion = rs485.read(); //leemos el carácter de función

        //---Si el carácter de función es una S entonces la trama es para mover el motor-----------
        if (funcion == 'S')
        {
          int angulo = rs485.parseInt(); //recibimos el ángulo
          if (rs485.read() == 'V') //Si el fin de trama es el correcto
          {
            int speed = rs485.parseInt(); //recibimos la velocidad

            if (rs485.read() == 'F') //Si el fin de trama es el correcto
            {
              if (angulo < 512) //verificamos que sea un valor en el rango del servo
              {
                //myservo.write(angulo); //movemos el servomotor al ángulo correspondiente.
                move_motor(angulo, speed);
                Serial.print("Slave ");
                Serial.print(mydireccion);
                Serial.print(" go to ");
                Serial.print(angulo);
                Serial.print(" ");
                Serial.println(speed);
              }
            }
          }
        }
        else if (funcion == 'C')
        {
          if (rs485.read() == 'F') //Si el fin de trama es el correcto
          {
            calibrate();
          }
        }
        //---Si el carácter de función  es L entonces el maestro está solicitando una lectura del sensor---
        else if (funcion == 'L')
        {
          if (rs485.read() == 'F') //Si el fin de trama es el correcto
          {
            //int lectura = analogRead(0); //realizamos  la lectura del sensor
            float temp_angle = actual_angle(); //realizamos  la lectura del senso
            digitalWrite(EnTxPin, HIGH); //rs485 como transmisor
            rs485.print("i"); //inicio de trama
            rs485.print(mydireccion); //direccion
            rs485.print(",");
            rs485.print((int)temp_angle); //valor del sensor
            rs485.print("f"); //fin de trama
            rs485.flush(); //Esperamos hasta que se envíen los datos
            digitalWrite(EnTxPin, LOW); //RS485 como receptor
            Serial.print("Slave ");
            Serial.print(mydireccion);
            Serial.print(" i am in ");
            Serial.println(temp_angle);
          }
        }
        else if (funcion == 'A')
        {
          if (rs485.read() == 'F') //Si el fin de trama es el correcto
          {
            //int lectura = analogRead(0); //realizamos  la lectura del sensor
            float limit_angle = pulses_to_angle(pulses_max); //realizamos  la lectura del senso
            digitalWrite(EnTxPin, HIGH); //rs485 como transmisor
            rs485.print("i"); //inicio de trama
            rs485.print(mydireccion); //direccion
            rs485.print(",");
            rs485.print((int)limit_angle); //valor del sensor
            rs485.print("f"); //fin de trama
            rs485.flush(); //Esperamos hasta que se envíen los datos
            digitalWrite(EnTxPin, LOW); //RS485 como receptor
            Serial.print("Slave ");
            Serial.print(mydireccion);
            Serial.print(" my limit is ");
            Serial.println(limit_angle);
          }
        }
      }
    }
  }
  delay(10);
}

void timerRead(void)
{
  if (flag_right)
  {
    //      #if DEBUG
    //        Serial.println("RIGHT");
    //      #endif
    if ((actual_angle() <= angle_final) && (search_angle))
    {
      md.disableDrivers();
      md.setM1Speed(0);
      //Serial.println("STOP");
      flag_right = false;
      search_angle = false;
    }

  }
  else if (flag_left)
  {
    //      #if DEBUG
    //        Serial.println("LEFT");
    //      #endif
    if ((actual_angle() >= angle_final) && (search_angle))
    {
      md.disableDrivers();
      md.setM1Speed(0);
      flag_left = false;
      search_angle = false;
    }
  }
#if DEBUG
  Serial.print("Pulsos: ");
  Serial.print(motor_pulses());
  Serial.print(" Angulo: ");
  Serial.print(actual_angle());
  Serial.print(" Angulo destino: ");
  Serial.println(angle_final);

  Serial.print(stop_right);
  Serial.print(" ");
  Serial.print(stop_left);
  Serial.print(" ");
  Serial.println(pulses_to_angle(pulses_max));

  Serial.print("M1 current: ");
  Serial.print(md.getM1CurrentMilliamps());
  Serial.println(" mA");
#endif
}

int motor_pulses()
{
  return pulses;
}

void stopIfFault()
{
  if (md.getM1Fault())
  {
    md.disableDrivers();
    delay(1);
    Serial.println("M1 fault");
    while (1);
  }
  if (md.getM2Fault())
  {
    md.disableDrivers();
    delay(1);
    Serial.println("M2 fault");
    while (1);
  }
}

//void enable_motor(){
//  if ((!stop_left)&&(!stop_right)) md.enableM1Driver();
//  else if ((stop_left)||(stop_right)) md.disableDrivers();
//}
//
//void disable_motor(){
//  if ((stop_left)||(stop_right)) md.disableDrivers();
//}

float angle_to_pulses(int val)
{
  return ((float)val * 2400 * N) / 360;
}

float pulses_to_angle(int val)
{
  return ((float)val * 360) / (2400 * N);
}

void calibrate()
{
  Serial.print(F("Calibrating right"));
  right_motor(100);
  while (!stop_right);// Serial.print(F("."));//delay(10);
  Serial.println();
  delay(2000);
  Serial.print(F("Calibrating left"));
  left_motor(100);
  while (!stop_left);// Serial.print(F("."));//delay(10);
  Serial.println();
  delay(2000);
  Serial.println(F("Calibration finish"));
  delay(1000);
 // move_motor( pulses_to_angle(pulses_max)/2, 100);
}

volatile unsigned long time_ramp = 0;

void right_motor(int speed)
{
  if (!stop_right)
  {
    time_ramp = millis();
    flag_right = true;
    if (speed > MAXSPEED) speed = MAXSPEED;
    md.enableM1Driver();
    for (int i = 0; i >= -speed; i--) //RIGHT
    {
      while (((millis() - time_ramp) <= TIMERAMP) && (!stop_right));
      if (!stop_right)
      {
        speed_actual = i;
        md.setM1Speed(i);
        time_ramp = millis();
      }
      else break;
    }
  }
#if DEBUG
  else Serial.println("No se puede ir a la derecha");
#endif
}


void left_motor(int speed)
{
  if (!stop_left)
  {
    time_ramp = millis();
    flag_left = true;
    if (speed > MAXSPEED) speed = MAXSPEED;
    md.enableM1Driver();
    for (int i = 0; i <= speed; i++) //LEFT
    {
      speed_actual = i;
      while (((millis() - time_ramp) <= TIMERAMP) && (!stop_left));
      if (!stop_left)
      {
        md.setM1Speed(i);
        time_ramp = millis();
      }
      else break;
    }
  }
#if DEBUG
  else Serial.println("No se puede ir a la izquierda");
#endif
}

float actual_angle()
{
  return pulses_to_angle(motor_pulses());
}

float temp_angle = actual_angle();

void move_motor(int angle, int speed)
{
  temp_angle = actual_angle();
  angle_final = angle;
  if (temp_angle > angle)
  {
    search_angle = true;
    right_motor(speed);
  }
  else if (temp_angle < angle)
  {
    search_angle = true;
    left_motor(speed);
  }
}