#include "ArduinoRobotMotorBoard.h"
#include "EasyTransfer2.h"
#include "Multiplexer.h"
#include "LineFollow.h"

RobotMotorBoard::RobotMotorBoard(){
	//LineFollow::LineFollow();
}
/*void RobotMotorBoard::beginIRReceiver(){
	IRrecv::enableIRIn();
}*/
void RobotMotorBoard::begin(){
	//initialze communication
	Serial1.begin(9600);
	messageIn.begin(&Serial1);
	messageOut.begin(&Serial1);

	//init MUX
	uint8_t MuxPins[]={MUXA,MUXB,MUXC};
	this->IRs.begin(MuxPins,MUX_IN,3);
	pinMode(MUXI,INPUT);
	digitalWrite(MUXI,LOW);
	
	isPaused=false;
}

void RobotMotorBoard::process(){
	if(isPaused)return;//skip process if the mode is paused
	
	if(mode==MODE_SIMPLE){
		//Serial.println("s");
		//do nothing? Simple mode is just about getting commands
	}else if(mode==MODE_LINE_FOLLOW){
		//do line following stuff here.
		LineFollow::runLineFollow();
	}else if(mode==MODE_ADJUST_MOTOR){
		//Serial.println('a');
		//motorAdjustment=analogRead(POT);
		//setSpeed(255,255);
		//delay(100);
	}
}
void RobotMotorBoard::pauseMode(bool onOff){
	if(onOff){
		isPaused=true;
	}else{
		isPaused=false;
	}
	stopCurrentActions();

}
void RobotMotorBoard::parseCommand(){
	uint8_t modeName;
	uint8_t codename;
	int value;
	int speedL;
	int speedR;
	if(this->messageIn.receiveData()){
		//Serial.println("data received");
		uint8_t command=messageIn.readByte();
		//Serial.println(command);
		switch(command){
			case COMMAND_SWITCH_MODE:
				modeName=messageIn.readByte();
				setMode(modeName);
				break;
			case COMMAND_RUN:
				if(mode==MODE_LINE_FOLLOW)break;//in follow line mode, the motor does not follow commands
				speedL=messageIn.readInt();
				speedR=messageIn.readInt();
				motorsWrite(speedL,speedR);
				break;
			case COMMAND_MOTORS_STOP:
				motorsStop();
				break;
			case COMMAND_ANALOG_WRITE:
				codename=messageIn.readByte();
				value=messageIn.readInt();
				_analogWrite(codename,value);
				break;
			case COMMAND_DIGITAL_WRITE:
				codename=messageIn.readByte();
				value=messageIn.readByte();
				_digitalWrite(codename,value);
				break;
			case COMMAND_ANALOG_READ:
				codename=messageIn.readByte();
				_analogRead(codename);
				break;
			case COMMAND_DIGITAL_READ:
				codename=messageIn.readByte();
				_digitalRead(codename);
				break;
			case COMMAND_READ_IR:
				_readIR();
				break;
			case COMMAND_READ_TRIM:
				_readTrim();
				break;
			case COMMAND_PAUSE_MODE:
				pauseMode(messageIn.readByte());//onOff state
				break;
			case COMMAND_LINE_FOLLOW_CONFIG:
				LineFollow::config(
					messageIn.readByte(),	//KP
					messageIn.readByte(),	//KD
					messageIn.readByte(),	//robotSpeed
					messageIn.readByte()	//IntegrationTime
				);
				break;
		}
	}
	//delay(5);
}
uint8_t RobotMotorBoard::parseCodename(uint8_t codename){
	switch(codename){
		case B_TK1:
			return TK1;
		case B_TK2:
			return TK2;
		case B_TK3:
			return TK3;
		case B_TK4:
			return TK4;
	}
}
uint8_t RobotMotorBoard::codenameToAPin(uint8_t codename){
	switch(codename){
		case B_TK1:
			return A0;
		case B_TK2:
			return A1;
		case B_TK3:
			return A6;
		case B_TK4:
			return A11;
	}
}

