/**********************************************************************
* CSL
* LARGE TRANSLATION MANAGEMENT 
* L298N DC MOTOR
***********************************************************************/
// DEFINES
#define STEPS 640     //Steps par tour (calibrated at 200mm)
#define PAS 5         //mm par tour
#define HOME "home"   //Homing command

#define IN1 9
#define IN2 8
#define SW1 2
#define SW2 3
#define SW3 4
#define TACH 4
#define ANALOG_IN A0
#define ENA 7
// GLOBAL VARIABLES
long speed = 0;
unsigned long debounceDelay = 50;
unsigned long lastDebounceTime = 0;
unsigned long currentTime = 0;
float stepCount = 0;
float distance = 0;
// Declaration of functions
void PciSetup(byte pin);
void Switch1Pressed();
void Switch2Pressed();
void Switch3Pressed();
void Switch1UNpressed();
void Switch2UNpressed();
void Switch3UNpressed();
boolean DebounceTimer();
void PrintMessa ();
void ProCommand (String command);
// Initiation
void setup() {
  // pin initiation
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(SW1, INPUT);
  pinMode(SW2, INPUT);
  pinMode(SW3, INPUT);
  pinMode(TACH, INPUT);
  pinMode(ENA, OUTPUT);
  PciSetup(TACH);
  TCCR2B = TCCR2B & B11111000 | B00000001; // for PWM frequency of 31372.55 Hz
  speed = 0;
  // Intialize interrrupt
  attachInterrupt(digitalPinToInterrupt(SW1),Switch1Pressed,RISING);
  attachInterrupt(digitalPinToInterrupt(SW2),Switch2Pressed,RISING);
  attachInterrupt(digitalPinToInterrupt(SW3),Switch3Pressed,RISING);
  Serial.begin(9600);
  Serial.println("System ON");
}
// Main Loop
void loop() {
  if(speed > 0)
  {    
      Serial.println("FORWARD");
      PrintMessa();
      analogWrite(ENA,0);// motor speed
      digitalWrite(IN1, LOW);// rotate forward
      digitalWrite(IN2,LOW);
      delay(500);
      analogWrite(ENA,speed);// motor speed
      digitalWrite(IN1, HIGH);// rotate forward
      digitalWrite(IN2,LOW);
  }
  else if (speed < 0)
  {
      Serial.println("BACKWARD");
      PrintMessa ();      
      analogWrite(ENA, 0);// motor speed  
      digitalWrite(IN1,LOW);// rotate forward
      digitalWrite(IN2,LOW);
      delay(500);
      analogWrite(ENA, -speed);// motor speed  
      digitalWrite(IN1,LOW);// rotate backwards
  }
  else
  {
    analogWrite(ENA,speed);
    digitalWrite(IN1,LOW);
    digitalWrite(IN2,LOW);
    PrintMessa();
  }

  while(! Serial.available()){}
  String cmd = "NONE";
  cmd = Serial.readString();
  Serial.print("CMD : ");
  Serial.println(cmd);
  ProCommand(cmd);
}

// Private functions
void PciSetup(byte pin)
{
  PCMSK2=16;
  PCIFR  |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt
  PCICR  |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group
}

ISR(PCINT2_vect)
{
  //Serial.println("ISR");
  if (speed < 0)
    stepCount--;
  if (speed > 0)
    stepCount++;
  distance = stepCount/STEPS*PAS;      //Calcul de distance
  if ((int)stepCount % (2*STEPS) == 0)
    PrintMessa();
}

void Switch1Pressed()
{
  if(DebounceTimer())
  {
    speed=0;
    detachInterrupt(digitalPinToInterrupt(SW1));
    analogWrite(ENA, speed);// motor speed  
    digitalWrite(IN1,LOW);// stop motor
    digitalWrite(IN2,LOW);
    Serial.println("Switch 1 pressed");
    PrintMessa();    
    attachInterrupt(digitalPinToInterrupt(SW1), Switch1UNpressed, FALLING); 
  }
}

void Switch2Pressed()
{
  if(DebounceTimer())
  {
    speed=0;
    detachInterrupt(digitalPinToInterrupt(SW2));
    analogWrite(ENA, speed);// motor speed  
    digitalWrite(IN1,LOW);// stop motor
    digitalWrite(IN2,LOW);
    Serial.println("Switch 2 pressed");
    PrintMessa();    
    attachInterrupt(digitalPinToInterrupt(SW2), Switch2UNpressed, FALLING); 
  }
}


void Switch3Pressed()
{
  if(DebounceTimer())
  {
    speed=0;
    detachInterrupt(digitalPinToInterrupt(SW3));
    analogWrite(ENA, speed);// motor speed  
    digitalWrite(IN1,LOW);// stop motor
    digitalWrite(IN2,LOW);
    Serial.println("Switch 3 pressed");
    PrintMessa();    
    attachInterrupt(digitalPinToInterrupt(SW3), Switch3UNpressed, FALLING); 
  }
}

void Switch1UNpressed()
{
  if(DebounceTimer())
  {
    detachInterrupt(digitalPinToInterrupt(SW1));
    Serial.println("Switch 1 realeased");
    attachInterrupt(digitalPinToInterrupt(SW1),Switch1Pressed,RISING);  
  }
}

void Switch2UNpressed()
{
  if(DebounceTimer())
  {
    detachInterrupt(digitalPinToInterrupt(SW2));
    Serial.println("Switch 2 realeased");
    attachInterrupt(digitalPinToInterrupt(SW2),Switch2Pressed,RISING);  
  }
}

void Switch3UNpressed()
{
  if(DebounceTimer())
  {
    detachInterrupt(digitalPinToInterrupt(SW3));
    Serial.println("Switch 3 realeased");
    attachInterrupt(digitalPinToInterrupt(SW3),Switch3Pressed,RISING);  
  }
}

boolean DebounceTimer()
{
  currentTime = millis();
  if(currentTime-lastDebounceTime > debounceDelay)
  {
    lastDebounceTime= currentTime;
    return true;
  }
    
  else
  {
    lastDebounceTime= currentTime;
    return false;
  }
}

void PrintMessa ()
{
  delay (100);
  Serial.print("Speed: ");
  Serial.print(speed);
  Serial.print(", Distance: ");
  Serial.println(distance);   
}

void ProCommand (String command)
{
  //if (command == HOME)
   //homing();
  //else
  speed = command.toInt();
  Serial.print("SET SPEED : ");
  Serial.println(speed);
}