#include "DualMC33926MotorShield.h"   //Pololu 3A motor shield
                                      // speed input vales 0-400

DualMC33926MotorShield md;           // fans are 24V 4" SEAFLO 3A Max

#define MOTORTHRESHOLD 50           // where the fans start spinning
#define PRINTCHANGES false

//    pins used by mc33926
//    A0 - M1FB
//    A1 - M2FB
//    4 - D2
//    7  - m1DIR
//    8  - m2DIR
//    9  - m1PWM
//    10 - m2PWM
//    12 - SF

int serialpin = 2;  // SPDT center terminal to ground (14)
int manualpin = 3;  // serial-manual switch
int knobpin = A2;    // pot knob pin for manual speed or max serial speed  (also GND, +5V)

int knobpos;
int switchpos;
int prev_switch = 0;
int prev_knob = 0;

int LSpeed;
int RSpeed;

byte bytesBuffer[6] = {0};


// SETUP ***********************************************
void setup()
{
  Serial.begin(115200);
  //  Serial.println("Dual MC33926 Motor Shield");
  md.init();

  pinMode(serialpin, INPUT_PULLUP);
  pinMode(manualpin, INPUT_PULLUP);
}

// MAIN LOOP ***********************************************
void loop() {

  switch (getSwitch()) {
    case 1 : {
        LSpeed = ReadKnob();
        RSpeed = LSpeed;
        blow(LSpeed, RSpeed);
        break;
      }
    case -1 : {
        ReadData();
        blow(LSpeed, RSpeed);
        break;
      }
    case 0 : {
        blow(0, 0);
        break;
      }
  }
  //  delay (500);
}

// CHECK SWITCH ***********************************************
int getSwitch() {
  // 0 is off
  // 1 is manual knob
  // -1 is serial
  int value = 0;
  // get switch position
  int manualpos = digitalRead( manualpin );
  int serialpos = digitalRead( serialpin );
  delay (1);
  if (manualpos == LOW && serialpos == HIGH ) value = 1;
  if (manualpos == HIGH && serialpos == LOW  ) value = -1;

  if (value != prev_switch && PRINTCHANGES) {
    Serial.println(value);
  }
  prev_switch = value;
  return value;
}

// BLOW ***********************************************
void blow( int LSpeedIn, int RSpeedIn) {
  //  dont spin until threshold
  if (LSpeedIn <= (MOTORTHRESHOLD)) {
    LSpeed = 0;
  }
  else {
    LSpeed = LSpeedIn;
  }
  if (RSpeedIn <= (MOTORTHRESHOLD)) {
    RSpeed = 0;
  }
  else {
    RSpeed = RSpeedIn;
  }
  md.setM1Speed(LSpeed);
  //  stopIfFault();

  md.setM2Speed(RSpeed);
  //  stopIfFault();
  printcurrent();
}


//  READ KNOB ********************************************
int ReadKnob() {
  int rawvalue = analogRead( knobpin );
  knobpos = MOTORTHRESHOLD + map(rawvalue, 0, 1023, 0, 400-MOTORTHRESHOLD);
  delay (1);
  if (knobpos != prev_knob && PRINTCHANGES) {
    Serial.print("Knob ");
    Serial.println(knobpos);
  }
  prev_knob = knobpos;
  return knobpos;
}

//  READ SERIAL  ********************************************
void ReadData() {

  int maxspeed = ReadKnob();
  if (Serial.available())
  {
    // Read new byte and put it at the end of the buffer
    bytesBuffer[0] = bytesBuffer[1];
    bytesBuffer[1] = bytesBuffer[2];
    bytesBuffer[2] = bytesBuffer[3];
    bytesBuffer[3] = bytesBuffer[4];
    bytesBuffer[4] = bytesBuffer[5];
    bytesBuffer[5] = Serial.read();
    //   counter--;
    Serial.println(bytesBuffer[3]);
    // Verify if we have a valid sequence "WIND"
    if (bytesBuffer[0] == 87 && bytesBuffer[1] == 73 && bytesBuffer[2] == 78 && bytesBuffer[3] == 68 )
    {
      Serial.print("valid: L:");
      Serial.print(bytesBuffer[4]);
      Serial.print(" R: ");
      Serial.println(bytesBuffer[5]);

      // Set the pins values
      LSpeed = MOTORTHRESHOLD + map(bytesBuffer[4], 0, 255, 0, 400-MOTORTHRESHOLD);
      RSpeed = MOTORTHRESHOLD + map(bytesBuffer[5], 0, 255, 0, 400-MOTORTHRESHOLD);

      // knob sets maximum

      LSpeed = map(LSpeed, MOTORTHRESHOLD, 400, MOTORTHRESHOLD, maxspeed);
      RSpeed = map(RSpeed, MOTORTHRESHOLD, 400, MOTORTHRESHOLD, maxspeed);
    }
     // get disconnect "STOP"
    if (bytesBuffer[0] == 83 && bytesBuffer[1] == 84 && bytesBuffer[2] == 79 && bytesBuffer[3] == 80 )
    {
      Serial.end();
      setup();
    }

  }
}


// FAULT ***********************************************

void stopIfFault()
{
  if (md.getFault())
  {
    md.setM1Speed(0);
    md.setM2Speed(0);
    //    Serial.println("fault");
    while (1);
  }
}


// PRINT DEBUGGING***********************************************

void printcurrent()
{
  switch (getSwitch()) {
    case 0 : {
        Serial.println("OFF: ");
        break;
      }

    case 1 : {
        Serial.print("Manual: ");
        printdata();
        break;
      }
    case -1 : {
        Serial.print("Serial: ");
        printdata();
        break;
      }
  }
}

void printdata() {
  Serial.print(knobpos);
  Serial.print("  M1: ");
  Serial.print(LSpeed);
  Serial.print("  mA:");
  Serial.print(md.getM1CurrentMilliamps());
  Serial.print("  M2: ");
  Serial.print(RSpeed);
  Serial.print("  mA:");
  Serial.println(md.getM2CurrentMilliamps());
}