#ifndef Ax12_h
#define Ax12_h

#define noResponse 0
#define readDataOnly 1
#define respondAll 2 

#include "SerialPort.h"
#include "FlowController.h"

class Ax12 {

private:
	unsigned char _flowControlPin;
	int _baudRate = 1000000; // Stores current baud value initial value in the Ax12 register
	unsigned char ID = 1;

	unsigned char _statusRecieved[25];
	int _bufferSize = 0;
	int _rxErrors = 0;
	bool _recieveError = false;
	bool _newPacket = false;
	bool _statusReturn = true; // has the motor been set to return a status packet

	int _temperature = 0; // read temperature in degrees celcius
	int _position = 0; // last read position 0 - 1023
	double _positionDeg = 0; // last read position 0 - 300 degrees
	int _speed = 0; //last speed to be written or read
	float _voltage = 0; // Returns last read voltage in volts
	int _punch = 0; // last set punch 
	int _load = 0; // last read load on actuator 
	bool _loadDirCw = true; // direction of load on the actuator
	bool _torqueEnabled = false; // last read or set torque
	bool _ledOn = false; // keeps track of weather the led is on or not

	int _tempLimit = 0;
	int _lowVoltLimit = 0;
	int _highVoltLimit = 0;
	int _cwLimit = 0;
	int _ccwLimit = 0x3FF;
	int maxTorque = 0x3FF;

	void recieveStatus(FlowController& ComMode, SerialPort& Serial); // recieves status packet
	bool validChecksum(unsigned char Packet[]); // returns true if checksum is valid

public:
	Ax12 (FlowController& ComMode, SerialPort& Serial,unsigned char ID); // Constructor
	Ax12(FlowController& ComMode, SerialPort& Serial, int baud, unsigned char ID); // Overloaded Constructor
	~ Ax12(void);

	void reset(FlowController& ComMode, SerialPort& Serial); // Restor original factory default values to memory table
	void ping(FlowController& ComMode, SerialPort& Serial);  // Check if a motor is present and responding

	void setID(FlowController& ComMode, SerialPort& Serial, unsigned char newID); // Change ID on a Ax12
	void setBaud(FlowController& ComMode, SerialPort& Serial, int baud); // Set baudrate

	void move(FlowController& ComMode, SerialPort& Serial, double degrees); // move actuator to posistion in degrees
	void setSpeed(FlowController& ComMode, SerialPort& Serial, int speed); // set speed before next move command 0 means no velocity controll
	void moveAtSpeed(FlowController& ComMode, SerialPort& Serial, int speed, double degrees); // move to pos with set speed

	void torqueEnable(FlowController& ComMode, SerialPort& Serial, bool Enable); // Enables and disables torques
	void ledOn(FlowController& ComMode, SerialPort& Serial, bool Enable); // Turns on and off on Board Led

	void setTempLimit(FlowController& ComMode, SerialPort& Serial, unsigned char Temp); // Set Temp limits
	void setVoltageLimit(FlowController& ComMode, SerialPort& Serial, unsigned char LowVoltage, unsigned char HighVoltage); //Set Voltage Limits
	void setAngleLimit(FlowController& ComMode, SerialPort& Serial, double CW, double CCW); // Set Angle limits
	void setTorqueLimit(FlowController& ComMode, SerialPort& Serial, unsigned char Torque); // Set Max Torque to both ROM and RAM

	void setLedAlarm(FlowController& ComMode, SerialPort& Serial, unsigned char AlarmType); // if (AlarmType) an Error occurs Led Blinks overheating error is default 85Deg
	void setShutDownAlarm(FlowController& ComMode, SerialPort& Serial, unsigned char AlarmType); // if (AlarmType) an Error occurs unit shuts Down overheating error is default 85Deg
	void setStatusReturnLevel(FlowController& ComMode, SerialPort& Serial, unsigned char Response); // Respond none 0, Respond to only Read_DATA, respond to all
	void setReturnDelay(FlowController& ComMode, SerialPort& Serial, unsigned char ReturnDelay); // This sets the amount of delay before the status packet is returned Default 500uS

	void setCompMargin(FlowController& ComMode, SerialPort& Serial, unsigned char CWMargin, unsigned char CCWMargin); // How far the goal position can deviate
	void setCompSlope(FlowController& ComMode, SerialPort& Serial, unsigned char CWSlope, unsigned char CCWSlope); // How much torque is applied to compensate for a deviation that is too large
	void setPunch(FlowController& ComMode, SerialPort& Serial, int Punch); // The minimun current supplied to the motor during operation

	void readAngleLimit(FlowController& ComMode, SerialPort& Serial);

	int readTemperature(FlowController& ComMode, SerialPort& Serial); // returns Temperature in degrees Celcius writes to _temperature var
	int readPosition(FlowController& ComMode, SerialPort& Serial); // returns Position writes to _position var and _positionDeg var
	float readVoltage(FlowController& ComMode, SerialPort& Serial); // returns Voltage writes to _volatge var
	int readSpeed(FlowController& ComMode, SerialPort& Serial); // returns speed writes _speed
	int readLoad(FlowController& ComMode, SerialPort& Serial); // returns load magnitude writes _load
	bool moving(FlowController& ComMode, SerialPort& Serial); // returns true if motor is moving under its own power

	int getRxErrors(void);

	/* All the getters below return the last read value which may not be the current value*/
	bool getStatusReturn(void);
	int getTemp(void);
	int getPosition(void);
	double getPositionDeg(void);
	int getSpeed(void);
	float getVoltage(void);
	int getPunch(void);
	int getloadMag(void);
	bool isLoadCW(void);
	bool isTorqueEnabled(void);
	bool isLedOn(void);
	int getCwLimit(void);
	int getCcwLimit(void);
	unsigned char getID(void);
	
};


#endif