#ifdef WIN32
#include "stdafx.h"
#include "Registry.h"
#endif

#ifdef _QNX
#include <stdlib.h>
#include <i86.h>
#include "Syslog.h"
#include <sys/times.h>
#include <time.h>
#endif
#include "CProp.h"

#include "math.h"

#undef SOFTWARE_SPEED_CONTROL

CProp::CProp()
{
	m_nNodeNum = m_nNumNodes = 0;
	m_nCurSpeed = m_nUnfiltSpeed = m_nSetSpeed = 0;
	m_nCurPos = 0;
	m_bAmpEn = m_bAmpEnCmd = FALSE;
	m_bEnableReverse = FALSE;
}

CProp::~CProp()
{
	m_nNumNodes = 0;
}

void CProp::Initialize(CPicServo *pic_servo, int num_nodes)
{
  //	m_nNodeNum = node_num;
	m_pPicServo = pic_servo;
	m_nNumNodes = num_nodes;

	m_nCurSpeed = m_nUnfiltSpeed = m_nSetSpeed = 0;
	m_nCurPos = 0;
	m_bEnableReverse = FALSE;


	// Set up the PIC-SERVO to send  every status message
	m_pPicServo->NmcDefineStatus(m_nNodeNum, SEND_POS | SEND_AD );
	// note: for 80 enc ticks per output rev * 400 RPM * 1 month = 2^30.36. We are okay with 32 bit resolution.
	m_pPicServo->ServoResetPos(m_nNodeNum); // Set encoder position to 0
	m_pPicServo->ServoStopMotor(m_nNodeNum, MOTOR_OFF); // Reset PIC-Servo internal regs
	m_bAmpEn = m_bAmpEnCmd = FALSE;

	// Enable Watchdog circuit
	m_pPicServo->ServoSetIoCtrl(m_nNodeNum, (m_bEnableReverse ? SET_OUT2 : 0)); // sets limit 2 and limit 1 as outputs, initial value=0

#ifndef SOFTWARE_SPEED_CONTROL
	m_pPicServo->ServoSetGain(m_nNodeNum, m_params.Kp, m_params.Kd, m_params.Ki, m_params.Ilim,
				m_params.pwm_lim, 0, m_params.pos_err_lim, m_params.rate, 0);
#else // SOFTWARE_SPEED_CONTROL
#endif // SOFTWARE_SPEED_CONTROL
}

void CProp::EnableAmp()
{
Boolean debug = False;

	dprintf("CProp::EnableAmp enabling amp\n");
		
	m_nTrajSpeed = 0;
	m_nSetSpeed = 0;
	m_fLastTraj = 0.0;

	if (m_pPicServo->nummod == m_nNumNodes)
	{
#ifndef SOFTWARE_SPEED_CONTROL
		/* NOTE: Need to really think about this one. What if prop is spinning in the water,
				just idling there and you want to enable it. It will not allow enables unless
				some minimum RPM is met. Think about this carefully! */
		//if (abs(m_nCurSpeed) > 20) // do not want to turn on servo if moving already
		//	return;
		m_pPicServo->ServoStopMotor(m_nNodeNum, MOTOR_OFF);
		m_pPicServo->ServoStopMotor(m_nNodeNum, STOP_HERE);
		m_pPicServo->ServoStopMotor(m_nNodeNum, AMP_ENABLE);
		m_pPicServo->ServoClearBits(m_nNodeNum); // clears the pos_err flag
#else // SOFTWARE_SPEED_CONTROL
		m_pPicServo->ServoStopMotor(m_nNodeNum, AMP_ENABLE | MOTOR_OFF);
#endif // SOFTWARE_SPEED_CONTROL

		m_bAmpEn = m_bAmpEnCmd = TRUE;
	}
}

void CProp::DisableAmp()
{
Boolean debug = False;

	dprintf("CProp::DisableAmp disabling amp\n");	
	if (m_pPicServo->nummod == m_nNumNodes)
	{
		m_pPicServo->ServoStopMotor(m_nNodeNum, MOTOR_OFF); // disable AMP
		m_bAmpEn = m_bAmpEnCmd = FALSE;
		m_nTrajSpeed = 0;
		m_nSetSpeed = 0;
		m_fLastTraj = 0.0;

	}
}

