#include <Wire.h>
#include <SparkFun_TB6612.h>

// i2c configuration
#define SLAVE_ADDRESS 0x04

// Motor A - Vertical
#define STANDBY_A A0 // Shared with B
#define AIN1      8
#define AIN2      9
#define PWMA      10
#define INVERT_A  1 // -1 to flip direction

// Motor B Firing
#define STANDBY_B A0 // Shared with B
#define BIN1      7
#define BIN2      6 
#define PWMB      5
#define INVERT_B  1 // -1 to flip direction

// Motor C - Horizontal
#define STANDBY_C A1 // Separate from A and B
#define CIN1      13
#define CIN2      12
#define PWMC      11
#define INVERT_C  1 // -1 to flip direction

#define LOADED_SWITCH A2

// Motors!
Motor m_vert(  AIN1, AIN2, PWMA, INVERT_A, STANDBY_A);
//Motor m_horiz( CIN1, CIN2, PWMC, INVERT_C, STANDBY_C);
//Motor m_firing(BIN1, BIN2, PWMB, INVERT_B, STANDBY_B);
Motor m_horiz( BIN1, BIN2, PWMB, INVERT_B, STANDBY_B);
Motor m_firing(CIN1, CIN2, PWMC, INVERT_C, STANDBY_C);

// Speeds
//    Actual PWM is a linear percentage between minspeed and maxspeed. Each speed step is 10%
#define VERTICAL 0
#define HORIZONTAL 1
#define FIRING 2

int speed[]     = { 0, 0, 0 };      // Starting Motor Speeds (vertical, horizontal, firing)
int minspeed[]  = { 96, 32, 32};    // Min PWM
int maxspeed[]  = { 192, 64, 255}; // Max PWM


// Run once at boot
void setup()
{
  Serial.begin(115200);
  Serial.println("Initializing...");

  // Initialize our switches
  pinMode(LOADED_SWITCH, INPUT_PULLUP);

  // i2c:
  Wire.begin(SLAVE_ADDRESS);
  Wire.onReceive(receiveData); // Interrupt
  Wire.onRequest(sendData);    // Interrupt

  // Directions. Note that these are the same over the i2c bus.
  Serial.println("Use asdw to increase/decrease speed of the directional motors.");
  Serial.println("Use rf to increase/decrease speed of the firing motor.");
  Serial.println("Use ' ' to stop all motion.");
  Serial.println("Ready");
/*
  for(int i = -100; i <= 100; i++) {
    Serial.print("Speed: ");
    Serial.print(i);
    Serial.print("; PWM: ");
    Serial.println(calc_speed(i, minspeed[VERTICAL], maxspeed[VERTICAL]));
  }
*/
}

void loop()
{
  handle_serial(); /* Adjust to any user input */
  /* i2c bus will also be fired automatically as data is received */

  m_vert.drive(calc_speed(speed[VERTICAL]*10, minspeed[VERTICAL], maxspeed[VERTICAL]));
  m_horiz.drive(calc_speed(speed[HORIZONTAL]*10, minspeed[HORIZONTAL], maxspeed[HORIZONTAL]));
  m_firing.drive(calc_speed(speed[FIRING]*10, minspeed[FIRING], maxspeed[FIRING]));
}/* --(end main loop )-- */

int calc_speed(int speed, int min, int max) {
  // Returns a PWM speed that is "speed percent" of the way between min and max.
  int sign = 1;
  
  if(speed < -100) {
    speed = -100;
  }
  if(speed > 100) {
    speed = 100;
  }
  
  if(speed == 0) {
    return 0;
  }
  
  if(speed < 0) {
    sign = -1;
    speed *= -1;
  }
  
  return sign * ( (speed * (max - min) / 100) + min);
}

void handle_serial(void) {
  // Read bytes from the serial port if they're available and process them.
  while(Serial.available() > 0) {
    char rc = Serial.read();
    process_char(rc);
  }
}

void receiveData(int byteCount) {
  while(Wire.available() > 0) {
    char rc = Wire.read();
    process_char(rc);
  }
}

void sendData() {
  // Master has asked us for data, so we send our three speeds
  char buffer[512];
  sprintf(buffer, "Master requested current speeds: Vertical: %i; Horizontal: %i; Firing: %i", speed[VERTICAL], speed[HORIZONTAL], speed[FIRING]);
  Serial.println(buffer);
  
  Wire.write(speed[FIRING]);
  Wire.write(speed[HORIZONTAL]);
  Wire.write(speed[VERTICAL]);  
}
  
void process_char(char rc) {
  char buffer[512]; // I smell security issue, but right now I odn't know how to do it better.
  bool changed = false;
  
  Serial.print("Processing character: ");
  Serial.println(rc);
  switch(rc) {
    case 'a':
      changed = true;
      speed[HORIZONTAL]--;
      Serial.print("Decreasing horizontal speed. New speed: ");
      Serial.println(speed[HORIZONTAL]);
      break;
    case 's':
      changed = true;
      speed[VERTICAL]--;
      Serial.print("Decreasing vertical speed. New speed: ");
      Serial.println(speed[VERTICAL]);
      break;
    case 'd':
      changed = true;
      speed[HORIZONTAL]++;
      Serial.print("Increasing horizontal speed. New speed: ");
      Serial.println(speed[HORIZONTAL]);
      break;
    case 'w':
      changed = true;
      speed[VERTICAL]++;
      Serial.print("Increasing vertical speed. New speed: ");
      Serial.println(speed[VERTICAL]);
      break;
    case 'r':
      changed = true;
      speed[FIRING]++;
      Serial.print("Increasing firing speed. New speed: ");
      Serial.println(speed[FIRING]);
      break;
    case 'f':
      changed = true;
      speed[FIRING]--;
      Serial.print("Decreasing firing speed. New speed: ");
      Serial.println(speed[FIRING]);
      break;
    case ' ':
      changed = true;
      speed[HORIZONTAL] = 0;
      speed[VERTICAL] = 0;
      speed[FIRING] = 0;
      Serial.println("Set all speeds to 0");
      break;
    default:
      Serial.println("No action associated.");
  }

  if(changed == true) {
    sprintf(buffer, "New Speeds. Current speeds: Vertical: %i; Horizontal: %i; Firing: %i", speed[VERTICAL], speed[HORIZONTAL], speed[FIRING]);
    Serial.println(buffer);
  }
}