void RobotMotorBoard::setMode(uint8_t mode){
	if(mode==MODE_LINE_FOLLOW){
		LineFollow::calibIRs();
	}
	/*if(mode==SET_MOTOR_ADJUSTMENT){
			save_motor_adjustment_to_EEPROM();
		}
	*/
	/*if(mode==MODE_IR_CONTROL){
		beginIRReceiver();
	}*/
	this->mode=mode;
	//stopCurrentActions();//If line following, this should stop the motors
}

void RobotMotorBoard::stopCurrentActions(){
	motorsStop();
	//motorsWrite(0,0);
}

void RobotMotorBoard::motorsWrite(int speedL, int speedR){
	/*Serial.print(speedL);
	Serial.print(" ");
	Serial.println(speedR);*/
	//motor adjustment, using percentage
	_refreshMotorAdjustment();
	
	if(motorAdjustment<0){
		speedR*=(1+motorAdjustment);
	}else{
		speedL*=(1-motorAdjustment);
	}
	
	if(speedR>0){
		analogWrite(IN_A1,speedR);
		analogWrite(IN_A2,0);
	}else{
		analogWrite(IN_A1,0);
		analogWrite(IN_A2,-speedR);
	}
	
	if(speedL>0){
		analogWrite(IN_B1,speedL);
		analogWrite(IN_B2,0);
	}else{
		analogWrite(IN_B1,0);
		analogWrite(IN_B2,-speedL);
	}
}
void RobotMotorBoard::motorsWritePct(int speedLpct, int speedRpct){
	//speedLpct, speedRpct ranges from -100 to 100
	motorsWrite(speedLpct*2.55,speedRpct*2.55);
}
void RobotMotorBoard::motorsStop(){
	analogWrite(IN_A1,255);
	analogWrite(IN_A2,255);

	analogWrite(IN_B1,255);
	analogWrite(IN_B2,255);
}


/*
*
*
*	Input and Output ports
*
*
*/
void RobotMotorBoard::_digitalWrite(uint8_t codename,bool value){
	uint8_t pin=parseCodename(codename);
	digitalWrite(pin,value);
}
void RobotMotorBoard::_analogWrite(uint8_t codename,int value){
	//There's no PWM available on motor board
}
void RobotMotorBoard::_digitalRead(uint8_t codename){
	uint8_t pin=parseCodename(codename);
	bool value=digitalRead(pin);
	messageOut.writeByte(COMMAND_DIGITAL_READ_RE);
	messageOut.writeByte(codename);
	messageOut.writeByte(value);
	messageOut.sendData();
}
void RobotMotorBoard::_analogRead(uint8_t codename){
	uint8_t pin=codenameToAPin(codename);
	int value=analogRead(pin);
	messageOut.writeByte(COMMAND_ANALOG_READ_RE);
	messageOut.writeByte(codename);
	messageOut.writeInt(value);
	messageOut.sendData();
}
int RobotMotorBoard::IRread(uint8_t num){
	return _IRread(num-1); //To make consistant with the pins labeled on the board
}

int RobotMotorBoard::_IRread(uint8_t num){
	IRs.selectPin(num); 
	return IRs.getAnalogValue();
}


void RobotMotorBoard::_readIR(){
	int value;
	messageOut.writeByte(COMMAND_READ_IR_RE);
	for(int i=0;i<5;i++){
		value=_IRread(i);
		messageOut.writeInt(value);
	}
	messageOut.sendData();
}

void RobotMotorBoard::_readTrim(){
	int value=analogRead(TRIM);
	messageOut.writeByte(COMMAND_READ_TRIM_RE);
	messageOut.writeInt(value);
	messageOut.sendData();	
}

void RobotMotorBoard::_refreshMotorAdjustment(){
	motorAdjustment=map(analogRead(TRIM),0,1023,-30,30)/100.0;
}

void RobotMotorBoard::reportActionDone(){
	setMode(MODE_SIMPLE);
	messageOut.writeByte(COMMAND_ACTION_DONE);
	messageOut.sendData();
}

RobotMotorBoard RobotMotor=RobotMotorBoard();