// speed in RPM of prop output shaft
void CProp::SetSpeed(int speed)
{
Boolean debug = False;

	if (speed > PROP_MAX_SPEED)
		speed = PROP_MAX_SPEED;
	else if (speed < -PROP_MAX_SPEED)
		speed = -PROP_MAX_SPEED;

	m_nSetSpeed = speed;

	// See if the motor is stopped and enable amp if using the PIC-Servo servo
#ifndef SOFTWARE_SPEED_CONTROL
	if (!m_bAmpEn && m_bAmpEnCmd)
	{
		m_pPicServo->ServoStopMotor(m_nNodeNum, STOP_HERE);
		m_pPicServo->ServoStopMotor(m_nNodeNum, AMP_ENABLE);
		m_pPicServo->ServoClearBits(m_nNodeNum); // clears the pos_err flag
		m_bAmpEn = TRUE;
	}
#endif //SOFTWARE_SPEED_CONTROL
//	if (!m_bAmpEn && abs(speed) > 0)
//		EnableAmp();

#ifndef SOFTWARE_SPEED_CONTROL
	int pic_speed = (int)(PROP_COUNTS_PER_REV * PROP_GEARBOX * m_params.rate /
					60.0 / 1000.0 * 0.512 * 65536.0 * (double)speed);
	int pic_accel = (int)(PROP_COUNTS_PER_REV * PROP_GEARBOX * m_params.rate * m_params.rate /
					60.0 / 1000.0 / 1000.0 * 65536.0 * 0.512 * 0.512 * (double)m_params.accel);
	dprintf("Speed=%d, Accel=%d, Rate=%d\n", pic_speed, pic_accel, m_params.rate);
	m_pPicServo->ServoLoadTraj(m_nNodeNum, ENABLE_SERVO | LOAD_VEL | LOAD_ACC | VEL_MODE | START_NOW,
		0, pic_speed, pic_accel, 0);
#endif // SOFTWARE_SPEED_CONTROL
}

int CProp::CalcTrajectory (int desired_speed)
{
	double new_traj=0.0;

#if 0
	if (desired_speed >= 50) // skip the first 50 RPM because we can't go that slow
	{
		if (m_fLastTraj < 50.0)
			m_fLastTraj = 50.0;
	} else {
		if (m_fLastTraj < 50.0)
			m_fLastTraj = 0.0;
	}
#endif

	if (desired_speed < m_fLastTraj)
	{
		new_traj = m_fLastTraj - m_params.accel/m_params.rate;
		if (new_traj < desired_speed)
			new_traj = desired_speed;
	} else if (desired_speed > m_fLastTraj)
	{
		new_traj = m_fLastTraj + m_params.accel/m_params.rate;
		if (new_traj > desired_speed)
			new_traj = desired_speed;
	} else // must be the same
	{
		new_traj = m_fLastTraj;
	}

	m_fLastTraj = new_traj;
	return ((int)new_traj);
}

