// Roomba632.cpp
// By Simon Jansen 2022-08-21 "Remember the pandemic?"

#include "Roomba632.h"

Roomba632::Roomba632(){
	//Publicaly set variables
	//music song to be played
	//driving instructions

	//Privately set public variables
	chargingState = chargingStateNotCharging;
	batteryVoltage = 15000;
	current = 0;
	batteryTemp = 20;
	batteryCharge = 0;
	batteryCapacity = 0;
	docked = false;
	OImode = 0; //enum for states
	encoderLeft = 0;
	encoderRight = 0;
	angle = 0; //calculated from encoders
	distance = 0; //calculated from encoders
	debugVal = 99; //resetvalue
	Xcoo = 0;
	Ycoo = 0;
	theta = 0;
	AuxYaw = 0;
	AuxXacc = 0;
	AuxYacc = 0;

	//Privately set public states. Only set and change states from within state machine.
	roombaState = roombaIdle; //enum for states
	dataState = dataIdle;
	p_undockedTimer = millis();
	//p_odometryTimer = millis();
	//p_requestStartDatastreamFlag = false;
	//int receivingDatastreamState = datastreamIdle; //enum for states
	//int sendingCommands = sendingCommandsIdle; //enum for states

	//Privately set public flags
	b_songPlaying = false;
	b_dirtDetected = false;
	//sensorTripped = false; //for bump cliff and wheel drop sensors

	//Private flags and variables
	//p_chargingFlag = false;
	p_cleanFlag = false;
	p_dockFlag = false;
	p_spotFlag = false;
	p_stopFlag = false;
	p_startScriptFlag = false;
	p_playMusicFlag = false;
	//p_driveFlag = false;

	p_encodersReady = false;

	//Private counters and timers
	//p_musicTimer = 0; //could be static?

	//Initialize serial connection to Roomba
	Serial.begin(115200);
}
//public functions for change in state set a flag (and vairables) as a 'request' whitch is handled in order of prirority
void Roomba632::stop(){
	SendStopCommand();
	p_stopFlag = true;
}
void Roomba632::clean(){
	Xcoo = 0;
	Ycoo = 0;
	theta = 0;
	p_cleanFlag = true;
}
void Roomba632::spot(){
	p_spotFlag = true;
}
void Roomba632::dock(){
	p_dockFlag = true;
}
void Roomba632::playScript(uint8_t script[]){
	//memcpy(script,script,sizeof(script));
	p_scriptPointer = script;
	p_startScriptFlag = true;
}

void Roomba632::playMusic(uint8_t song[],int songNumberOfBytes){
	//set music to be played
	p_songPointer = song;
	p_songNumberOfBytes = songNumberOfBytes;
	p_playMusicFlag = true;
}
/*
void Roomba632::drive(arguments){
	//set driving variables
	p_driveFlag = true;
}
*/

