/*
 * eGizmo_PBot_Uno.cpp - eGizmo_PBot_Uno library
 * Copyright (c) 2020 e-Gizmo Mechatronix Central Inc
 * All right reserved.
 */

#include "eGizmo_PBot_Uno.h"
#include "Wire.h"

eGizmo_PBot_Uno::eGizmo_PBot_Uno(){

  this->SETSERVO1();
  this->SETSERVO2();
  this->SETSERVO3();
  this->SETSERVO4();
}

/*
 * Public Methods
 */
// Functions available in Wiring sketches, this library, and other libraries
void eGizmo_PBot_Uno::BEGIN(){

  Wire.begin();
  eGizmo_PBot_Uno::ALLON();
  Serial.begin(9600);
  pinMode(TRIGGER,OUTPUT);
  pinMode(ECHO,INPUT);
}

/*
 * Sensors
 */

// Read collision sensors
// input and output: sensor varaiable name (type struct csense)
boolean eGizmo_PBot_Uno::COL1_RIGHT() {

  return (twi_read(25, TWI_READCOL) & 0x01);
}

boolean eGizmo_PBot_Uno::COL2_CENTER() {
	
return (twi_read(25, TWI_READCOL) & 0x02);
}

boolean eGizmo_PBot_Uno::COL3_LEFT() {

  return (twi_read(25, TWI_READCOL) & 0x04);
}


// Read line sensors
// input and output: sensor varaiable name (type struct lsense)
boolean eGizmo_PBot_Uno::LS1_LEFT() {

  return (twi_read(25, TWI_READLINE) & 0x01);
}

boolean eGizmo_PBot_Uno::LS2_CENTER() {

  return (twi_read(25, TWI_READLINE) & 0x02);
}

boolean eGizmo_PBot_Uno::LS3_RIGHT() {

  return (twi_read(25, TWI_READLINE) & 0x04);
}

// Distance Sensor
// Read distance value
void eGizmo_PBot_Uno::DISTANCE(void)
{
  digitalWrite(TRIGGER, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER, HIGH);
  delayMicroseconds(5);
  digitalWrite(TRIGGER,LOW);
  //pinMode(_pin,INPUT);
  DURATION = pulseIn(ECHO,HIGH);
  RANGE = DURATION/29/2; 
}

/*
 * PS2MOTORCONTROL in Left Joystick
 */
