#ifndef __CProp_H__
#define __CProp_H__

#include "PicServo.h"
#include <fstream.h>

#define	PROP_DEF_KP		(10)
#define	PROP_DEF_KD		(1)
#define	PROP_DEF_KI		(0)
#define	PROP_DEF_PLIM	(300) // rpm of output
#define	PROP_DEF_ILIM	(100)
#define	PROP_DEF_RATE	(5) // Hz
#define PROP_DEF_DEADBAND	(10) // RPM of output
#define PROP_DEF_ACCEL	(10) // RPM/s of output
#define PROP_DEF_PWM_LIM (50) // from 0 to 255
#define PROP_DEF_POS_ERR_LIM (1000) // from 0 to 16000

#define PROP_MAX_SPEED	(400) // RPM of output
#define PROP_COUNTS_PER_REV	(8) // 8 hall effect "encoder" counts per motor rev
#define PROP_GEARBOX	(10) // 10:1 gearbox

typedef struct {
	int		Kp;
	int		Kd;
	int		Ki;
	int		Plim;
	int		Ilim;
	int		rate;
	int		deadband;
	double	accel;
	int		pwm_lim;
	int		pos_err_lim;
  int IntLimit;
} prop_params_t;

class CProp {

private:
	BOOL		m_bAmpEn; // actual state of amp enable
	BOOL		m_bAmpEnCmd; // command state of amp enable
	BOOL		m_bAtSpeed;
	UCHAR		m_cStatus;
	UCHAR		m_cAuxStatus;
	int			m_nNumNodes;
	int			m_nCurSpeed; // RPMs
	int			m_nUnfiltSpeed; // RPMs
	int			m_nSetSpeed; // RPMs
	int			m_nTrajSpeed; // RPMs
	double		m_fLastTraj;
	int			m_nTorque; // ounce inches
	BOOL		m_bEnableReverse; // set this to true to allow reverse operation
	BOOL		m_bPositionError; // this gets set true when the pic-servo gets one

	long		m_nCurPos;
	double		m_fCurrent; // amps
	CPicServo	*m_pPicServo; //pointer to PicServo object
	int		CalcTrajectory (int desired_speed);
	int		CloseSpeedLoop();

public:
	int			m_nNodeNum;
	prop_params_t m_params;
			CProp();
	virtual ~CProp();

	void	Initialize(CPicServo *pic_servo, int num_nodes);
	void	EnableAmp();
	void	DisableAmp();
	void	RefreshWD ();	
	void	SetSpeed(int speed); // RPMs
	void	SetParams (int kP, int kD, int kI, int Plim, int Ilim, int Cmdlim, int Accel, int Deadband);
	void	SetReverse (BOOL enable); // sets the m_bEnableReverse flag
	void	ClearPositionError () {m_bPositionError = FALSE;}

	UCHAR   GetStatus() {return m_cStatus;}
	BOOL	GetAmpEnabled() {return m_bAmpEn;}
	BOOL	GetAmpEnabledCmd() {return m_bAmpEnCmd;}
	int		GetCurSpeed() {return m_nCurSpeed;} // RPMs
	int		GetSetSpeed() {return m_nSetSpeed;} // RPMs
	BOOL	GetAtSpeed() {return m_bAtSpeed;}
	BOOL	GetReverseEnabled () {return m_cStatus & LIMIT2;}
	BOOL	GetPositionError () {return m_bPositionError;}
	double	GetCurrent() {return m_fCurrent;} // amperes
	int		GetTorque() {return m_nTorque;} // ounce*inches
	void	GetParams(prop_params_t &params) {memcpy(&params, &m_params, sizeof(m_params));}

	int		Update(); // updates status and registers

	//int		GetAmpStatus(); // must do this at a higher level

};

#endif // __CProp_H__