#include <SoftwareSerial.h>
int PortOneTX = 11;
int PortOneRX = 12;
SoftwareSerial NanoTX(PortOneRX, PortOneTX);
// Actuator 1
#define ENA1_PIN 13  // the Arduino pin connected to the EN1 pin L298N
#define IN1_PIN A0   // the Arduino pin connected to the IN1 pin L298N
#define IN2_PIN A1   // the Arduino pin connected to the IN2 pin L298N

int AddressActu1 = 0;
int offset = 30000;
long THRESHOLD_AC1 = 20000 + offset;  //30000
long THRESHOLD_AC1_Active = 0;
bool IsAC1_Active = false;
bool IsAC1_Retracting = false;
bool IsAC1_Extending = false;
bool IsAC1Fully_Extended = false;
bool IsAC1Fully_Retracted = false;
unsigned long previousAC1Millis = 0;  // will store last time LED was updated

int AddressActu2 = 1;
long THRESHOLD_AC2 = 20000 + offset;  //30000
long THRESHOLD_AC2_Active = 0;
bool IsAC2_Active = false;
bool IsAC2_Retracting = false;
bool IsAC2_Extending = false;
bool IsAC2Fully_Extended = false;
bool IsAC2Fully_Retracted = false;
unsigned long previousAC2Millis = 0;  // will store last time LED was updated

double ch1 = 0;
double ch2 = 0;

#define ENGINE_START_SWITCH_PIN 4
#define ENGINE_STOP_SWITCH_PIN 8
#define RELAY_PIN 7
int AddressStartEngineSwitch = 2;
int engineStartSwitchVal = 0;
int engineStopSwitchVal = 0;

bool IS_ENGINE_ON = false;
bool IS_ENGINE_OFF = false;

// Chute Motor
double ch5 = 0;
double ch6 = 0;
bool IsChuteMotorRight = true;
#define ENA2_PIN 12  // the Arduino pin connected to the EN1 pin L298N
#define IN3_PIN A2   // the Arduino pin connected to the IN1 pin L298N
#define IN4_PIN A3   // the Arduino pin connected to the IN2 pin L298N
#define CHUTE_CH5_PIN 9
#define CHUTE_CH6_PIN 10
int ChuteRevolution = 5263;
bool IsChuteMotor = false;
bool IsChuteMotorInit = false;
unsigned long IS_CHUTE_ON_MARK;
int TestCounter = 0;
 
#define SPTR_SIZE 20
char *sPtr[SPTR_SIZE];
String command;

void initActuatorsPins(int Actuator_Numb = 1) {

  if (Actuator_Numb == 1) {
    pinMode(ENA1_PIN, OUTPUT);
    pinMode(IN1_PIN, OUTPUT);
    pinMode(IN2_PIN, OUTPUT);

    //Activate Actuator 1
    digitalWrite(ENA1_PIN, HIGH);
  } else if (Actuator_Numb == 2) {

    pinMode(ENA2_PIN, OUTPUT);
    pinMode(IN3_PIN, OUTPUT);
    pinMode(IN4_PIN, OUTPUT);

    //Activate Actuator 2
    digitalWrite(ENA2_PIN, HIGH);
  }
}


void setup() {
  Serial.begin(9600); 

  //init Actuator One
  initActuatorsPins(1);
  //init Actuator Two
  initActuatorsPins(2);
  
  NanoTX.begin(115200); 

  pinMode(A4, INPUT);
  pinMode(A5, INPUT);
 
}


void stopActuator(int Actuator_Numb = 1) {
  if (Actuator_Numb == 1) {
    // stop the actuator
    digitalWrite(IN1_PIN, LOW);
    digitalWrite(IN2_PIN, LOW);
    digitalWrite(ENA1_PIN, LOW);
    IsAC1_Active = false;
    IsAC1_Retracting = false;
    IsAC1_Extending = false;
  } else if (Actuator_Numb == 2) {
    // stop the actuator
    digitalWrite(IN3_PIN, LOW);
    digitalWrite(IN4_PIN, LOW);
    digitalWrite(ENA2_PIN, LOW);
    IsAC2_Active = false;
    IsAC2_Retracting = false;
    IsAC2_Extending = false;
  }
}

