// include libraries
#include <PS2X_lib.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <Ultrasonic.h>


Ultrasonic ultrasonic(2,3); // (Trig PIN,Echo PIN)
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates


#define BRAKEVCC 0                  // определяем значение резкого тормоза
#define CW   1                      // определяем значение вращения по часовой стрелке
#define CCW  2                      // определяем значение вращения против часовой стрелки
#define BRAKEGND 3                  // определяем значение остановки

const uint8_t inApin[2] = {7, 4};   // определяем выводы ключей A
const uint8_t inBpin[2] = {8, 9};   // определяем выводы ключей B
const uint8_t pwmpin[2] = {5, 6};   // определяем выводы ШИМ
const uint8_t cspin[2]  = {A2, A3}; // определяем выводы считывания тока
const uint8_t enpin[2]  = {A0, A1}; // определяем выводы состояния ключей AB. Ключи открываются, если притянуть к 0.

char thisChar;
byte availableBytes;
String strData = "";
boolean recievedFlag;
int motor1, motor2, delay1;
int servo1, servo2, servo3;

uint8_t servonum1 = 0;
uint8_t servonum2 = 1;
uint8_t servonum3 = 2;



PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning
//you must always either restart your Arduino after you conect the controller,
//or call config_gamepad(pins) again after connecting the controller.
int error = 0;
byte type = 0;
byte vibrate = 0;

void setup(){

   for (int i=0; i<2; i++)
  {
    pinMode(inApin[i], OUTPUT);     // выводы ключей A
    pinMode(inBpin[i], OUTPUT);     // выводы ключей B
    pinMode(pwmpin[i], OUTPUT);     // выводы ШИМ
  }
  // инициируем моторы выключенными
  motorOff(0);
  motorOff(1);

 // Start serial communication
 Serial.begin(57600);

 delay(2000);

 error = ps2x.config_gamepad(13,11,10,12, true, true);   //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error

  // Check for error
 if(error == 0){
  Serial.println("Found Controller, configured successful");
 }

 else if(error == 1)
  Serial.println("No controller found, check wiring or reset the Arduino");

 else if(error == 2)
  Serial.println("Controller found but not accepting commands");

 else if(error == 3)
  Serial.println("Controller refusing to enter Pressures mode, may not support it.");

 // Check for the type of controller
 type = ps2x.readType();
 switch(type) {
  case 0:
    Serial.println("Unknown Controller type");
    break;
  case 1:
    Serial.println("DualShock Controller Found");
    break;
  case 2:
    Serial.println("GuitarHero Controller Found");
    break;
  }

 pwm.begin();

 pwm.setOscillatorFrequency(27000000);
 pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

 delay(10);
}

void motorOff(int motor)
{
  for (int i=0; i<2; i++)
  {
    digitalWrite(inApin[i], LOW);
    digitalWrite(inBpin[i], LOW);
  }
  analogWrite(pwmpin[motor], 0);
}


