#include <SPI.h>
#include <SoftwareSerial.h>

#define HC595_CLK 13
#define HC595_Data 11
#define HC595_OutLatch 10

#define speedPin A0

#define motor1 1
#define motor2 2
#define motor3 3
#define motor4 4

#define MOTOR1_A 2
#define MOTOR1_B 3
#define MOTOR2_A 1
#define MOTOR2_B 4
#define MOTOR4_A 0
#define MOTOR4_B 6
#define MOTOR3_A 5
#define MOTOR3_B 7

// Constants that the user passes in to the motor calls
#define FORWARD 1
#define BACKWARD 2
#define BRAKE 3
#define RELEASE 4

// Constants that the user passes in to the stepper calls#define speedPin A0
uint16_t speed = 0;

uint8_t latch_state = 0;

void DCMotorRun(uint8_t motorNum, uint8_t cmd) {
  uint8_t a, b;
  switch (motorNum) {
  case motor1:
    a = MOTOR1_A; b = MOTOR1_B; break;
  case motor2:
    a = MOTOR2_A; b = MOTOR2_B; break;
  case motor3:
    a = MOTOR3_A; b = MOTOR3_B; break;
  case motor4:
    a = MOTOR4_A; b = MOTOR4_B; break;
  default:
    return;
  }
  
  switch (cmd) {
  case FORWARD:
    latch_state |= _BV(a);
    latch_state &= ~_BV(b); 
    break;
  case BACKWARD:
    latch_state &= ~_BV(a);
    latch_state |= _BV(b); 
    break;
  case RELEASE:
    latch_state &= ~_BV(a);     // A and B both low
    latch_state &= ~_BV(b); 
    break;
  }

  digitalWrite(HC595_OutLatch, LOW);
  SPI.transfer(latch_state);
  digitalWrite(HC595_OutLatch, HIGH);
}

void setup() {
  Serial.begin(95200);
  pinMode(3, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(9, OUTPUT);

  pinMode(HC595_CLK,      OUTPUT);
  pinMode(HC595_Data,     OUTPUT);
  pinMode(HC595_OutLatch, OUTPUT);
  // initialize SPI:
  SPI.begin();
  SPI.beginTransaction(SPISettings(8000000, MSBFIRST, SPI_MODE0));


  speed = analogRead(speedPin) >> 2;
  analogWrite(3, speed);
  analogWrite(5, speed);
  analogWrite(6, speed);
  analogWrite(9, speed);

  while (!Serial) {}
  Serial.println("Bluetooth motor ready.");
}

void loop() {
  // Serial.println("Entered loop");
  uint16_t speedNew = analogRead(speedPin) >> 2;
  if (speed != speedNew) {
    speed = speedNew;
    analogWrite(3, speed);
    analogWrite(5, speed);
    analogWrite(6, speed);
    analogWrite(9, speed);
    Serial.print("Speed: ");
    Serial.println(speed);
  }

  if (Serial.available()) {
    // char cmd = toupper(BT.read());
    char cmd = toupper(Serial.read());
    Serial.println(cmd);

    if (cmd == 'F') {          // Forward
      DCMotorRun(motor1, FORWARD);
      DCMotorRun(motor2, FORWARD);
      DCMotorRun(motor3, FORWARD);
      DCMotorRun(motor4, FORWARD);
      Serial.println("You pressed F");
    }
    else if (cmd == 'B') {     // Backward
      DCMotorRun(motor1, BACKWARD);
      DCMotorRun(motor2, BACKWARD);
      DCMotorRun(motor3, BACKWARD);
      DCMotorRun(motor4, BACKWARD);
    }
    else if (cmd == 'L') {      // Left turn
      DCMotorRun(motor1, BACKWARD);
      DCMotorRun(motor2, FORWARD);
      DCMotorRun(motor3, BACKWARD);
      DCMotorRun(motor4, FORWARD);
    }
    else if (cmd == 'R') {       // Right turn
      DCMotorRun(motor1, FORWARD);
      DCMotorRun(motor2, BACKWARD);
      DCMotorRun(motor3, FORWARD);
      DCMotorRun(motor4, BACKWARD);
    }
    else if (cmd == 'S') {     // Stop
      DCMotorRun(motor1, RELEASE);
      DCMotorRun(motor2, RELEASE);
      DCMotorRun(motor3, RELEASE);
      DCMotorRun(motor4, RELEASE);
    }
    else {
      Serial.println("Didn't get anything");
    }
  }
}