#define motor1Pin1 9
#define motor1Pin2 3
#define motor2Pin1 6
#define motor2Pin2 5
//#define redLed 4
//#define greenLed 5
//#define blueLed 6

#include <PS3USB.h>
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif

USB Usb;
/* You can create the instance of the class in two ways */
PS3USB PS3(&Usb); // This will just create the instance
//PS3USB PS3(&Usb,0x00,0x15,0x83,0x3D,0x0A,0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch

boolean printAngle;
uint8_t state = 0;

void setup()
{
  pinMode(motor1Pin1, OUTPUT);   //this is an output because the Arduino is telling the motor to spin
  pinMode(motor1Pin2, OUTPUT);
  pinMode(motor2Pin1, OUTPUT);
  pinMode(motor2Pin2, OUTPUT);
  //pinMode(redLed, OUTPUT);
  //pinMode(greenLed, OUTPUT);
  //pinMode(blueLed, OUTPUT);
    Serial.begin(115200);
     while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while (1); //halt
  }
     Serial.print(F("\r\nPS3 USB Library Started"));
}

void loop()
{
  Usb.Task();

  if (PS3.PS3Connected || PS3.PS3NavigationConnected)
  {
    if (PS3.getButtonPress (R1))
      {
      GoForward();
      //Serial.print(F("\r\nR1"));
      }
    if (PS3.getButtonPress (L1))
      {
      GoBackward();
      //Serial.print(F("\r\nL1"));
      }

    if (PS3.getAnalogHat(LeftHatX) == 255)
      {
      GoRight();
      //Serial.print(F("\r\nLeftHatX: "));
      //Serial.print(PS3.getAnalogHat(LeftHatX));
      }
    if (PS3.getAnalogHat(LeftHatX) == 0)
      {
      GoLeft();
     // Serial.print(F("\r\nLeftHatX: "));
      //Serial.print(PS3.getAnalogHat(LeftHatX));
      }
    if (PS3.getButtonClick (CIRCLE))
     {
      Stop();
     }
  }
}

void GoForward()
  {
  digitalWrite(motor1Pin1, LOW);
  digitalWrite(motor1Pin2, HIGH);
  digitalWrite(motor2Pin1, LOW);
  digitalWrite(motor2Pin2, HIGH);
  }

void GoBackward()
  {
  digitalWrite(motor1Pin1, HIGH);
  digitalWrite(motor1Pin2, LOW);
  digitalWrite(motor2Pin1, HIGH);
  digitalWrite(motor2Pin2, LOW);
  }
void Stop()
  {
  digitalWrite(motor1Pin1, LOW);
  digitalWrite(motor1Pin2, LOW);
  digitalWrite(motor2Pin1, LOW);
  digitalWrite(motor2Pin2, LOW);
  }
void GoLeft()
  {
  digitalWrite(motor1Pin1, LOW);
  digitalWrite(motor1Pin2, HIGH);
  digitalWrite(motor2Pin1, HIGH);
  digitalWrite(motor2Pin2, LOW);
  }
void GoRight()
  {
  digitalWrite(motor1Pin1, HIGH);
  digitalWrite(motor1Pin2, LOW);
  digitalWrite(motor2Pin1, LOW);
  digitalWrite(motor2Pin2, HIGH);
  }