int CProp::CloseSpeedLoop ()
{
#if defined(WIN32)
	static DWORD last_time = GetTickCount();
#elif defined(_QNX)
	struct timespec timeval;
        clock_gettime(CLOCK_REALTIME, &timeval);
	static double last_time =
	  (double)(timeval.tv_sec+timeval.tv_nsec/1.0e9);
#endif
	static double last_Perr = 0.0; // prop error
	static double Ierr = 0.0; // integral
	static double last_output = 0.0;
	BOOL ret = FALSE;
	Boolean debug = False;

	m_nTrajSpeed = CalcTrajectory(m_nSetSpeed);

	if (m_bAmpEn && (m_pPicServo->nummod == m_nNumNodes))
	{
		if (abs(m_nTrajSpeed - m_nCurSpeed) >= m_params.deadband)
		{
			double Perr, Derr;
#if defined(WIN32)			
			DWORD time_now = GetTickCount();
#elif defined(_QNX)
			clock_gettime(CLOCK_REALTIME, &timeval);
			double time_now =
			  (double)(timeval.tv_sec+timeval.tv_nsec/1.0e9);
#endif
			double output = 0.0; // PWM output value

			Perr = m_nTrajSpeed - m_nUnfiltSpeed;
			if (Perr > m_params.Plim) Perr = m_params.Plim;
			else if (Perr < -m_params.Plim) Perr = -m_params.Plim;

			Derr = ((double)(Perr - last_Perr)/(double)(time_now-last_time)) * 100.0;
			
			Ierr += Perr;
			if (m_nTrajSpeed == 0) // zero out integral gain
				Ierr = 0.0;

			if (Ierr > m_params.Ilim) Ierr = m_params.Ilim;
			else if (Ierr < -m_params.Ilim) Ierr = -m_params.Ilim;

			double out = (m_params.Kp * Perr + m_params.Kd * Derr + m_params.Ki * Ierr) / 1.0;
			
			if (m_nTrajSpeed != 0)
			{
				if (out > last_output)
					output = out*0.0020 + last_output*0.9980;
				else
					if (m_nCurSpeed > m_nTrajSpeed * 1.10) // problems... drop faster
						output = out*0.01 + last_output*0.99;
					else
						output = out*0.0020 + last_output*0.9980;
			}

			last_output = output;

			if ((fabs(output) > 1.0) && (m_nCurSpeed < 30) && (m_nTrajSpeed > 10) && (m_nCurSpeed < m_nTrajSpeed))
				output += 20.0;
			else
				output += 0.0;

			if (output > m_params.pwm_lim) output = m_params.pwm_lim;
			else if (output < -m_params.pwm_lim) output = -m_params.pwm_lim;

			if (!m_bEnableReverse && output < 0)
				output = 0; // protect against reversing motor

			dprintf("Perr=%d, Derr=%d, Ierr=%d, traj=%d, cur=%d, pwm=%d\n", 
				(int)Perr, (int)Derr, (int)Ierr, m_nTrajSpeed, m_nCurSpeed, (int)output);
			
			if (output < 0)
			{
				if (!(ret = m_pPicServo->ServoLoadTraj(m_nNodeNum, LOAD_PWM | REVERSE | START_NOW, 0, 0, 0, (UCHAR)abs((int)output))))
					return FALSE;
			} else {
				if (!(ret = m_pPicServo->ServoLoadTraj(m_nNodeNum, LOAD_PWM | START_NOW, 0, 0, 0, (UCHAR)abs((int)output))))
					return FALSE;
			}

			last_Perr = Perr;
			last_time=time_now;
			
		}
	} else { // zero out variables
#if defined(WIN32)
		last_time = GetTickCount();
#elif defined(_QNX)
		clock_gettime(CLOCK_REALTIME, &timeval);
		last_time =
		  (double)(timeval.tv_sec+timeval.tv_nsec/1.0e9);
#endif
		Ierr = 0.0;
		last_Perr = 0.0;
		last_output = 0.0;
	}

	return (int)ret;
}


int CProp::Update()
{
	static long last_pos = 0;
#if defined(WIN32)
	static DWORD last_time = GetTickCount();
	DWORD time_now  = GetTickCount(); 
#elif defined(_QNX)
	struct timespec timeval;
        clock_gettime(CLOCK_REALTIME, &timeval);
	static double last_time =
	  (double)(timeval.tv_sec+timeval.tv_nsec/1.0e9);
        clock_gettime(CLOCK_REALTIME, &timeval);
	double time_now =
	  (double)(timeval.tv_sec+timeval.tv_nsec/1.0e9);
#endif
	BOOL ret = FALSE;
	


	if (m_pPicServo->nummod == m_nNumNodes)
	{
		ret = m_pPicServo->NmcNoOp(m_nNodeNum); // get latest register values
		if (!ret) // problems, so get out!
			return (int)ret;
	}

	m_nCurPos = m_pPicServo->ServoGetPos(m_nNodeNum);	
	double speed = (double)(m_nCurPos - last_pos) / (double)(time_now - last_time); // ticks/ms
	speed = speed / (double)PROP_COUNTS_PER_REV / (double)PROP_GEARBOX * 60.0; // RPM
	m_nUnfiltSpeed = (int)speed;
	//m_nCurSpeed = (int) (speed*0.15 + m_nCurSpeed*0.85); // filtered RPM
	m_nCurSpeed = (int) (speed*1.00 + m_nCurSpeed*0.00); // filtered RPM
	m_cStatus = m_pPicServo->NmcGetStat(m_nNodeNum);
	m_bAtSpeed = m_cStatus & MOVE_DONE;
	//	RefreshWD();

	m_fCurrent = m_pPicServo->ServoGetAD(m_nNodeNum) * 0.0366368111116729 + 0.0553288713686155; // amps, given 8 bit ADC
	m_nTorque = (int)(m_pPicServo->ServoGetAD(m_nNodeNum) * 2.48743304491142 + -24.5048968336982); // ounce*inches, given 8 bit ADC, at 300RPM
	if (m_nTorque < 0) m_nTorque = 0;

	// See if the motor is stopped and disable amp if using the PIC-Servo servo
#ifndef SOFTWARE_SPEED_CONTROL
	if (m_cStatus & MOVE_DONE && abs(m_nCurSpeed) < 10 && m_nSetSpeed==0)
	{
		m_pPicServo->ServoStopMotor(m_nNodeNum, MOTOR_OFF); // disable AMP
		m_bAmpEn = FALSE;
	}
#endif //SOFTWARE_SPEED_CONTROL
	
	// Check for Servo Position Error if using the PIC-Servo servo
#ifndef SOFTWARE_SPEED_CONTROL
	if (m_cStatus & POS_ERR && m_bAmpEn)
	{
		m_pPicServo->ServoStopMotor(m_nNodeNum, MOTOR_OFF); // disable AMP
		m_pPicServo->ServoStopMotor(m_nNodeNum, STOP_HERE); // turn on servo
		m_pPicServo->ServoClearBits(m_nNodeNum); // clears the pos_err flag
		// I wouldn't do the following if reverse is enabled
		m_pPicServo->ServoStopMotor(m_nNodeNum, AMP_ENABLE); // enable servo
		SetSpeed(m_nSetSpeed);

		m_bPositionError = TRUE;
	}
#endif //SOFTWARE_SPEED_CONTROL

	last_pos = m_nCurPos;
	last_time = time_now;

#ifdef SOFTWARE_SPEED_CONTROL
	CloseSpeedLoop();
#endif //SOFTWARE_SPEED_CONTROL

	return (int)ret;
}

