#include <eGizmo_PS2Controller.h>
#include <SoftwareSerial.h>


byte serial_state = SER_SYNC1;
byte serial_rxcounter = 0;
bool serial_available = false;  //true when data packet is available

eGizmo_PS2Controller::eGizmo_PS2Controller(SoftwareSerial *ss){
	hwSerial = NULL;
	swSerial = ss;
	mySerial = swSerial;
}

eGizmo_PS2Controller::eGizmo_PS2Controller(HardwareSerial *hs){
	swSerial = NULL;
	hwSerial = hs;
	mySerial = hwSerial;
}


void eGizmo_PS2Controller::begin(uint32_t baudrate){

	if (hwSerial) hwSerial->begin(baudrate);

	if (swSerial) swSerial->begin(baudrate);
}

void eGizmo_PS2Controller::end(void){
	if (hwSerial) hwSerial->end();

	if (swSerial) swSerial->end();
}


void eGizmo_PS2Controller::fetch(void){
	while(mySerial->available()){
		//get new bytes
		byte inChar = (byte)mySerial->read();

		//do stuff specific to what is received
		switch(serial_state){
			case SER_SYNC1:
				//Serial.println(F("SER_SYNC1"));
				//Check if first byte is valid
				serial_rxcounter = 0;
				if(inChar == 0x5A) serial_state = SER_SYNC2;
			break;

			case SER_SYNC2:
				//Serial.println(F("SER_SYNC2"));
				//Check if second byte is valid, reset to find first byte if failed
				if(inChar == 0xA5){
					serial_state = SER_FETCHDATA;
				}else{
					serial_state = SER_SYNC1;
				}
			break;

			case SER_FETCHDATA:
				//Serial.println(F("RECORDING"));

				//First and Second byte is valid, proceed to record data
				inputData[serial_rxcounter] = inChar;
				serial_rxcounter++;

				//if processed more than or equal to 10 bytes
				if(serial_rxcounter >= 10){
					//verify checksum

					//CHECKSUM
					int i;
					byte chk = 0x5A + 0xA5;

					for(i = 0; i < 9; i++){
						chk += inputData[i];
					}
					//CHECKSUM


					if(chk == inputData[9]){
					//Serial.println(F("VALID"));
						//checksum valid, proceed with data recorded
						serial_state = SER_SYNC1;
						serial_rxcounter = 0;
						//EXP1A
						serial_available = true;
					}else{
						//Serial.println(F("BAD"));
						//bad checksum, discard data
						serial_state = SER_SYNC1;

						/*EXP 1A
						for(i = 0; i < DATALENGTH; i++){
							inputData[i] = 0;
						}

						*/
						serial_available = false;
					}//else

				}//if

			break;
		}//switch

	}//while
}

bool eGizmo_PS2Controller::update(void){
	this->fetch();
	if(serial_available){
		this->up		=	!(inputData[3]&0x08);
		this->down		=	!(inputData[3]&0x02);
		this->left		=	!(inputData[3]&0x01);
		this->right	=	!(inputData[3]&0x04);

		this->triangle	=	!(inputData[4]&0x08);
		this->circle	=	!(inputData[4]&0x04);
		this->cross		=	!(inputData[4]&0x02);
		this->square	=	!(inputData[4]&0x01);

		this->left1		=	!(inputData[4]&0x20);
		this->left2		=	!(inputData[4]&0x80);

		this->right1	=	!(inputData[4]&0x10);
		this->right2	=	!(inputData[4]&0x40);

		this->start		=	!(inputData[3]&0x10);

		this->leftx 	= inputData[7];
		this->lefty 	= inputData[8];

		this->rightx 	= inputData[5];
		this->righty 	= inputData[6];

		return true;
	}else{

		this->up		=	false;
		this->down		=	false;
		this->left		=	false;
		this->right		=	false;

		this->triangle	=	false;
		this->circle	=	false;
		this->cross		=	false;
		this->square	=	false;

		this->left1		=	false;
		this->left2		=	false;

		this->right1	=	false;
		this->right2	=	false;

		this->start		=	false;

		//this->leftx	=	DEFAULTIDLEJSDATA;
		//this->lefty	=	DEFAULTIDLEJSDATA;

		//this->rightx	=	DEFAULTIDLEJSDATA;
		//this->righty	=	DEFAULTIDLEJSDATA;
		return false;
	}

}