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

#ifndef ROOMBA632_H
#define ROOMBA632_H

// Arduino versioning.
#include "Arduino.h"

#define DISTANCEPERCOUNTL 0.4525 //(pi * 72.0 / 508.8) mm / count
#define DISTANCEPERCOUNTR 0.4400 //(pi * 72.0 / 508.8) mm / count
#define WHEELDISTANCE 247.0 //mm center wheel <-> center wheel
#define TURNINGSPEED 27 // scalar to determine time to take a turn of given angle
#define TURNINGOFFSET 47 //Offset for difference between left and right turn
#define DISTANCESCALAR 100 // scalar to determine time to drive set distance

//#define ODOMETRYINTERVAL 15

#define DATASTREAMTIMEOUT 500
#define CHARGINGMESSAGETIMEOUT 3000
#define DATASTREAMMAXERROR 5
#define COMMANDSTARTTIMEOUT 50
#define COMMANDMODETIMEOUT 50
#define COMMANDTIMEOUT 20
#define DOCKEDDEBOUNCETIME 10000
#define MAXSONGLENGTH 32

enum OImodes {
	Off = 0,
	Passive = 1,
	Safe = 2,
	Full = 3
};
enum roombaStates {
	roombaIdle,				//0
	roombaHome,				//1
	roombaMusic,			//2
	roombaCleaning,			//3
	roombaSpotCleaning,		//4
	roombaDocking,			//5
	roombaPlayingScript,	//6
	roombaSinging,			//7
	roombaDriving			//8
};
enum dataStates {
	dataIdle,							//0
	datastreamStart,					//1
	datastreamStarting,					//2
	datastreamSetMode,					//3
	datastreamWaitingForHeader,			//4 -- undocked
	datastreamWaitingForNbytes,			//5
	datastreamReadingData,				//6
	datastreamCheckingData,				//7
	datastreamStopping,					//8
	chargingMessageWaitingForHeader,	//9 -- docked
	chargingMessageReadingHeader,		//10
	chargingMessageReadingMessage		//11
};
enum chargingStates{
	chargingStateNotCharging,
	chargingStateReconditioning,
	chargingStateFull,
	chargingStateTrickle,
	chargingStateWaiting,
	chargingStateFaultCondition
};
enum sendingCommandStates {
	sendingCommandsIdle,
	sendingCommandsSetMode,
	sendingCommandsSending
};
enum commandStates{
	startIO,
	setMode,
	sendCommand
};
enum opcodes{
	OCstart = 128,
	OCSafemode = 131,
	OCFullmode = 132,
	OCSpot = 134,
	OCClean = 135,
	OCDrive = 137,
	OCLeds = 139,
	OCSong = 140,
	OCPlaySong = 141,
	OCDatastream = 148,
	OCPauseDatastream = 150,
	OCDock = 143,
	OCStop = 173
};
enum scriptopcodes{
	RMIdle,		//0
	RMStart,	//1
	RMStarting,	//2
	RMHalt,		//3 scriptable
	RMHalting,	//4
	RMDriveF,	//5 scriptable verb + noun steps of 25mm
	RMDrivingF,	//6
	RMDriveR,	//7 scriptable verb + noun steps of 25mm
	RMDrivingR,	//8
	RMTurnL,	//9 scriptable verb + noun full circle = 255
	RMTurningL,	//10
	RMTurnR,	//11 scriptable verb + noun full circle = 255
	RMTurningR,	//12
	RMWait,		//13 scriptable verb + noun ticks of 128ms
	RMWaiting,	//14
	RMGoto,		//15 scriptable verb + noun goto script addres
	RMRepeat	//16 scriptable verb + noun repeats given number of times
};
enum musicStates{
	musicInit,
	musicWriteSongHeader,
	musicWriteNotes,
	musicStartSong,
	musicPlaySong
};