void Roomba632::setCallbackDoneCleaning(void (*callbackFuncDoneCleaning)()){
	_callbackFuncDoneCleaning = callbackFuncDoneCleaning;
}
void Roomba632::EventDoneCleaning(){
	if (_callbackFuncDoneCleaning){
		_callbackFuncDoneCleaning();
	}
}
void Roomba632::setCallbackStateChange(void (*callbackFuncStateChange)()){
	_callbackFuncStateChange = callbackFuncStateChange;
}
void Roomba632::EventStateChange(){
	if (_callbackFuncStateChange){
		_callbackFuncStateChange();
	}
}
void Roomba632::handler(){
	//state machine for high level states
	switch (roombaState){
		case roombaHome: //Charging
			//Check flags in appropriate order of prirority
			if(p_cleanFlag){
				SendCleanCommand();
			}
			else if(p_playMusicFlag){
				p_playMusicFlag=false;
				//SendPlayMusicCommand();
			}	
			if (dataState < 9){
				roombaState = roombaIdle;
				EventStateChange();
			}
			break;
		case roombaIdle: //IDLE: not charging, driving, playing music or cleaning
			p_stopFlag = false; //Clear stop request flag if set
			//Keep alive
			//Check flags in appropriate order of prirority
			if(p_cleanFlag){
				SendCleanCommand();
			}
			else if(p_spotFlag){
				SendSpotCommand();
			}
			else if(p_dockFlag){
				SendDockCommand();
			}
			else if(p_playMusicFlag){
				SendPlayMusicCommand();
			}	
			else if(p_startScriptFlag){
				roombaState = roombaPlayingScript;
				EventStateChange();
			}
			if((millis() - p_undockedTimer) > DOCKEDDEBOUNCETIME){
				if(docked){
					roombaState = roombaHome;
					EventStateChange();
				}
			}
			break;
		case roombaPlayingScript:
			if(ScriptHandler() == RMIdle){
				roombaState = roombaIdle;
				EventStateChange();
			}
			UpdateOdometry();
			break;
		case roombaCleaning:
			p_cleanFlag = false; //clear request flag
			if(p_stopFlag){
				SendStopCommand();
			}
			else if(p_dockFlag){
				SendDockCommand();
			}
			else if(p_spotFlag){
				SendSpotCommand();
			}
			else if(p_playMusicFlag){
				SendPlayMusicCommand();
			}
			if((millis() - p_undockedTimer) > DOCKEDDEBOUNCETIME){
				if(docked){
					roombaState = roombaHome;
					EventStateChange();
					EventDoneCleaning();
				}
			}
			UpdateOdometry();
			break;
		case roombaSpotCleaning:
			p_spotFlag = false; //clear request flag
			if(p_stopFlag){
				SendStopCommand();
			}
			else if(p_cleanFlag){
				SendCleanCommand();
			}
			else if(p_dockFlag){
				SendDockCommand();
			}
			else if(p_playMusicFlag){
				SendPlayMusicCommand();
			}	
			if((millis() - p_undockedTimer) > DOCKEDDEBOUNCETIME){
				if(docked){
					roombaState = roombaHome;
					EventStateChange();
				}
			}
			UpdateOdometry();
			break;
		case roombaDocking:
			p_dockFlag = false; //clear request flag
			if(p_stopFlag){
				SendStopCommand();
			}
			else if(p_cleanFlag){
				SendCleanCommand();
			}
			else if(p_spotFlag){
				SendSpotCommand();
			}
			else if(p_playMusicFlag){
				SendPlayMusicCommand();
			}	
			if((millis() - p_undockedTimer) > DOCKEDDEBOUNCETIME){
				if(docked){
					roombaState = roombaHome;
					EventStateChange();
				}
			}
			UpdateOdometry();
			break;
		case roombaDriving:
			p_driveFlag = false;
			roombaState = roombaIdle;
			EventStateChange();
			UpdateOdometry();
			break;
		case roombaMusic:
			p_playMusicFlag = false;
			b_songPlaying = true;
			//time music or poll state
			static int songPointerIndex;
			static uint8_t maxIndex, i;
			static int musicState;
			static unsigned long commandTimerMusic, musicTimer, songTime;
			switch (musicState){
				case musicInit:
					songPointerIndex = 0;
					musicState = musicWriteSongHeader;
					//Serial.write(OCPauseDatastream);
		            //Serial.write(0);
					commandTimerMusic = millis();
        			break;
				case musicWriteSongHeader:
					if ((millis() - commandTimerMusic) > COMMANDTIMEOUT){
						Serial.write(OCSong);
						Serial.write(1);
						songTime = 0;
						if ((p_songNumberOfBytes-songPointerIndex)<MAXSONGLENGTH){
							maxIndex = (p_songNumberOfBytes-songPointerIndex);
						}
						else{
							maxIndex = MAXSONGLENGTH;
						}
						Serial.write(maxIndex/2); //number of notes
						musicState = musicWriteNotes;
						i = 0;
						commandTimerMusic = millis();
					}
					break;
				case musicWriteNotes:
					if ((millis() - commandTimerMusic) > COMMANDTIMEOUT){
						if(i < maxIndex){
							Serial.write(p_songPointer[songPointerIndex]);
							if(songPointerIndex % 2 != 0){
								songTime += p_songPointer[songPointerIndex];
								if (p_songPointer[songPointerIndex] != 0){
									songTime -= 2;
								}
							}
							songPointerIndex++;
							i++;
						}
						else{
							musicState = musicStartSong;
						}
						commandTimerMusic = millis();
					}
					break;
				case musicStartSong:
					if ((millis() - commandTimerMusic) > COMMANDTIMEOUT){
						musicTimer = millis();
						songTime = (1000 * songTime)/64;
						musicState = musicPlaySong;
						Serial.write(OCPlaySong);
						Serial.write(1);
					}
					break;
				case musicPlaySong:
					if((millis() - musicTimer) > songTime){
						if(songPointerIndex >= p_songNumberOfBytes){
							musicState = musicInit;
							//Serial.println("done singing");
							roombaState = roombaIdle;
							EventStateChange();
							b_songPlaying = false;
							SendStopCommand();
							//Serial.write(OCPauseDatastream);
				            //Serial.write(1);
						}
						else {
							musicState = musicWriteSongHeader; //next song
							//Serial.println("next song");
						}
					}
					//waiting while song plays
					break;
			}
			break;
	}
	//State machine for getting data from Roomba in docked and undocked state.
	static unsigned long messagetimeouttimer, commandTimer;
	static char* pos;
	static char incomming, buffer[7];
	static char i, j, errorCounter;
	static char chargingHeaderBuffer[10], chargingMessageBuffer[80];
	static byte databuffer[30]; // 12 packetID's + 18 databytes
    static byte checksum;  
	switch (dataState){
		//Idle: try to start datastream
		case dataIdle:
			//try to start datastream
			dataState = datastreamStart;
			break;
		//Handle serial charging message
		//Example:
		//"bat:   min 16  sec 35  mV 16198  mA 1102  tenths-deg-C 223  mAH 2496  state 5"
		case chargingMessageWaitingForHeader:
		
			if(Serial.available()){
				incomming = Serial.read();
				if(incomming =='b'){
					dataState = chargingMessageReadingHeader;
					j=0;
				}
			}
			//timeout?
			else if ((millis() - messagetimeouttimer) > CHARGINGMESSAGETIMEOUT){
   				//start datastream to figure out what it's up to
				messagetimeouttimer = millis(); //reset timer
				dataState = datastreamStart;
			}
		
			break;
		case chargingMessageReadingHeader:
			if (Serial.available()){
				chargingHeaderBuffer[j] = Serial.read();
				j++;
				if (j==10){
					if (strncmp(chargingHeaderBuffer, "at:   min ",10)==0){ 
						dataState = chargingMessageReadingMessage;
		                messagetimeouttimer = millis(); //reset timer
					}
					else{
						dataState = chargingMessageWaitingForHeader;
					}
					j = 0;
				}
			}
			//timeout?
			else if ((millis() - messagetimeouttimer) > CHARGINGMESSAGETIMEOUT){
   				//start datastream to figure out what it's up to
				dataState = datastreamStart;
			}
			break;
		case chargingMessageReadingMessage:
			if(Serial.available()){
				chargingMessageBuffer[j] = Serial.read();
				if (j==79){
					j=0;
					dataState = chargingMessageReadingHeader;
				}
				if (chargingMessageBuffer[j] == '\n'){
					pos = strtok(chargingMessageBuffer, " ");
					j = 1;				
					while (pos != NULL){
						if (j== 5){ //Voltage
							strcpy(buffer, pos);
							batteryVoltage = atol(buffer);
						}
						if (j== 7){
							strcpy(buffer, pos);
							current = atol(buffer);
						}
						if (j== 9){
							strcpy(buffer, pos);
							batteryTemp = atol(buffer)/10; //Gaat nog niet goed. Is in tienden! Dus delen door 10
						}
						if (j== 11){
							strcpy(buffer, pos);
							batteryCharge = atol(buffer);
						}
						if (j== 13){
							strcpy(buffer, pos);
							chargingState = atoi(buffer);
						}
						j++;
						pos = strtok(NULL, " ");
					}
					//state = roombaHome;
					//charging = true;
					//docked = true;
					//p_requestStopDatastreamFlag = true;				
					dataState = chargingMessageReadingHeader;
					j = 0;
				}
				j++;
			}
			//timeout?
			else if ((millis() - messagetimeouttimer) > CHARGINGMESSAGETIMEOUT){
   				//start datastream to figure out what it's up to
				dataState = datastreamStart;
			}
			break;
		//Handle datastream
		case datastreamStart:
			Serial.write(OCstart); //Roomba will change to passive mode
			//maybe clear data?
			commandTimer = millis();		
			debugVal = 0; //resetvalue
			dataState = datastreamStarting;
			break;
 		case datastreamStarting:
            if ((millis() - commandTimer) > COMMANDTIMEOUT){
                //setup datastream
                Serial.write(OCDatastream); //start stream
                Serial.write(12); //12 packets
                Serial.write(15); //1- dirt detect U 1
                Serial.write(21); //2- Charging state U 1
                Serial.write(22); //3- voltage U 2
                Serial.write(23); //4- current S 2
                Serial.write(24); //5- Battery temp S 1
                Serial.write(25); //6- Battery charge U 2
                Serial.write(26); //7- Battery capacity U 2
                Serial.write(34); //8- Charging sources available U 1
                Serial.write(35); //9- OI mode U 1
                Serial.write(37); //10- Song playing? U 1
                Serial.write(43); //11- Wheel encoder counts left S 2
                Serial.write(44); //12-Wheel encoder counts right S 2
                //serial write startDatastream
                dataState = datastreamWaitingForHeader;
                //Set timer
                messagetimeouttimer = millis();
            }
            break;
        case datastreamWaitingForHeader:
			//disentangle datastream and do checksum for valid data. 
            if (Serial.available()){
				byte serialByte= Serial.read();
				if(serialByte == 19){ //Header for datastream (save to variable to do multiple checks for charging message etc)
                    dataState = datastreamWaitingForNbytes;
                    messagetimeouttimer = millis(); //reset timer
                }            
                //timeout
                else if ((millis() - messagetimeouttimer) > CHARGINGMESSAGETIMEOUT){
                    messagetimeouttimer = millis(); //reset timer
                    errorCounter++;
                    if (errorCounter == DATASTREAMMAXERROR){
						dataState = datastreamStopping;
                    }
                }
            }
            break;    
        case datastreamWaitingForNbytes:
            if (Serial.available()){
			    if(Serial.read() == 30){ //number of bytes
                    dataState = datastreamReadingData;
                    i = 0;
                    checksum = 49; //19 + 30 starting value of checksum
                    messagetimeouttimer = millis(); //reset timer
                }    
                else { //number of bytes should imediately follow header. Else, header was just a rando 19 value
					//Go back to finding the next header
					dataState = datastreamWaitingForHeader;
                }
            }    
            //timeout
            else if ((millis() - messagetimeouttimer) > CHARGINGMESSAGETIMEOUT){
                messagetimeouttimer = millis(); //reset timer
                errorCounter++;
                if (errorCounter == DATASTREAMMAXERROR){
					dataState = datastreamStopping;
                }
            }
            break;
        case datastreamReadingData:
            if(Serial.available()){
                databuffer[i] = Serial.read();
                checksum += databuffer[i];
                if(i==29){
                    dataState = datastreamCheckingData;
                    messagetimeouttimer = millis(); //Reset timer
                }
                i++;
            }
            //timeout
            else if ((millis() - messagetimeouttimer) > CHARGINGMESSAGETIMEOUT){
                messagetimeouttimer = millis(); //reset timer
                errorCounter++;
                if (errorCounter == DATASTREAMMAXERROR){
 					dataState = datastreamStopping;
               }
            }
            break;
        case datastreamCheckingData:
            if(Serial.available()){
                checksum += Serial.read(); //Addition to checksum should give 256 on rollover of byte 0
                if (checksum == 0){
                    //checksum passed
                    dirtDetect = databuffer[1];
                    chargingState = databuffer[3];
					batteryVoltage = (databuffer[5] << 8) | databuffer[6];
                    current = (databuffer[8] << 8) | databuffer[9];
                    batteryTemp = databuffer[11];
                    batteryCharge = (databuffer[13] << 8) | databuffer[14];
                    batteryCapacity = (databuffer[16] << 8) | databuffer[17];
                    docked = bitRead(databuffer[19],1);
					OImode = databuffer[21];
                    songPlaying = databuffer[23];
                    encoderLeft = (databuffer[25] << 8) | databuffer[26];
                    encoderRight = (databuffer[28] << 8) | databuffer[29];
                    p_encodersReady = true;
					errorCounter = 0;
					if(docked){
						dataState = datastreamStopping;
					}
					else{
			            dataState = datastreamWaitingForHeader;
					}
                }
                else{
                    //checksum failed
                    errorCounter++;
                    if (errorCounter == DATASTREAMMAXERROR){
 						dataState = datastreamStopping;
                   }
                }
            }
            //timeout
            else if ((millis() - messagetimeouttimer) > CHARGINGMESSAGETIMEOUT){
                dataState = datastreamWaitingForHeader;
                errorCounter++;
                if (errorCounter == DATASTREAMMAXERROR){
 					dataState = datastreamStopping;
                }
            }
            break;
        case datastreamStopping:
            //handle last message
            //serial write stop datastream
            Serial.write(OCPauseDatastream);
            Serial.write(0);
            Serial.write(OCStop);
 			dataState = chargingMessageWaitingForHeader;
			messagetimeouttimer = millis(); //reset timer
            errorCounter = 0; 
			break;
 	}
}
void Roomba632::SendStopCommand(){
	static bool commandstate;
	static unsigned long commandTimer;
	if (!commandstate){
		Serial.write(OCStop);
		commandstate = true;
		commandTimer = millis();
	}
	else{		
		if ((millis() - commandTimer) > COMMANDTIMEOUT){
			commandstate = false;
			roombaState = roombaIdle;	
			EventStateChange();
		}
	}
}
void Roomba632::SendCleanCommand(){
	static int commandstate;
	static unsigned long commandTimer;
	switch (commandstate){
		case startIO:
			Serial.write(OCstart);
			commandstate = setMode;
			commandTimer = millis();
			break;
		case setMode:
			if ((millis() - commandTimer) > COMMANDMODETIMEOUT){
				Serial.write(OCSafemode);
				commandstate = sendCommand;
				commandTimer = millis();
			}
			break;
		case sendCommand:
			if ((millis() - commandTimer) > COMMANDTIMEOUT){
				Serial.write(OCClean);
				commandstate = startIO; //reset state
				roombaState = roombaCleaning;
				EventStateChange();
				p_undockedTimer = millis();
			}
			break;					
	}
}
void Roomba632::SendDockCommand(){
	static int commandstate;
	static unsigned long commandTimer;
	switch (commandstate){
		case startIO:
			Serial.write(OCstart);
			commandstate = setMode;
			commandTimer = millis();
			break;
		case setMode:
			if ((millis() - commandTimer) > COMMANDMODETIMEOUT){
				Serial.write(OCSafemode);
				commandstate = sendCommand;
				commandTimer = millis();
			}
			break;
		case sendCommand:
			if ((millis() - commandTimer) > COMMANDTIMEOUT){
				Serial.write(OCDock);
				commandstate = startIO; //reset state
				roombaState = roombaDocking;
				EventStateChange();
				p_undockedTimer = millis();
			}
			break;					
	}
}
void Roomba632::SendSpotCommand(){
	static int commandstate;
	static unsigned long commandTimer;
	switch (commandstate){
		case startIO:
			Serial.write(OCstart);
			commandstate = setMode;
			commandTimer = millis();
			break;
		case setMode:
			if ((millis() - commandTimer) > COMMANDMODETIMEOUT){
				Serial.write(OCSafemode);
				commandstate = sendCommand;
				commandTimer = millis();
			}
			break;
		case sendCommand:
			if ((millis() - commandTimer) > COMMANDTIMEOUT){
				Serial.write(OCSpot);
				commandstate = startIO; //reset state
				roombaState = roombaSpotCleaning;
				EventStateChange();
			}
			break;					
	}
}
void Roomba632::SendPlayMusicCommand(){
	static int commandstate;
	static unsigned long commandTimer;
	switch (commandstate){
		case startIO:
			Serial.write(OCstart);
			commandstate = setMode;
			commandTimer = millis();
			break;
		case setMode:
			if ((millis() - commandTimer) > COMMANDMODETIMEOUT){
				Serial.write(OCFullmode);
				commandstate = startIO; //reset state
				roombaState = roombaMusic; //sending music commands is handled in the music state
				EventStateChange();
			}
			break;		
	}
}
int Roomba632::ScriptHandler(){
	int16_t velocity, radius;
	uint8_t scriptDistance, scriptAngle, scriptTime;
	static uint8_t scriptRepeat;
	static int scriptState, p_programCounter; //program counter for scripts;
	static unsigned long scriptTimer, waitingTime;
	switch (scriptState){
		case RMIdle:	//0
			if (p_startScriptFlag){
				p_startScriptFlag = false;
				scriptState = RMStart;
			}
			break;
		case RMStart:	//1
			Serial.write(OCstart);
			scriptTimer = millis();
			scriptState = RMStarting;
			break;
		case RMStarting://2
			if ((millis() - scriptTimer) > COMMANDSTARTTIMEOUT){
				Serial.write(OCSafemode);
				p_programCounter= 0;	//Reset script
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
			}
			break;
		case RMHalt:	//3 scriptable
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				Serial.write(OCStop);
				scriptTimer = millis();
				scriptState = RMHalting;
			}
			break;
		case RMHalting:	//4
			if((millis() - scriptTimer) > COMMANDSTARTTIMEOUT){
				p_programCounter = 0;
				scriptState = RMIdle;
			}
			break;
		case RMDriveF:	//5 scriptable verb + noun
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				scriptDistance = p_scriptPointer[p_programCounter + 1]; //Distance in steps of 25mm (gives maximum of 6.375m)
				waitingTime = scriptDistance * DISTANCESCALAR; 
				velocity = 250; //mm/s
				radius = 32767; //Drive straight
				SendDriveCommand(velocity, radius);
				scriptTimer = millis();
				scriptState = RMDrivingF;
			}
			break;
		case RMDrivingF://6
			if((millis() - scriptTimer) > waitingTime){
				p_programCounter = p_programCounter + 2;
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
				debugVal = scriptState;
			}
			break;
		case RMDriveR:	//7 scriptable verb + noun
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				scriptDistance = p_scriptPointer[p_programCounter + 1]; //Distance in steps of 25mm (gives maximum of 6.375m)
				waitingTime = scriptDistance * DISTANCESCALAR; 
				velocity = -250; //mm/s
				radius = 32767; //Drive straight
				SendDriveCommand(velocity, radius);
				scriptTimer = millis();
				scriptState = RMDrivingR;
			}
			break;
		case RMDrivingR://8
			if((millis() - scriptTimer) > waitingTime){
				p_programCounter = p_programCounter + 2;
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
			}
			break;
		case RMTurnL:	//9 scriptable verb + noun
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				scriptAngle = p_scriptPointer[p_programCounter +1]; //Angle in 256 steps for full circle (quarter turn = 64)
				waitingTime = scriptAngle * TURNINGSPEED;//quarter turn should give 1570ms
				velocity = 256; //mm/s 
				radius = 256; //radius in mm's
				SendDriveCommand(velocity, radius);
				scriptTimer = millis();
				scriptState = RMTurningL;
			}
			break;
		case RMTurningL://10
			if((millis() - scriptTimer) > waitingTime){
				p_programCounter = p_programCounter + 2;
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
			}
			break;
		case RMTurnR:	//11 scriptable verb + noun
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				scriptAngle = p_scriptPointer[p_programCounter +1]; //Angle in 256 steps for full circle (quarter turn = 64)
				waitingTime = scriptAngle * (TURNINGSPEED);//quarter turn should give 1570ms
				velocity = 256 - TURNINGOFFSET; //mm/s 
				radius = -256; //radius in mm's
				SendDriveCommand(velocity, radius);
				scriptTimer = millis();
				scriptState = RMTurningR;
			}
			break;
		case RMTurningR://12
			if((millis() - scriptTimer) > waitingTime){
				p_programCounter = p_programCounter + 2;
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
			}
			break;
		case RMWait:	//13 scriptable verb + noun
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				SendDriveCommand(0, 0);
				scriptTime = p_scriptPointer[p_programCounter +1]; //Wating time in steps of 128ms for maximum wating time of ~32,5 seconds
				waitingTime = scriptTime * 128;
				scriptState = RMWaiting;
				scriptTimer = millis();
			}
			break;
		case RMWaiting:	//14
			if((millis() - scriptTimer) > waitingTime){
				p_programCounter = p_programCounter + 2;
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
			}
			break;
		case RMGoto: //15 Will go on forever!
			p_programCounter = p_scriptPointer[p_programCounter + 1];
			scriptState = p_scriptPointer[p_programCounter];
			scriptTimer = millis();
			break;
		case RMRepeat: //16 repeat for total of numbered runs
			if (scriptRepeat == 0){
				scriptRepeat = p_scriptPointer[p_programCounter + 1];
				scriptState = p_scriptPointer[0];
			}
			else if(scriptRepeat != 1){
				scriptRepeat--;
				scriptState = p_scriptPointer[0];
			}
			else if(scriptRepeat == 1){
				scriptState = RMHalt;
			}
			p_programCounter = 0;
			scriptTimer = millis();
			break;
		default:
			//Reset
			scriptState = RMHalt;
			break;
	}
	return scriptState;
}
void Roomba632::SendDriveCommand(int16_t velocity, int16_t radius){
	Serial.write(OCDrive);
	Serial.write(velocity >> 8);
	Serial.write(velocity);
	Serial.write(radius >> 8);
	Serial.write(radius);
}
/*
void Roomba632::_evilLights(){
}
*/

/*
unsigned long Roomba632::_songLength(song as argument){
	return songlength;
}
*/
void Roomba632::UpdateOdometry(){
	if(p_encodersReady){
		p_encodersReady = false;
		//p_odometryTimer = millis();
		static int16_t encoderRPosPrev = 0;
		static int16_t encoderLPosPrev = 0;
		int16_t encoderRDelta = encoderRight - encoderRPosPrev;
		int16_t encoderLDelta = encoderLeft - encoderLPosPrev;
		float SR = DISTANCEPERCOUNTR * (encoderRDelta);
		float SL = DISTANCEPERCOUNTL * (encoderLDelta);
		float meanDistance = (SL + SR)/2;
		encoderRPosPrev = encoderRight;
		encoderLPosPrev = encoderLeft;
		theta += atan((SR - SL) / WHEELDISTANCE);
		if(theta > 6.28){
			theta -= 6.28;
		}
		else if(theta < -6.28){
			theta += 6.28;
		}
		//Internal values
		//Xcoo += meanDistance * cos(theta);           
		//Ycoo += meanDistance * sin(theta);
		//Internal values
		Xcoo += meanDistance * cos(AuxYaw);           
		Ycoo += meanDistance * sin(AuxYaw);
	}
}