void extendActuator(long duration = 20000, int Actuator_Numb = 1) {
  if (Actuator_Numb == 1) {
    if (!IsAC1Fully_Extended) {
      // extend the actuator
      digitalWrite(ENA1_PIN, HIGH);
      digitalWrite(IN1_PIN, HIGH);
      digitalWrite(IN2_PIN, LOW);
      IsAC1_Active = true;
      IsAC1_Extending = true;
      IsAC1_Retracting = false;
      THRESHOLD_AC1_Active = duration;
    } else {
      //Serial.println("Actuator 1 max limit ");
    }
  } else if (Actuator_Numb == 2) {

    if (!IsAC2Fully_Extended) {
      // extend the actuator
      digitalWrite(ENA2_PIN, HIGH);
      digitalWrite(IN3_PIN, HIGH);
      digitalWrite(IN4_PIN, LOW);
      IsAC2_Active = true;
      IsAC2_Extending = true;
      IsAC2_Retracting = false;
      THRESHOLD_AC2_Active = duration;
    } else {
      //Serial.println("Actuator 2 max limit ");
    }
  }
}

void retractActuator(long duration = 20000, int Actuator_Numb = 1) {
  if (Actuator_Numb == 1) {
    if (!IsAC1Fully_Retracted) {
      // retracts the actuator
      digitalWrite(ENA1_PIN, HIGH);
      digitalWrite(IN1_PIN, LOW);
      digitalWrite(IN2_PIN, HIGH);
      IsAC1_Active = true;
      IsAC1_Retracting = true;
      IsAC1_Extending = false;
      THRESHOLD_AC1_Active = duration;
    } else {
      //Serial.println("Actuator 1 min limit ");
    }
  } else if (Actuator_Numb == 2) {

    if (!IsAC2Fully_Retracted) {
      // retracts the actuator
      digitalWrite(ENA2_PIN, HIGH);
      digitalWrite(IN3_PIN, LOW);
      digitalWrite(IN4_PIN, HIGH);
      IsAC2_Active = true;
      IsAC2_Retracting = true;
      IsAC2_Extending = false;
      THRESHOLD_AC2_Active = duration;
    } else {
      //Serial.println("Actuator 2 min limit ");
    }
  }
}

void checkActuators() {
  unsigned long currentMillis = millis();
  if (currentMillis - previousAC1Millis > THRESHOLD_AC1_Active && IsAC1_Active) {  // Check if 20000 seconds has been reached
    // save the last time you extend the actuator
    previousAC1Millis = currentMillis;
    if (IsAC1_Retracting) {
      IsAC1Fully_Retracted = true;
      IsAC1Fully_Extended = false;
      THRESHOLD_AC1_Active = 0; 

    } else if (IsAC1_Extending) {
      IsAC1Fully_Extended = true;
      IsAC1Fully_Retracted = false;
      THRESHOLD_AC1_Active = 0; 
    }
    stopActuator(1);
  }
  currentMillis = millis();
  if (currentMillis - previousAC2Millis > THRESHOLD_AC2_Active && IsAC2_Active) {  // Check if 20000 seconds has been reached
    // save the last time you extend the actuator
    previousAC2Millis = currentMillis;
    if (IsAC2_Retracting) {
      IsAC2Fully_Retracted = true;
      IsAC2Fully_Extended = false;
      THRESHOLD_AC2_Active = 0; 
    } else if (IsAC2_Extending) {
      IsAC2Fully_Extended = true;
      IsAC2Fully_Retracted = false;
      THRESHOLD_AC2_Active = 0; 
    }
    stopActuator(2);
  }
}