// функция включения мотора:
void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm)
{
// если номер мотора правильный:
  if (motor <= 1)
  {
// если направление совпадает со значениями направлений:
    if (direct <=3)
    {
// если направление мотора по часовой или плавный стоп,
// устанавливаем соответствующие значения ключа А выбранного мотора:
      if (direct <=1)
        digitalWrite(inApin[motor], HIGH);
      else
        digitalWrite(inApin[motor], LOW);

// если направление мотора по часовой или резкий стоп,
// устанавливаем соответствующие значения ключа B выбранного мотора:
      if ((direct==0)||(direct==2))
        digitalWrite(inBpin[motor], HIGH);
      else
        digitalWrite(inBpin[motor], LOW);
// устанавливаем ШИМ выбранного мотора
      analogWrite(pwmpin[motor], pwm);
    }
  }
}
// Main loop
void loop(){
Serial.print("L");
Serial.println(ultrasonic.Ranging(CM)); // CM or INC

if (Serial.available() > 0) {                 // если есть что то на вход
strData = "";                               // очистить строку
while (Serial.available() > 0) {            // пока идут данные
strData += (char)Serial.read();           // забиваем строку принятыми данными
delay(2);                                 // обязательно задержка, иначе вылетим из цикла раньше времени
}
recievedFlag = true;  // поднять флаг что получили данные
//Serial.print(strData);
}
if (recievedFlag) {                           // если есть принятые данные
int intVal = strData.substring(2).toInt();  // перевести в int всю строку кроме первых двух символов!
String header = strData.substring(0, 2);    // создать мини строку, содержащую первые два символа
if (strData.startsWith("m1"))               // если строка начинается с m1
motor1 = intVal;
if (strData.startsWith("m2"))               // если строка начинается с m2
motor2 = intVal;
if (strData.startsWith("m3"))               // если строка начинается с m3
{
  motor1 = intVal;
  motor2 = intVal;
}
if (strData.startsWith("m4"))               // если строка начинается с m3
{
  motor1 = intVal;
  motor2 = -intVal;
}
if (strData.startsWith("d1"))               // если строка начинается с m1
delay1 = intVal;
if (strData.startsWith("s1"))               // если строка начинается с b1
{
  servo1 = intVal;
  if (servo1>SERVOMIN && servo1<543)
  {
  Serial.println(servo1);
  pwm.setPWM(servonum1, 0, servo1);
  }
}
if (strData.startsWith("s2"))               // если строка начинается с b2
{
  servo2 = intVal;
  if (SERVOMIN<servo2 && servo2<543)
  {
  Serial.println(servo2);
  pwm.setPWM(servonum2, 0, servo2);
  }

}
if (strData.startsWith("s3"))               // если строка начинается с b2
{
  servo3 = intVal;
  if (SERVOMIN<servo3 && servo3<543)
  {
  Serial.println(servo3);
  pwm.setPWM(servonum3, 0, servo3);
  }

}
if (strData.startsWith("stop"))               // если строка начинается с b2
{
  motorOff(0);
  motorOff(1);
  }

}
recievedFlag = false;                       // данные приняты, мой капитан!
// выводим в порт для отладки



     if(motor1>50 && motor2==0) {
        motorGo(0, CW, 255);
        delay(delay1);
        motor1 = 0;
        delay1 = 0;
     }
     if(motor1<-50 && motor2==0) {
        motorGo(0, CCW, 255);
        delay(delay1);
        motor1 = 0;
        delay1 = 0;
     }
     if(motor2>50 && motor1==0) {
        motorGo(1, CW, 255);
        delay(delay1);
        motor2 = 0;
        delay1 = 0;
     }
     if(motor2<-50 && motor1==0){
        motorGo(1, CCW, 255);
        delay(delay1);
        motor2 = 0;
        delay1 = 0;
     }
     if(motor2<-50 && motor1<-50){
        motorGo(0, CCW, 255);
        motorGo(1, CW, 255);
        delay(delay1);
        motor1 = 0;
        motor2 = 0;
        delay1 = 0;
     }
      if(motor2>50 && motor1>50){
        motorGo(0, CW, 255);
        motorGo(1, CCW, 255);
        delay(delay1);
        motor1 = 0;
        motor2 = 0;
        delay1 = 0;
     }
      if((motor2>50 && motor1<-50) or (motor2<-50 && motor1>50)){
        if (motor1>0)
          {motorGo(0, CW, 125);
          motorGo(1, CW, 125);}
        if (motor1<0)
          {motorGo(0, CCW, 125);
          motorGo(1, CCW, 125);}
        delay(delay1);
        motor1 = 0;
        motor2 = 0;
        delay1 = 0;
     }

 if(error == 1)
 {//skip loop if no controller found
  return;
 }
 else { //DualShock Controller

     ps2x.read_gamepad(false, vibrate); // disable vibration of the controller

    int nJoyLY = ps2x.Analog(PSS_LY); // read left stick
    int nJoyRY = ps2x.Analog(PSS_RY); // read right stick
    int nJoyLX = ps2x.Analog(PSS_LX); // read left stick
    int nJoyRX = ps2x.Analog(PSS_RX); // read right stick

    nJoyLY = map(nJoyLY, 0, 255, 255, -255);
    nJoyRY = map(nJoyRY, 0, 255, 255, -255);
    nJoyLX = map(nJoyLX, 0, 255, -255, 255);
    nJoyRX = map(nJoyRX, 0, 255, -255, 255);

     //вперед
     if(nJoyLY>50 && abs(nJoyLX)<50) {
        motorGo(0, CCW, 255);
        motorGo(1, CW, 255);
     }
     //назад
     if(nJoyLY<-50 && abs(nJoyLX)<50) {
        motorGo(0, CW, 255);
        motorGo(1, CCW, 255);
     }
     //вправо
     if(nJoyLX>50 && abs(nJoyLY)<50) {
        motorGo(0, CW, 125);
        motorGo(1, CW, 125);
     }
     //влево
     if(nJoyLX<-50 && abs(nJoyLY)<50) {
        motorGo(0, CCW, 125);
        motorGo(1, CCW, 125);
     }
     if (abs(nJoyLX)<50 && abs(nJoyLY)<50) {
        analogWrite(pwmpin[1], 0);
        analogWrite(pwmpin[0], 0);
     }
//     if(nJoyRY>50 && nJoyRX<50){
//      pwm.setPWM(servonum2, 0, 500);
//      pwm.setPWM(servonum3, 0, 540);
//     }
//      if(nJoyRY>50 && nJoyRX>50){
//      pwm.setPWM(servonum2, 0, 500);
//      pwm.setPWM(servonum3, 0, 160);
//     }
//      if(nJoyRY<50 && nJoyRX<50){
//      pwm.setPWM(servonum2, 0, 250);
//      pwm.setPWM(servonum3, 0, 540);
//     }
//      if(nJoyRY<50 && nJoyRX>50){
//      pwm.setPWM(servonum2, 0, 250);
//      pwm.setPWM(servonum3, 0, 160);
//     }
//     if (abs(nJoyRX)<50 && abs(nJoyRY)<50) {
//      pwm.setPWM(servonum2, 0, 400);
//      pwm.setPWM(servonum3, 0, 350);
//     }


 delay(50);


 if(ps2x.Button(PSB_PAD_RIGHT)){

  pwm.setPWM(servonum1, 0, 151);
  }
      // TURN LEFT
 if(ps2x.Button(PSB_PAD_LEFT)){

  pwm.setPWM(servonum1, 0, 542);
  }
   if(ps2x.Button(PSB_PAD_UP)){

  pwm.setPWM(servonum1, 0, 346);
  }


 if(ps2x.Button(PSB_GREEN)){

  pwm.setPWM(servonum2, 0, 500);
  }
   if(ps2x.Button(PSB_BLUE)){

  pwm.setPWM(servonum2, 0, 250);
  }
   if(ps2x.Button(PSB_PINK)){

  pwm.setPWM(servonum3, 0, 540);
  }
   if(ps2x.Button(PSB_RED)){

  pwm.setPWM(servonum3, 0, 160);
  }
  if(ps2x.Button(PSB_R1)){

  pwm.setPWM(servonum3, 0, 350);
  pwm.setPWM(servonum2, 0, 400);
  }







// Serial.print("nJoyRX=");
//// Serial.println(ps2x.Analog(PSS_RY), DEC);
// Serial.println(nJoyRX);
//  Serial.print("nJoyRY=");
//// Serial.println(ps2x.Analog(PSS_RY), DEC);
// Serial.println(nJoyRY);
//  Serial.print("nJoyLX=");
//// Serial.println(ps2x.Analog(PSS_RY), DEC);
// Serial.println(nJoyLX);
//  Serial.print("nJoyLY=");
//// Serial.println(ps2x.Analog(PSS_RY), DEC);
// Serial.println(nJoyLY);


//Serial.print("motor1= "); Serial.print(motor1); Serial.print(" ");
//Serial.print("delay1= "); Serial.print(delay1); Serial.print(" ");
//Serial.print("motor2= "); Serial.print(motor2); Serial.print(" ");
//Serial.print("servo1= "); Serial.print(servo1); Serial.print(" ");
//Serial.print("servo2= "); Serial.print(servo2); Serial.print(" ");
//Serial.print("servo3= "); Serial.print(servo3); Serial.println();



}
}