#include <PS3BT.

h>
#include <Kangaroo.h>
USB Usb;
BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
PS3BT PS3(&Btd); // This will just create the instance
KangarooSerial K(Serial3);
KangarooChannel K1(K, '1');
KangarooChannel K2(K, '2');

int redPin = 8;
int bluePin = 9;

void setup() {
Serial3.begin(19200);

Serial.begin(9600);
pinMode(redPin , OUTPUT);
pinMode(bluePin , OUTPUT);

K1.start();
K1.home().wait();
K2.start();
K2.home().wait();

#if !defined(__MIPSEL__)
while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy
and other boards with built-in USB CDC serial connection
#endif
if (Usb.Init() == -1) {
Serial.print(F("\r\nOSC did not start"));
while (1); //halt
}
Serial.print(F("\r\nPS3 Bluetooth Library Started"));
}

void loop() {
Usb.Task();
if (PS3.PS3Connected || PS3.PS3NavigationConnected) {
int v = 0 , wl = 0, wr = 0 ;
int w = wl + wr ;

int Vin = PS3.getAnalogHat(LeftHatY);

int WLin = PS3.getAnalogButton(L2);
int WRin = PS3.getAnalogButton(R2);

if (PS3.getButtonClick(TRIANGLE)){

digitalWrite(bluePin , HIGH);
digitalWrite(redPin, LOW);
}

if (PS3.getButtonClick(CIRCLE)){
digitalWrite(redPin , HIGH);
digitalWrite(bluePin , LOW);
}
if (PS3.getButtonClick(CROSS)){
digitalWrite(redPin , LOW);
digitalWrite(bluePin , LOW);
}

if (PS3.getButtonClick(SQUARE)){
digitalWrite(redPin , HIGH);
digitalWrite(bluePin , HIGH);
}

int Win = -WLin + WRin ;

int power1;
int power2 ;

//if(Vin < 15 )Vin = 15;

//else if (Vin > 117 && Vin < 137)Vin = 127 ;
//else if(Vin > 250)Vin = 250;

int errorVal = 5;
int kangMax = 1000 , kangMin = 0 ;

if (Vin <= 127) {

if (Vin < errorVal)Vin = errorVal;

else if (Vin > 127 - errorVal)Vin = 127 - errorVal;

Vin = map(Vin, errorVal , 127 - errorVal , -kangMax , -kangMin);

else if (Vin > 127) {

if (Vin < 127 + errorVal) Vin = 127 + errorVal;

else if (Vin > 255 - errorVal)Vin = 255 - errorVal;

Vin = map(Vin , 127 + errorVal , 255 - errorVal , kangMin , kangMax);

}
if (Win != 0) {
if (Win < -250 ) Win = -250;
else if (Win > 250) Win = 250;

Win = map(Win , -250 , 250 , -kangMax , kangMax);

if ( Vin - v < -errorVal || Vin - v > errorVal)v = Vin ;

if ( Win - w < -errorVal || Win - w > errorVal)w = Win ;

power1 = map(v - w, -2 * kangMax , 2 * kangMax , -kangMax , kangMax) ;

power2 = map(v + w, -2 * kangMax , 2 * kangMax , -kangMax , kangMax) ;
}
else {
power1 = Vin;
power2 = Vin;

int x1 = -power1;
 int x2 = -power2;

//Serial.println(Win);
Serial.println(x1);
Serial.println(x2);
//Serial.println(v);
//Serial.println(w);
delay(30);

K2.s(x2).wait();
K1.s(x1).wait();

}
}