/**
 * Arduino Nano
 * L298N 
 **/

#include <SoftwareSerial.h>

#define Fpos 9
#define Fneg 10

#define Bpos 5
#define Bneg 6

#define Fsignal 11
#define Bsignal 12

#define Fbutton 3
#define Bbutton 4

const int Buttons[2] = {Fbutton, Bbutton};
volatile int ButtonState[4] = {1, 1};
volatile long ButtonTime[4] = {0, 0};

const int Fmotor = 0;
const int Bmotor = 1;

volatile int Fspeed = 255;
volatile int Bspeed = 255;

volatile boolean Frunning = false;
volatile boolean Brunning = false;

volatile char cmdChar = 'z';
const char Fcmd = 'D';
const char Bcmd = 'E';

SoftwareSerial softSerial (Fsignal, Bsignal);

void setup() {
  Serial.begin(57600);
  softSerialmy.begin(9600);
  pinMode(Fpos, OUTPUT);
  pinMode(Fneg, OUTPUT);
  pinMode(Bpos, OUTPUT);
  pinMode(Bneg, OUTPUT);

  pinMode(Fbutton, INPUT_PULLUP);
  pinMode(Bbutton, INPUT_PULLUP);
}

void loop() {
  if (Serial.available()) {
    cmdChar = (char)Serial.read();
  }
  if (cmdChar != 'z') {
    cmd(cmdChar);
  }

  if (softSerial.available() > 0) {
    cmdChar = (char)softSerial.read();
  }
  if (cmdChar != 'z') {
    cmd(cmdChar);
  }

  cmdChar = 'z';
}

void cmd (char which) {

}

//input value from 0 to 255
void set_speed (int motor, int val){ 
  if (motor == 0) {
    Fspeed = val;
  } else if (motor == 1) {
    Bspeed = val;
  }
}

void clockwise_fade (int motor, int startSpeed, int stopSpeed, int ms) {
  int steps = ceil((float) ms / (float) abs(stopSpeed - startSpeed));
  int stepMs = round((float) ms / (float) steps);
  int motorSpeed = startSpeed;

  if (motor == Fmotor) {
    analogWrite(Fneg, 0);
  } else if (motor == Bmotor) {
    analogWrite(Bneg, 0);
  }
  for (int i = 0; i < steps; i++) {
      if (motor == Fmotor) {
        analogWrite(Fpos, motorSpeed);
        Frunning = true;
      } else if (motor == Bmotor) {
        analogWrite(Bpos, motorSpeed);
        Brunning = true;
      }
      delay(stepMs);
      motorSpeed += stopSpeed > startSpeed ? 1 : -1;
  }
  if (motor == Fmotor) {
    Fspeed = stopSpeed;
  } else if (motor == Bmotor) {
    Bspeed = stopSpeed;
  }
}

void counter_clockwise_fade (int motor, int startSpeed, int stopSpeed, int ms) {
  int steps = ceil((float) ms / (float) abs(stopSpeed - startSpeed));
  int stepMs = round((float) ms / (float) steps);
  int motorSpeed = startSpeed;

  if (motor == Fmotor) {
    analogWrite(Fpos, 0);
  } else if (motor == Bmotor) {
    analogWrite(Bpos, 0);
  }
  for (int i = 0; i < steps; i++) {
      if (motor == Fmotor) {
        analogWrite(Fneg, motorSpeed);
        Frunning = true;
      } else if (motor == Bmotor) {
        analogWrite(Bneg, motorSpeed);
        Brunning = true;
      }
      delay(stepMs);
      motorSpeed += stopSpeed > startSpeed ? 1 : -1;
  }
  if (motor == Fmotor) {
    Fspeed = stopSpeed;
  } else if (motor == Bmotor) {
    Bspeed = stopSpeed;
  }
}

void clockwise (int motor){
  if (motor == Fmotor) {
    analogWrite(Fpos, Fspeed);
    analogWrite(Fneg, 0);
    Frunning = true;
  } else if (motor == Bmotor) {
    analogWrite(Bpos, Bspeed);
    analogWrite(Bneg, 0);
    Brunning = true;
  }
}

void counter_clockwise (int motor){
  if (motor == Fmotor) {
    analogWrite(Fpos, 0);
    analogWrite(Fneg, Fspeed);
    Frunning = true;
  } else if (motor == Bmotor) {
    analogWrite(Bpos, 0);
    analogWrite(Bneg, Bspeed);
    Brunning = true; 
  }
}

void brake (){
  if (Frunning) {
    brakeMotor(Fmotor);
  }
  if (Brunning) {
    brakeMotor(Bmotor);
  }
}

void brakeMotor (int motor) {
  if (motor == Fmotor) {
    digitalWrite(Fpos, LOW);
    digitalWrite(Fneg, LOW);
    Frunning = false;
  } else if (motor == Bmotor) {
    digitalWrite(Bpos, LOW);
    digitalWrite(Bneg, LOW);
    Brunning = false;
  }
}

/* ------------------------------------------------
 *  Reads the state of a specific button and compares
 *  it to a stored value in the button_state array.
 *  If the value is different than what is stored,
 *  check if button is pressed and store new timer value,
 *  if button is released, compare current time to the stored
 *  time value and pass that to the button_end function.
 * ------------------------------------------------*/
void btn (int index) {
  int val = digitalRead(Buttons[index]);
  if (val != ButtonState[index]) {
    if (val == LOW) { // pressed
      ButtonTime[index] = millis();
    } else if (val == HIGH) { // not pressed
      buttontime = millis() - ButtonTime[index];
      button_end(index, buttontime);
    }
  }
  ButtonState[index] = val;
}

/* ------------------------------------------------
 *  Determines a specific action for each press length
 *  of each button.
 * ------------------------------------------------*/
void button_end (int index, long buttontime) {
  if (index == 0) { //forward
    if (buttontime > 10) {
      //
    }
  } else if (index == 1) { //backward
    if (buttontime > 10) {
      //
    }
  }
  buttontime = 0;
}

/**
 * Vex = higher voltage that can power the motor and/or the shield as well as the Arduino if wanted
 * 5V = Must be a clean 5V that can power the Arduino and shield, and if wanted also the motor itself
 * GND = ass always, the ground of everything
 * B- = Connection of the first motor or first winding of the stepper motor
 * B+ = Connection of the first motor or first winding of the stepper motor
 * A- = Connection of the second motor or second winding of the stepper motor
 * A+ = Connection of the second motor or second winding of the stepper motor
 **/