void rotateChuteMotor(bool dir = true, int degrees = 5263) {
  // dir = true CW , false CCW
  // 80/60 = 1.33 r/s
  //1.33 => 1
  //7 =>  y
  //y * 1.33 = 7
  // y = 5.2 *1000
  if (!IsChuteMotorInit) {
    if (dir) {
      digitalWrite(IN3_PIN, LOW);
      digitalWrite(IN4_PIN, HIGH);
      IsChuteMotorRight = true;
    } else {
      digitalWrite(IN3_PIN, HIGH);
      digitalWrite(IN4_PIN, LOW);
      IsChuteMotorRight = false;
    }
    IsChuteMotor = true;
    // start the motor
    digitalWrite(ENA2_PIN, HIGH);
    // wait until enough degrees rotated
    ChuteRevolution = degrees;
    IS_CHUTE_ON_MARK = millis();
  }
}

void CheckChuteMotor() {
  if (IsChuteMotor && (millis() - IS_CHUTE_ON_MARK >= ChuteRevolution)) {
    digitalWrite(ENA2_PIN, LOW);
    IsChuteMotor = false;
  }
}

  
void loop() {
  
  //if (NanoTX.available() > 0) {

  if (true) {
    //command = NanoTX.readStringUntil('\n'); 
    
    if(TestCounter < 5){
      if(TestCounter == 0) command = "0 0"; //Stop
      if(TestCounter == 1) command = "1535 1535"; //Forward
      if(TestCounter == 2) command = "1535 1450";  //turn left 
      if(TestCounter == 3) command = "1450 1535"; // turn right 
      if(TestCounter == 4) command = "1450 1450";  //backward
      TestCounter++;
    }
    
    if(TestCounter >= 5) TestCounter=0;
  
    Serial.print("command : ");
    Serial.println(command);
    
    command.trim();
    int N = separate(command, sPtr, SPTR_SIZE);
    // Serial.print("N : ");
    // Serial.println(N);
    char *data5 = sPtr[0];
    char *data6 = sPtr[1];
    // Serial.print("data5 : ");
    // Serial.println(data5);  
    // Serial.print("data5 : ");
    // Serial.println(data6);  
    ch5 = atoi(data5);
    ch6 = atoi(data6);
  }
  
  CheckChuteMotor();

  //ch5 = pulseIn(CHUTE_CH5_PIN, HIGH);
  //ch6 = pulseIn(CHUTE_CH6_PIN, HIGH);

  //Serial.print("ch5 = " + String(ch5));
  //Serial.println(" ch6 = " + String(ch6));


  if ((ch5 == 0) && (ch6 == 0)) {
    stopActuator(1);

    // Stop Chute Motor
    digitalWrite(ENA2_PIN, LOW);
    digitalWrite(IN3_PIN, LOW);
    digitalWrite(IN4_PIN, LOW);

    IsChuteMotorInit = false;
  }

  //Forward
  else if ((ch5 > 1530) && (ch6 > 1530)) {
    retractActuator(THRESHOLD_AC1, 1);
    rotateChuteMotor(false);
    IsChuteMotorInit = true;
  }
  //turn left
  else if ((ch5 > 1530) && (ch6 < 1460)) {

    retractActuator(THRESHOLD_AC1, 1);
    rotateChuteMotor(true);
    IsChuteMotorInit = true;
  }
  // turn right
  else if ((ch5 < 1460) && (ch6 > 1530)) {
    extendActuator(THRESHOLD_AC1, 1);
    rotateChuteMotor(false);
    IsChuteMotorInit = true;
  }
 //backward
  else if ((ch5 < 1460) && (ch6 < 1460)) {
    extendActuator(THRESHOLD_AC1, 1);
    rotateChuteMotor(true);
    IsChuteMotorInit = true;
  }

  else {
    stopActuator(1);

    // Stop Chute Motor
    digitalWrite(ENA2_PIN, LOW);
    digitalWrite(IN3_PIN, LOW);
    digitalWrite(IN4_PIN, LOW);

    IsChuteMotorInit = false;
  }

  Serial.print("out 1 : ");
  Serial.println(analogRead(A4));
    Serial.print("out 2 : ");
  Serial.println(analogRead(A5)); 
   delay(100);
}

int separate(
  String str,
  char **p,
  int size) {
  int n;
  char s[100];

  strcpy(s, str.c_str());

  *p++ = strtok(s, " ");
  for (n = 1; NULL != (*p++ = strtok(NULL, " ")); n++)
    if (size == n)
      break;

  return n;
}