enum notes{																				N_G1 = 31,  N_G1S = 32,	N_A1 = 33,	N_A1S = 34,	N_B1 = 35,
	N_C2 = 36,	N_C2S = 37,	N_D2 = 38,	N_D2S = 39,	N_E2 = 40,	N_F2 = 41,	N_F2S = 42,	N_G2 = 43,	N_G2S = 44,	N_A2 = 45,	N_A2S = 46,	N_B2 = 47,
	N_C3 = 48,	N_C3S = 49,	N_D3 = 50,	N_D3S = 51,	N_E3 = 52,	N_F3 = 53,	N_F3S = 54,	N_G3 = 55,	N_G3S = 56,	N_A3 = 57,	N_A3S = 58,	N_B3 = 59,
	N_C4 = 60,	N_C4S = 61,	N_D4 = 62,	N_D4S = 63,	N_E4 = 64,	N_F4 = 65,	N_F4S = 66,	N_G4 = 67,	N_G4S = 68,	N_A4 = 69,	N_A4S = 70,	N_B4 = 71,
	N_C5 = 72,	N_C5S = 73,	N_D5 = 74,	N_D5S = 75,	N_E5 = 76,	N_F5 = 77,	N_F5S = 78,	N_G5 = 79,	N_G5S = 80,	N_A5 = 81,	N_A5S = 82,	N_B5 = 83,
	N_C6 = 84,	N_C6S = 85,	N_D6 = 86,	N_D6S = 87,	N_E6 = 88,	N_F6 = 89,	N_F6S = 90,	N_G6 = 91,	N_G6S = 92,	N_A6 = 93,	N_A6S = 94,	N_B6 = 95,
	N_C7 = 96,	N_C7S = 97,	N_D7 = 98,	N_D7S = 99,	N_E7 = 100,	N_F7 = 101,	N_F7S = 102,N_G7 = 103,	N_G7S = 104,N_A7 = 105,	N_A7S = 106,N_B7 = 107
};

class Roomba632 {
public:
	Roomba632();
	void handler();
	//Set modes:
	//Set a flag (and variables) as a "request" witch is handled in order of priority:
	void stop();
	void clean();
	void spot();
	void dock();
	void playScript(uint8_t script[]);
	void playMusic(uint8_t song[],int songNumberOfBytes);
	//void drive();

	//Callbacks:
	void setCallbackDoneCleaning(void (*callbackFuncDoneCleaning)());
	void setCallbackStateChange(void (*callbackFuncStateChange)());

	//Privately set public variables
	//char debugmessage[80];
	uint8_t dirtDetect;
	uint8_t chargingState;
	uint16_t batteryVoltage;
	int16_t current;
	int8_t batteryTemp;
	uint16_t batteryCharge;
	uint16_t batteryCapacity;
	bool docked;
	uint8_t OImode; //enum for states
	uint8_t songPlaying;
	int16_t encoderLeft;
	int16_t encoderRight;
	int16_t angle; //calculated from encoders
	int8_t distance; //calculated from encoders
	int debugVal;
	float AuxYaw;
	float AuxXacc;
	float AuxYacc;
	float Xcoo;
	float Ycoo;
	float theta;
	//Privately set public states. Only set and change states from within state machine.
	int roombaState; //enum for states
	int dataState;
	//int receivingDatastreamState = datastreamIdle; //enum for states
	//int sendingCommands = sendingCommandsIdle; //enum for states

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

private:
	//unsigned long _songLength(song as argument); //song
	void SendStopCommand();
	void SendCleanCommand();
	void SendDockCommand();
	void SendSpotCommand();
	int ScriptHandler();
	void SendDriveCommand(int16_t velocity, int16_t radius);
	void SendPlayMusicCommand();
	//void _evilLights();
	void GetValue(char incommingChar, char SearchString[13], int& value);
	void UpdateOdometry();
	void (*_callbackFuncDoneCleaning)();
	void EventDoneCleaning();
	void (*_callbackFuncStateChange)();
	void EventStateChange();
	//Private flags
	volatile bool p_cleanFlag;
	//volatile bool p_chargingFlag;
	volatile bool p_dockFlag;
	volatile bool p_spotFlag;
	volatile bool p_stopFlag;
	volatile bool p_playMusicFlag;
	volatile bool p_startScriptFlag;
	volatile bool p_driveFlag;
	volatile bool p_encodersReady;
	//Private variables
	uint8_t *p_songPointer;
	int p_songNumberOfBytes;
	uint8_t *p_scriptPointer;
	unsigned long p_undockedTimer;
	//unsigned long p_odometryTimer;
	//uint8_t p_chargingState;
	//Private counters and timers
	//uint8_t p_musicCounter; //could be static?
	//unsigned long p_musicTimer; //could be static?
};

#endif