#include "Device.h"
#include "PCInterrupt.h"
#include "irController.h"
#include "myHardware.h"

#include <Servo.h> 
#include <ArduMoto.h>
#include <NewPing.h>
int key;
IRController controller;

const int mindist=15;
const int pingpin=4;  // Arduino pin tied to trigger pin on the ultrasonic sensor.
const int maxdist=400 ;// Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
Servo servoping;
NewPing sonar(pingpin, pingpin, maxdist); // NewPing setup of pins and maximum distance.

ArduMoto Moto;  //создаем объект для работы с моторами

const float kB=0.55 ; //коэффициент для мотора B, так как он крутится быстрее
const float kA=1;
const float kAll=0.7;//общий коэффициент
const int delayServo=200;
int distcmAhead;
int distcmRight;
int distcmLeft;


void setup()
{

  servoping.attach(A0);
  Moto.begin();//подключаем моторы
  Moto.setSpeed('A',0);//останавливаем моторы на всякий случай
  Moto.setSpeed('B',0);
  servoping.write(90);
  Serial.begin(9600);
  int res = controller.begin(IR_PIN, OTHER_DEVICE);
  forward(0);
}

void stopall(){
  forward(0);
}

int wheretogo()
{
  if( (distcmLeft>=distcmRight)&&(distcmLeft>mindist)||(distcmLeft=0))//можем ли мы ехать налево
  {
    return 1;//go left     
  }

  else {
    if( (distcmLeft<distcmRight)&&(distcmRight>mindist)||(distcmRight=0))////можем ли мы ехать направо
    {
      return 2;//go right
    }
    else{
      return 3;//go back
    }
  }
}

int lookahead()
{
  servoping.write(90);
  delay(delayServo);
  int distcm=sonar.ping_cm();
  return distcm;
}

int lookleft(){
  servoping.write(180);
  delay(delayServo*2);
  int distcm=sonar.ping_cm();
  return distcm;

}
int lookright(){
  servoping.write(0);
  //SoftwareServo::refresh();
  delay(delayServo*2);
  int distcm=sonar.ping_cm();
  return distcm;
}

void left(int velocity)
{
  Moto.setSpeed('A',velocity*kAll*kA);
  Moto.setSpeed('B',-velocity*kAll*kB);
}

void right(int velocity)
{
  Moto.setSpeed('A',-velocity*kAll*kA);
  Moto.setSpeed('B',velocity*kAll*kB);
}


void forward(int velocity)
{
  Serial.println('go forward');
  Moto.setSpeed('A',velocity*kAll*kA);
  Moto.setSpeed('B',velocity*kAll*kB);
}

void backward(int velocity)
{
  Moto.setSpeed('A',-velocity*kAll*kA);
  Moto.setSpeed('B',-velocity*kAll*kB);
}
void forwardleft(int velocity)
{
  Moto.setSpeed('A',velocity*kAll*kA);
  Moto.setSpeed('B',0.4*velocity*kAll*kB);
}
void forwardright(int velocity)
{
  Moto.setSpeed('B',velocity*kAll*kB);
  Moto.setSpeed('A',0.4*velocity*kAll*kA);
}
void backwardright(int velocity){
  Moto.setSpeed('B',-velocity*kAll*kB);
  Moto.setSpeed('A',-0.4*velocity*kAll*kA);
}
void backwardleft(int velocity){
  Moto.setSpeed('A',-velocity*kAll*kA);
  Moto.setSpeed('B',-0.4*velocity*kAll*kB);
}

void loop() {     
  int dir;
  distcmAhead=lookahead();
  if( (distcmAhead<=mindist)&&(distcmAhead>0)){
    stopall();
label1:   
    distcmLeft=lookleft();
    distcmRight=lookright();
    dir=wheretogo();
    switch (dir){
    case 1://едем налево
      left(100);
      delay(500);
      forward(100);
      break;
    case 2://едем направо
      right(100);
      delay(500);
      forward(100);
      break;
    case 3:
      backward(100);  
      do{
        distcmLeft=lookleft();
        distcmRight=lookright();
      }
      while( (distcmLeft<mindist)&&(distcmRight<mindist)&&(distcmRight>0)&&(distcmLeft>0));
      goto label1; 
    }
  } 


  key = controller.read();
  if (key>=0){//если получили сигнал то надо что-то сделать
    switch (key){
    case  16128://едем вперед
      forward(100);
      break;
    case 16192:
      backward(100);
      break;
    case   14726:
      forward(0);
      break;
    case  15427 :
      forwardleft(100);
      break;
    case   14662:
      forwardright(100);
      break;
    case   16256:
      backwardright(100);
      break;
    case  10200   :
      backwardleft(100);
      break;
    case  15746 :
      left(100);
      break;
    case  14471  :
      right(100);
      break;
    }
  }
}