// L298N - Version: Latest 
#include <L298N.h>

const unsigned int bridgeC4 = 43;
const unsigned int bridgeC3 = 42; 
const unsigned int bridgeC2 = 41;
const unsigned int bridgeC1 = 40; 
const unsigned int bridgeB4 = 39;
const unsigned int bridgeB3 = 38;
const unsigned int bridgeB2 = 37; 
const unsigned int bridgeB1 = 36; 
const unsigned int bridgeA4 = 35; 
const unsigned int bridgeA3 = 34;
const unsigned int bridgeA2 = 33;
const unsigned int bridgeA1 = 32;
int irH = 31;
int irG = 30;
int irF = 29;
int irE = 28;
int irD = 27;
int irC = 26;
int irB = 25;
int irA = 24;


L298N motorA(bridgeA1, bridgeA2);
L298N motorB(bridgeA3, bridgeA4);
L298N motorC(bridgeB1, bridgeB2);
L298N motorD(bridgeB3, bridgeB4);
L298N motorE(bridgeC1, bridgeC2);
L298N motorF(bridgeC3, bridgeC4);

String command;
//String label =       "| A | B | C | D | E | F |G|H|";
const String mState =  "| - | - | - | - | - | - | | |";
const String irState = "|o--|o--|o--|o--|o--|o--|o|o|";