/*
void eGizmo_PBot_Uno::PS2MOTORCONTROL(uint8_t dataX, uint8_t dataY){
	//TODO: INVERT Y AXIS

	if(dataY > IDLEJSVALUE){
		//REVERSE
			this->DIRECTION(MOTOR_BOTH, MOTOR_REV);

			if(dataX > IDLEJSVALUE){
				//RIGHT
					//this->SPEED(MOTOR_B, ( (map(dataX, IDLEJSVALUE+1, MAXJSVALUE, MINMOTORSPEED, MAXMOTORSPEED)) - (map(dataY, MAXJSVALUE, IDLEJSVALUE+1, MINMOTORSPEED, MAXMOTORSPEED) ) ));
					this->SPEED(MOTOR_B, ( (map(dataX, IDLEJSVALUE+1, MAXJSVALUE, MINMOTORSPEED, MAXMOTORSPEED)) * (map(dataY, MAXJSVALUE, IDLEJSVALUE+1, MINMOTORSPEEDPRECENT, 100)/100 ) ));
					this->SPEED(MOTOR_A, map(dataY, IDLEJSVALUE+1, MAXJSVALUE, MINMOTORSPEED, MAXMOTORSPEED));

			}else if(dataX < IDLEJSVALUE){
				//LEFT
					this->SPEED(MOTOR_B, map(dataY, IDLEJSVALUE+1, MAXJSVALUE, MINMOTORSPEED, MAXMOTORSPEED));
          this->SPEED(MOTOR_A, ((map(dataX, IDLEJSVALUE-1, MINJSVALUE, MINMOTORSPEED, MAXMOTORSPEED)) * (map(dataY, MAXJSVALUE, IDLEJSVALUE+1, MINMOTORSPEEDPRECENT, 100)/100 ) ));
					//this->SPEED(MOTOR_A, ((map(dataX, IDLEJSVALUE-1, MINJSVALUE, MINMOTORSPEED, MAXMOTORSPEED)) - (map(dataY, MAXJSVALUE, IDLEJSVALUE+1, MINMOTORSPEED, MAXMOTORSPEED))));

			}else{
				//X == IDLEJSVALUE
					this->SPEED(MOTOR_BOTH, map(dataY, IDLEJSVALUE+1, MAXJSVALUE, MINMOTORSPEED, MAXMOTORSPEED));
			}

	}else if(dataY < IDLEJSVALUE){
		//FORWARD
			this->DIRECTION(MOTOR_BOTH, MOTOR_FWD);
			if(dataX > IDLEJSVALUE){
				//RIGHT
          this->SPEED(MOTOR_A, map(dataY, IDLEJSVALUE-1, MINJSVALUE, MINMOTORSPEED, MAXMOTORSPEED));
          this->SPEED(MOTOR_B, ( (map(dataX, IDLEJSVALUE+1, MAXJSVALUE, MINMOTORSPEED, MAXMOTORSPEED)) * (map(dataY, MINJSVALUE, IDLEJSVALUE-1, MINMOTORSPEEDPRECENT, 100)/100 ) ));
          //this->SPEED(MOTOR_B, ((map(dataX, IDLEJSVALUE+1, MAXJSVALUE, MINMOTORSPEED, MAXMOTORSPEED)) - (map(dataY, MINJSVALUE, IDLEJSVALUE-1, MINMOTORSPEED, MAXMOTORSPEED))));

			}else if(dataX < IDLEJSVALUE){
				//LEFT
          //this->SPEED(MOTOR_A, ((map(dataX, IDLEJSVALUE-1, MINJSVALUE, MINMOTORSPEED, MAXMOTORSPEED)) - (map(dataY, MINJSVALUE, IDLEJSVALUE-1, MINMOTORSPEED, MAXMOTORSPEED))));
          this->SPEED(MOTOR_A, ((map(dataX, IDLEJSVALUE-1, MINJSVALUE, MINMOTORSPEED, MAXMOTORSPEED)) * (map(dataY, MINJSVALUE, IDLEJSVALUE-1, MINMOTORSPEEDPRECENT, 100)/100 ) ));
  				this->SPEED(MOTOR_B, map(dataY, IDLEJSVALUE-1, MINJSVALUE, MINMOTORSPEED, MAXMOTORSPEED));

			}else{
				//X == IDLEJSVALUE
					this->SPEED(MOTOR_BOTH, map(dataY, IDLEJSVALUE-1, MINJSVALUE, MINMOTORSPEED, MAXMOTORSPEED));
			}

	}else{
		//IDLE Y== IDLEJSVALUE
			if(dataX > IDLEJSVALUE){
				//RIGHT
					this->DIRECTION(MOTOR_A, MOTOR_FWD);
					this->DIRECTION(MOTOR_B, MOTOR_REV);

					this->SPEED(MOTOR_BOTH, map(dataX, IDLEJSVALUE+1, MAXJSVALUE, MINMOTORSPEED, MAXMOTORSPEED));

				}else if(dataX < IDLEJSVALUE){
				//LEFT
					this->DIRECTION(MOTOR_A, MOTOR_REV);
					this->DIRECTION(MOTOR_B, MOTOR_FWD);

					this->SPEED(MOTOR_BOTH, map(dataX, IDLEJSVALUE-1, MINJSVALUE, MINMOTORSPEED, MAXMOTORSPEED));

			}else{
				//X == IDLEJSVALUE
					this->STOP(); //STOP
			}

	}

}
*/

/*
 * "Please do not modify or make any changes on this functions"
 * TWI driver for the MBOT Controller
 * MBOT device id device = 25
 * command : Command type (see command defines)
 * parameter : command parameter as may be required
 * parameter type  partype : P_NONE - no parameter
 *							  P_BYTE - byte type parameter
 *							  P_INT  - integer type parameter
 */

// Send command and parameter to MBOT controller
void eGizmo_PBot_Uno::twi_send(uint8_t device, uint8_t command, uint16_t parameter, int8_t partype) {

  Wire.beginTransmission(device);
  Wire.write(uint8_t(command));
  // 16 bit (integer) transfer, send data in two byte wide parts
  if (partype == P_INT) {
	Wire.write(highByte(parameter));  // high byte
	Wire.write(lowByte(parameter));   // low byte
  }
  if (partype == P_BYTE) {
	Wire.write((parameter));
  }
  Wire.endTransmission();             // close transfer
}

// Read command
byte eGizmo_PBot_Uno::twi_read(int device, byte command) {

  // send command
  Wire.beginTransmission(device);
  Wire.write(uint8_t(command));
  Wire.endTransmission();
  delayMicroseconds(1);
  // read MBOT response
  Wire.requestFrom(device, 1);
  Wire.read();
}