void CProp::RefreshWD ()
{
	static UCHAR flag = 0; //need to toggle this each run to refresh WD
	
	flag = flag ^ SET_OUT1; // toggle the flag between 0 and 1
	m_pPicServo->ServoSetIoCtrl(m_nNodeNum,  (m_bEnableReverse ? SET_OUT2 : 0) | flag);
}

void CProp::SetParams (int kP, int kD, int kI, int Plim, int Ilim, int Cmdlim, int Accel, int Deadband)
{
	m_params.Kp = kP;
	m_params.Kd = kD;
	m_params.Ki = kI;
	m_params.Plim = Plim;
	m_params.Ilim = Ilim;
	m_params.pwm_lim = Cmdlim;
	m_params.accel = Accel;
	m_params.deadband = Deadband;

#ifndef SOFTWARE_SPEED_CONTROL
	if (m_pPicServo)
		if (m_pPicServo->nummod == m_nNumNodes)
			m_pPicServo->ServoSetGain(m_nNodeNum, m_params.Kp, m_params.Kd, m_params.Ki, m_params.Ilim,
				m_params.pwm_lim, 0, m_params.pos_err_lim, m_params.rate, 0);
#endif // SOFTWARE_SPEED_CONTROL

}

void CProp::SetReverse (BOOL enable)
{
	m_bEnableReverse = enable;
	// the next line screws with the W/D, but asuming the update rate is fast enough, nothing
	//   bad should happen.
	m_pPicServo->ServoSetIoCtrl(m_nNodeNum,  (m_bEnableReverse ? SET_OUT2 : 0));
}

#ifdef WIN32
void CProp::LoadParams() // loads in new settings from registry
{
	CString strKey;
	
	strKey.Format("Software\\MBARI\\Dorado\\NMCNodes\\%d", m_nNodeNum);

	CRegistry reg(HKEY_LOCAL_MACHINE);
	CRegVal regval;
 	CString sStringName;
	DWORD value;

	sStringName = "Kp";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)PROP_DEF_KP);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.Kp = (int)value;

	sStringName = "Kd";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)PROP_DEF_KD);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.Kd = (int)value;

	sStringName = "Ki";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)PROP_DEF_KI);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.Ki = (int)value;

	sStringName = "rate";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)PROP_DEF_RATE);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.rate = (int)value;

	sStringName = "deadband";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)PROP_DEF_DEADBAND);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.deadband = (int)value;

	sStringName = "acceleration";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)PROP_DEF_ACCEL);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.accel = (double)value;

	sStringName = "PWM limit";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)PROP_DEF_PWM_LIM);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.pwm_lim = (int)value;

	sStringName = "Pos Error Limit";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)PROP_DEF_POS_ERR_LIM);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.pos_err_lim = (int)value;

}
#endif