/*
	eGizmo_PBot_Uno.h - eGizmo_PBot_Uno library
	Copyright (c) 2020 e-Gizmo Mechatronix Central Inc
    All right reserved.
*/
#ifndef eGizmo_PBot_Uno_h
#define eGizmo_PBot_Uno_h

#if ARDUINO >= 100
	#include "Arduino.h"
#else
	#include "WProgram.h"
#endif

#include <inttypes.h>
#include "Stream.h"
#include <string.h>
#include <stdbool.h>

#include <Wire.h>

/*
	#define int bool
	#define true 1
	#define false 0
*/

// partype
#define P_NONE 0
#define P_BYTE 1
#define P_INT 2

// Command defines
#define TWI_MSPEEDA  0x10
#define TWI_MSPEEDB 0x11
#define TWI_MFWDA 0x12
#define TWI_MREVA 0x13
#define TWI_MFWDB 0x14
#define TWI_MREVB 0x15
#define TWI_MOFF  0x16
#define TWI_MON   0x17

#define TWI_SERVO1  0x20
#define TWI_SERVO2  0x21
#define TWI_SERVO3  0x22
#define TWI_SERVO4  0x23

#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo

#define TWI_READLINE  0x50
#define TWI_READCOL   0x51

#define MOTOR_A  1
#define MOTOR_B  2
#define MOTOR_BOTH  3
#define MOTOR_FWD HIGH
#define MOTOR_REV LOW

#define IDLEJSVALUE 128
#define MAXJSVALUE	255
#define MINJSVALUE	0

#define MINMOTORSPEEDPRECENT	10

#define MINMOTORSPEED	80
#define MAXMOTORSPEED	255

#define ECHO 16
#define TRIGGER 15

class eGizmo_PBot_Uno
{
	public:
		eGizmo_PBot_Uno();

		void BEGIN();

        /* Motor Functions*/
		void ALLON();
		void ALLOFF();
		void FORWARD(uint8_t fwdSpeed);
		void FORWARD(uint8_t Lspeed, uint8_t Rspeed);
		void REVERSE(uint8_t revSpeed);
		void REVERSE(uint8_t Lspeed, uint8_t Rspeed);
		void STOP(void);
		void TURNLEFT(uint8_t Rspeed);
		void TURNRIGHT(uint8_t Lspeed);
		void SPINLEFT(uint8_t spinSpeed);
		void SPINRIGHT(uint8_t spinSpeed);

		/* Servo Motor Functions*/		
			//Variables
				int servo1Pos;
				int servo2Pos;
				int servo3Pos;
				int servo4Pos;

				int servo1Max;
				int servo1Min;
				int servo1Home;

				int servo2Max;
				int servo2Min;
				int servo2Home;

				int servo3Max;
				int servo3Min;
				int servo3Home;

				int servo4Max;
				int servo4Min;
				int servo4Home;

			//Functions
				void SERVO(uint8_t,uint16_t);
				void SETSERVO1(uint16_t minVal = 0, uint16_t maxVal = 180, uint16_t homeVal = 90);
				void SETSERVO2(uint16_t minVal = 0, uint16_t maxVal = 180, uint16_t homeval = 90);
				void SETSERVO3(uint16_t minVal = 0, uint16_t maxVal = 180, uint16_t homeval = 90);
				void SETSERVO4(uint16_t minVal = 0, uint16_t maxVal = 180, uint16_t homeval = 90);

				void SERVO1(uint16_t);
				void SERVO2(uint16_t);
				void SERVO3(uint16_t);
				void SERVO4(uint16_t);

		/* Collision Sensors Functions*/
		boolean COL1_RIGHT();
		boolean COL2_CENTER();
		boolean COL3_LEFT();

		/* Line Sensors Functions*/
		boolean LS1_LEFT();
		boolean LS2_CENTER();
		boolean LS3_RIGHT();

		/* Distance sensor Functions*/
	    long RANGE;
 	    long DURATION;
 	    void DISTANCE(void);

		/* Ps2 Control Functions on the Left Joytick*/
		void PS2MOTORCONTROL(uint8_t, uint8_t);

		/* TWI */
		void twi_send(uint8_t,uint8_t,uint16_t,int8_t);
		void twi_send(int, int, int);

		byte twi_read(int, byte);


	private:
		byte device;
		int8_t min;		// minimum is this value times 4 added to MIN_PULSE_WIDTH
		int8_t max;		// maximum is this value times 4 added to MAX_PULSE_WIDTH

		bool dir;

};
#endif