#if defined(WIN32)
#include "stdafx.h"
#include "Registry.h"
#elif defined(_QNX)
#include <stdlib.h>
#include <assert.h>
#define ASSERT(val) assert(val)
#include "Syslog.h"
#endif
#include <math.h>
#include "CStepper.h"


static int iabs(int i)
{
  if(i < 0)
    i = -1*i;
  return i;
}

CStepper::CStepper()
{
	m_bAmpEn = FALSE;
        _currentSteps = _goalSteps = 0;
	m_cStatus = 0;
	m_cInputs = 0;
}

CStepper::~CStepper()
{
	//DisableAmp();
}

void CStepper::Initialize(CPicServo *pic_servo, int num_nodes)
{

	//	m_nNodeNum = node_num;
	m_nNumNodes = num_nodes;
	m_pPicServo = pic_servo;
	_currentSteps = _goalSteps = 0;

	if (m_pPicServo->nummod == m_nNumNodes) {
	  // disable AMP, stop here
	  m_pPicServo->StepStopMotor(m_nNodeNum, STOP_ABRUPT);
	  m_pPicServo->StepSetParam(m_nNodeNum,
				    SPEED_2X | IGNORE_LIMITS | IGNORE_ESTOP, 
				    m_params.minspeed,
				    m_params.runcur,
				    m_params.holdcur, 0);
	  m_pPicServo->NmcDefineStatus(m_nNodeNum,
				       SEND_POS | SEND_AD | SEND_INBYTE);
	  m_pPicServo->StepSetOutputs(m_nNodeNum,
				      0x08 | 0x04 | 0x02); // OUT4,3,2 asserted
	  m_pPicServo->StepLoadTraj(m_nNodeNum,
				    LOAD_SPEED | LOAD_ACC, 0,
				    m_params.speed, m_params.accel, 0.0f);

	  _currentSteps = m_pPicServo->StepGetPos(m_nNodeNum);
	  _A2D = m_pPicServo->StepGetAD(m_nNodeNum);
	}
}

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

	dprintf("CStepper::EnableAmp enabling amp %d\n", m_nNodeNum);
	if (m_pPicServo->nummod == m_nNumNodes)
		m_pPicServo->StepStopMotor(m_nNodeNum, AMP_ENABLE);
}

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

	dprintf("CStepper::DisableAmp disabling amp %d\n", m_nNodeNum);	
	if (m_pPicServo->nummod == m_nNumNodes)
		m_pPicServo->StepStopMotor(m_nNodeNum, STOP_ABRUPT); // disable AMP, stop here
}

int CStepper::SetDestPos(int pos)
{
  int ret = TRUE;
  _goalSteps = pos;

  if (m_pPicServo->nummod == m_nNumNodes) {
    if (!(m_cStatus & MOTOR_MOVING)) {
      if (!m_pPicServo->StepLoadTraj(m_nNodeNum,
				     LOAD_POS | LOAD_SPEED |
				     LOAD_ACC | START_NOW, pos,
				     m_params.speed, m_params.accel, 0.0f))
	ret = FALSE;
    }
  }
  return ret;
}

int CStepper::ClosePosLoop()
{
int errorSteps, potSteps, cmdSteps;

    //reset if we see more than 2 A/D cnts error
    // convert to stepper motor steps here
    errorSteps =  _Pot1CntsToSteps * 12;
    cmdSteps = _currentSteps - _offset;

    //convert from A/D counts to steps
    potSteps = (_A2D-_Pot1Home) * _Pot1CntsToSteps;

    if (!(m_cStatus & MOTOR_MOVING)) { //have we stopped moving
      if (iabs(potSteps - cmdSteps) >= errorSteps) {
	_offset = _currentSteps - potSteps; //if we're not, them rehome
	//	return SetDestPos(cmdSteps + _offset);
      }
    }
    return True;
}

int CStepper::Update()
{
	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_cInputs = m_pPicServo->StepGetInbyte(m_nNodeNum);
	m_cStatus = m_pPicServo->NmcGetStat(m_nNodeNum);
	_currentSteps = m_pPicServo->StepGetPos(m_nNodeNum);
	m_bAmpEn = m_cStatus & AMP_ENABLED;
	_A2D = m_pPicServo->StepGetAD(m_nNodeNum);
	return True;
}

#if defined(WIN32)
void CStepper::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, sPosGains, sVelGains, sMisc;
	DWORD value;

	sStringName = "minspeed";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)STEP_DEF_MINSPEED);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.minspeed = (UCHAR)value;

	sStringName = "runcur";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)STEP_DEF_RUNCUR);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.runcur = (UCHAR)value;

	sStringName = "holdcur";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)STEP_DEF_HOLDCUR);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.holdcur = (UCHAR)value;

	sStringName = "speed";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)STEP_DEF_SPEED);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.speed = (UCHAR)value;

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

	sStringName = "max_pot";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)STEP_DEF_MAX_POT);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.max_pot = (UCHAR)value;

	sStringName = "min_pot";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)STEP_DEF_MIN_POT);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.min_pot = (UCHAR)value;

	sStringName = "zero_pot";
	if (reg.LoadKey (strKey, sStringName, regval))
	{
		regval.GetValue(value);
	} else {
		regval.SetValue(value=(DWORD)STEP_DEF_ZERO_POT);
		reg.SaveKey( strKey, sStringName, value);
	}
	m_params.zero_pot = (UCHAR)value;

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

void CStepper::SaveParams() // saves settings to registry
{
	CString strKey, sStringName;
	
	strKey.Format("Sofware\\MBARI\\Dorado\\NMCNodes\\%d", m_nNodeNum);

	CRegistry reg(HKEY_LOCAL_MACHINE);
	CRegVal regval;

	sStringName = "minspeed";
	reg.SaveKey( strKey, sStringName, m_params.minspeed);

	sStringName = "runcur";
	reg.SaveKey( strKey, sStringName, m_params.runcur);

	sStringName = "holdcur";
	reg.SaveKey( strKey, sStringName, m_params.holdcur);

	sStringName = "speed";
	reg.SaveKey( strKey, sStringName, m_params.speed);

	sStringName = "accel";
	reg.SaveKey( strKey, sStringName, m_params.accel);

	sStringName = "max_pot";
	reg.SaveKey( strKey, sStringName, m_params.max_pot);

	sStringName = "min_pot";
	reg.SaveKey( strKey, sStringName, m_params.min_pot);

	sStringName = "zero_pot";
	reg.SaveKey( strKey, sStringName, m_params.zero_pot);

	sStringName = "total_steps";
	reg.SaveKey( strKey, sStringName, m_params.total_steps);
}
#elif defined(_QNX)
void CStepper::LoadParams() // loads in new settings from registry
{
}

void CStepper::SaveParams() // saves settings to registry
{
}
#endif