void guish(){
  Serial.println();
  Serial.println();
  Serial.println();
  Serial.println();
  Serial.println();
  Serial.println("| A | B | C | D | E | F |G|H|");
  Serial.println(mState);
  irRead();
}
void motorN(){
  if (command.equalsIgnoreCase("a n"))
  {
    motorA.forward();
    mState.setCharAt(2, 'N');
  }
  else if (command.equalsIgnoreCase("b n"))
  {
    motorB.forward();
    mState.setCharAt(6, 'N');
  }
  else if (command.equalsIgnoreCase("c n"))
  {
    motorC.forward();
    mState.setCharAt(10, 'N');
  }
  else if (command.equalsIgnoreCase("d n"))
  {
    motorD.forward();
    mState.setCharAt(14, 'N');
  }
  else if (command.equalsIgnoreCase("e n"))
  {
    motorE.forward();
    mState.setCharAt(18, 'N');
  }
  else if (command.equalsIgnoreCase("f n"))
  {
    motorF.forward();
    mState.setCharAt(22, 'N');
  }
}
void motorS(){
  if (command.equalsIgnoreCase("a s"))
  {
    motorA.backward();
    mState.setCharAt(2, 'S');
  }
  else if (command.equalsIgnoreCase("b s"))
  {
    motorB.backward();
    mState.setCharAt(6, 'S');
  }
  else if (command.equalsIgnoreCase("c s"))
  {
    motorC.backward();
    mState.setCharAt(10, 'S');
  }
  else if (command.equalsIgnoreCase("d s"))
  {
    motorD.backward();
    mState.setCharAt(14, 'S');
  }
  else if (command.equalsIgnoreCase("e s"))
  {
    motorE.backward();
    mState.setCharAt(18, 'S');
  }
  else if (command.equalsIgnoreCase("f s"))
  {
    motorF.backward();
    mState.setCharAt(22, 'S');
  }
}
void motorOff(){
  if (command.equalsIgnoreCase("a o"))
  {
    motorA.stop();
    mState.setCharAt(2, 'X');
  }
  else if (command.equalsIgnoreCase("b o"))
  {
    motorB.stop();
    mState.setCharAt(6, 'X');
  }
  else if (command.equalsIgnoreCase("c o"))
  {
    motorC.stop();
    mState.setCharAt(10, 'X');
  }
  else if (command.equalsIgnoreCase("d o"))
  {
    motorD.stop();
    mState.setCharAt(14, 'X');
  }
  else if (command.equalsIgnoreCase("e o"))
  {
    motorE.stop();
    mState.setCharAt(18, 'X');
  }
  else if (command.equalsIgnoreCase("f o"))
  {
    motorF.stop();
    mState.setCharAt(22, 'X');
  }
  else if (command.equalsIgnoreCase("off"))
  {
    motorA.stop();
    mState.setCharAt(2, 'X');
    motorB.stop();
    mState.setCharAt(6, 'X');
    motorC.stop();
    mState.setCharAt(10, 'X');
    motorD.stop();
    mState.setCharAt(15, 'X');
    motorE.stop();
    mState.setCharAt(18, 'X');
    motorF.stop();
    mState.setCharAt(22, 'X');
  }
}
void irRead(){
  const String irState = "|o--|o--|o--|o--|o--|o--|o|o|";
  if (digitalRead(irA) == 1) // Check if the pin high or not
  {
    irState.setCharAt(1, "x");
  }
  if (digitalRead(irB) == 1) // Check if the pin high or not
  {
    irState.setCharAt(5, "X");
  }
  if (digitalRead(irC) == 1) // Check if the pin high or not
  {
    irState.setCharAt(9, "X");
  }
  if (digitalRead(irD) == 1) // Check if the pin high or not
  {
    irState.setCharAt(13, "X");
  }
  if (digitalRead(irE) == 1) // Check if the pin high or not
  {
    irState.setCharAt(17, "X");
  }
  if (digitalRead(irF) == 1) // Check if the pin high or not
  {
    irState.setCharAt(21, "X");
  }
  if (digitalRead(irG) == 1) // Check if the pin high or not
  {
    irState.setCharAt(25, "X");
  }
  if (digitalRead(irH) == 1) // Check if the pin high or not
  {
    irState.setCharAt(27, "X");
  }
  Serial.println(irState);
}
void trigger(){
  irState = "|o--|o--|o--|o--|o--|o--|o|o|";
  Serial.println("Trigger mode initiated");
  guish();
  while(digitalRead(irA) == 1){
    delay(1);
  }
  motorA.forward();
  mState.setCharAt(2, 'N');
  guish();
  while(digitalRead(irB) == 1){
    delay(1);
  }
  motorA.backward();
  mState.setCharAt(2, 'S');
  motorB.forward();
  mState.setCharAt(6, 'N');
  guish();
  while(digitalRead(irC) == 1){
    delay(1);
  }
  motorA.stop();
  mState.setCharAt(2, '-');
  motorB.backward();
  mState.setCharAt(6, 'S');
  motorC.forward();
  mState.setCharAt(10, 'N');
  guish();
  while(digitalRead(irD) == 1){
    delay(1);
  }
  motorB.stop();
  mState.setCharAt(6, '-');
  motorC.backward();
  mState.setCharAt(10, 'S');
  motorD.forward();
  mState.setCharAt(14, 'N');
  guish();
  while(digitalRead(irE) == 1){
    delay(1);
  }
  motorC.stop();
  mState.setCharAt(10, '-');
  motorD.backward();
  mState.setCharAt(14, 'S');
  motorE.forward();
  mState.setCharAt(18, 'N');
  guish();
  while(digitalRead(irF) == 1){
    delay(1);
  }
  motorD.stop();
  mState.setCharAt(14, '-');
  motorE.backward();
  mState.setCharAt(18, 'S');
  motorF.forward();
  mState.setCharAt(22, 'N');
  guish();
  while(digitalRead(irG) == 1){
    delay(1);
  }
  motorE.stop();
  mState.setCharAt(18, '-');
  motorF.backward();
  mState.setCharAt(22, 'S');
  guish();
  while(digitalRead(irH) == 1){
    delay(1);
  }
  motorF.stop();
  mState.setCharAt(22, '-');
  Serial.println("Trigger complete");
  guish();
}

void setup()
{
  Serial.begin(9600); // Init Serila at 9600 Baud
  Serial.println("Serial Working"); // Test to check if serial is working or not
  pinMode(irA, INPUT);
  pinMode(irB, INPUT);
  pinMode(irC, INPUT);
  pinMode(irD, INPUT);
  pinMode(irE, INPUT); // IR Sensor pin INPUT
  pinMode(irF, INPUT);
  pinMode(irG, INPUT);
  pinMode(irH, INPUT);
}

void loop()
{
   if (Serial.available() > 0) {
    while (Serial.available() > 0) {
      command = Serial.readStringUntil('\n');
      Serial.println ("tested: " + command);
      if (command.endsWith(" n"))
      {
        motorN();
      }
      else if (command.endsWith(" s"))
      {
        motorS();
      }
      else if (command.endsWith(" o"))
      {
       motorOff();
      }
      else if (command.endsWith("off"))
      {
      motorOff();
      }
      else if (command.equalsIgnoreCase("motor"))
      {
        guish();
      }
      else if (command.equalsIgnoreCase("trigger"))
      {
        trigger();
      }
    }
  }
}