#include "KitiBot.h"

uint8_t Ports[7][2]={{A0,0},{A1,0},{A2,0},{A3,0},{A4,4},{A5,5},{A6,6}};
IRrecv irrecv(7);
decode_results results;
long lastIRTime = 0;
char irRead = 0;

//  Default Arduino Car pin define
KitiBot::KitiBot()
{  
  PWMA = 45;             //Left Motor Speed pin (ENA)
  AIN2 = 41;             //Motor-L forward (IN1).
  AIN1 = 40;             //Motor-L backward (IN2)
  
  PWMB = 46;             //Right Motor Speed pin (ENB)
  BIN1 = 37;             //Motor-R forward (IN4)
  BIN2 = 36;             //Motor-R backward (IN3)
	
 

}

//  Default Arduino Car pin define
void KitiBot::init()
{  
	pinMode(PWMA,OUTPUT);                     
	pinMode(AIN2,OUTPUT);      
	pinMode(AIN1,OUTPUT);

	pinMode(PWMB,OUTPUT);       
	pinMode(BIN1,OUTPUT);     
	pinMode(BIN2,OUTPUT); 
	irrecv.enableIRIn();
}

//drive the left motor run forward
void KitiBot::setSpeed(char direction, int speed)
{
  int in1,in2,pwm;
  speed = speed > 255 ? 255 : speed;
  speed = speed < -255 ? -255 : speed;  
  if(direction == 2)
  {
    in1 = AIN1;
    in2 = AIN2;
    pwm = PWMA;
  }
  else if(direction == 1)
  {
     in1 = BIN1;
     in2 = BIN2;
     pwm = PWMB;
  }
  if(speed >= 0)
  {
    digitalWrite(in2,LOW);
    digitalWrite(in1,HIGH);
    analogWrite(pwm,speed);
  }
  else
  {    
    digitalWrite(in2,HIGH);
    digitalWrite(in1,LOW);
    analogWrite(pwm,-speed);
  }
}

void KitiBot::move(char direction, int speed)
{
	int leftSpeed = 0;
	int rightSpeed = 0;
	if(direction == 1){
		leftSpeed = speed;
		rightSpeed = speed;
	}else if(direction == 2){
		leftSpeed = -speed;
		rightSpeed = -speed;
	}else if(direction == 3){
		leftSpeed = -speed;
		rightSpeed = speed;
	}else if(direction == 4){
		leftSpeed = speed;
		rightSpeed = -speed;
	}
	setSpeed(1,rightSpeed);
	setSpeed(2,leftSpeed);
}

void KitiBot::moveDelay(char direction, int speed,char time)
{
	move(direction,speed);
	delay(time*1000);
	move(0,0);
}

float KitiBot::distance(char port)
{
    int TRIG = Ports[port][0];
    int ECHO = Ports[port][1];
    pinMode(ECHO, INPUT);    // Define the ultrasonic echo input pin
    pinMode(TRIG, OUTPUT);   // Define the ultrasonic trigger input pin   
    
    digitalWrite(TRIG, LOW);   // set trig pin low 2μs
    delayMicroseconds(2);
    digitalWrite(TRIG, HIGH);  // set trig pin 10μs , at last 10us 
    delayMicroseconds(10);
    digitalWrite(TRIG, LOW);    // set trig pin low
    noInterrupts();
    float Fdistance = pulseIn(ECHO, HIGH,20000);  // Read echo pin high level time(us)
    interrupts();
    return Fdistance/58;       //Y m=（X s*344）/2
}

void KitiBot::loop()
{
	if (irrecv.decode(&results))
	{
		if(results.value & 0xFF + (results.value >> 8) & 0xFF == 0xFF)
		{
			irRead = (results.value>>8)& 0xFF;
		}
		lastIRTime = millis();
		irrecv.resume(); // Receive the next value
	}
}

boolean KitiBot::getIR(unsigned char r)
{
	loop();
	if(r == 0){
		if(millis()-lastIRTime>200){
			return 1;
		}else{
			return 0;
		}
	}
	else{
		if(millis()-lastIRTime>200){
			return 0;
		}
		if(irRead==r){
			irRead = 0;
			return 1;
		}else{
			return 0;
		}
	}
}
