//---------------------------------------------------------------------------
#include <stdio.h>

#include "sio_util.h"
#include "nmccom.h"
#include "picservo.h"
//---------------------------------------------------------------------------
extern NMCMOD	mod[]; 		//Array of modules
extern int nummod;
extern HANDLE ComPort;

//---------------------------------------------------------------------------
//Returns pointer to an initialized IOMOD structure
SERVOMOD * ServoNewMod()
{
SERVOMOD *p;

p = new SERVOMOD;
p->pos = 0;
p->ad = 0;
p->vel = 0;
p->aux = 0;
p->home = 0;
p->perror = 0;
p->cmdpos = 0;
p->cmdvel = 0;
p->cmdacc = 0;
p->cmdpwm = 0;

(p->gain).kp = 0;
(p->gain).kd = 0;
(p->gain).ki = 0;
(p->gain).il = 0;
(p->gain).ol = 0;
(p->gain).cl = 0;
(p->gain).el = 0;
(p->gain).sr = 1;
(p->gain).dc = 0;
(p->gain).sm = 1;

p->stoppos = 0;
p->ioctrl = 0;
p->homectrl = 0;
p->movectrl = 0;
p->stopctrl = 0;
p->ph_adv = 0;
p->ph_off = 0;
return p;
}

//---------------------------------------------------------------------------
BOOL ServoGetStat(byte addr)
{
int numbytes, numrcvd;
int i, bytecount;
byte cksum;
byte inbuf[20];
SERVOMOD *p;
char msgstr[80];

p = (SERVOMOD *)(mod[addr].p);  //cast the data pointer to the right type

//Find number of bytes to read:
numbytes = 2;       //start with stat & cksum
if ( (mod[addr].statusitems) & SEND_POS )	numbytes +=4;
if ( (mod[addr].statusitems) & SEND_AD ) 	numbytes +=1;
if ( (mod[addr].statusitems) & SEND_VEL ) 	numbytes +=2;
if ( (mod[addr].statusitems) & SEND_AUX ) 	numbytes +=1;
if ( (mod[addr].statusitems) & SEND_HOME )	numbytes +=4;
if ( (mod[addr].statusitems) & SEND_ID ) 	numbytes +=2;
if ( (mod[addr].statusitems) & SEND_PERROR ) numbytes +=2;
if ( (mod[addr].statusitems) & SEND_NPOINTS ) numbytes +=1;
numrcvd = SioGetChars(ComPort, (char *)inbuf, numbytes);

//Verify enough data was read
if (numrcvd != numbytes)
	{
    sprintf(msgstr,"ServoGetStat (%d) failed to read chars",addr);
    ErrorMsgBox(msgstr);
    return false;
    }

//Verify checksum:
cksum = 0;
for (i=0; i<numbytes-1; i++) cksum = (byte)(cksum + inbuf[i]);
if (cksum != inbuf[numbytes-1])
	{
    sprintf(msgstr,"ServoGetStat(%d): checksum error",addr);
    ErrorMsgBox(msgstr);
    return false;
    }

//Verify command was received intact before updating status data
mod[addr].stat = inbuf[0];
if (mod[addr].stat & CKSUM_ERROR)
	{
    ErrorMsgBox("Command checksum error!");
    return false;
    }

//Finally, fill in status data
bytecount = 1;
if ( (mod[addr].statusitems) & SEND_POS )
	{
	p->pos = *( (long *)(inbuf + bytecount) );
    bytecount +=4;
    }
if ( (mod[addr].statusitems) & SEND_AD )
	{
    p->ad = inbuf[bytecount];
    bytecount +=1;
    }
if ( (mod[addr].statusitems) & SEND_VEL )
	{
	p->vel = *( (short int *)(inbuf + bytecount) );
    bytecount +=2;
    }
if ( (mod[addr].statusitems) & SEND_AUX )
	{
    p->aux = inbuf[bytecount];
    bytecount +=1;
    }
if ( (mod[addr].statusitems) & SEND_HOME )
	{
    p->home = *( (unsigned long *)(inbuf + bytecount) );
    bytecount +=4;
    }
if ( (mod[addr].statusitems) & SEND_ID )
	{
    mod[addr].modtype = inbuf[bytecount];
    mod[addr].modver = inbuf[bytecount+1];
    bytecount +=2;
    }
if ( (mod[addr].statusitems) & SEND_PERROR )
	{
	p->perror = *( (short int *)(inbuf + bytecount) );
    bytecount +=2;
    }
if ( (mod[addr].statusitems) & SEND_NPOINTS )
	{
    p->npoints = inbuf[bytecount];
    //bytecount +=1;
    }

return TRUE;
}
//---------------------------------------------------------------------------
long ServoGetPos(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->pos;
}
//---------------------------------------------------------------------------
byte ServoGetAD(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->ad;
}
//---------------------------------------------------------------------------
short int ServoGetVel(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->vel;
}
//---------------------------------------------------------------------------
byte ServoGetAux(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->aux;
}
//---------------------------------------------------------------------------
long ServoGetHome(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->home;
}
//---------------------------------------------------------------------------
short int ServoGetPError(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->perror;
}
//---------------------------------------------------------------------------
byte ServoGetNPoints(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->npoints;
}
//---------------------------------------------------------------------------
long ServoGetCmdPos(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->cmdpos;
}
//---------------------------------------------------------------------------
long ServoGetCmdVel(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->cmdvel;
}
//---------------------------------------------------------------------------
long ServoGetCmdAcc(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->cmdacc;
}
//---------------------------------------------------------------------------
long ServoGetStopPos(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->stoppos;
}
//---------------------------------------------------------------------------
byte ServoGetCmdPwm(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->cmdpwm;
}
//---------------------------------------------------------------------------
byte ServoGetIoCtrl(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->ioctrl;
}
//---------------------------------------------------------------------------
byte ServoGetHomeCtrl(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->homectrl;
}
//---------------------------------------------------------------------------
byte ServoGetStopCtrl(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->stopctrl;
}
//---------------------------------------------------------------------------
byte ServoGetMoveCtrl(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->movectrl;
}
//---------------------------------------------------------------------------
byte ServoGetPhAdv(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->ph_adv;
}
//---------------------------------------------------------------------------
byte ServoGetPhOff(byte addr)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
return p->ph_off;
}
//---------------------------------------------------------------------------
void ServoGetGain(byte addr, short int * kp, short int * kd, short int * ki,
				  short int * il, byte * ol, byte * cl, short int * el,
                  byte * sr, byte * dc)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
*kp = (p->gain).kp;
*kd = (p->gain).kd;
*ki = (p->gain).ki;
*il = (p->gain).il;
*ol = (p->gain).ol;
*cl = (p->gain).cl;
*el = (p->gain).el;
*sr = (p->gain).sr;
*dc = (p->gain).dc;
}
//---------------------------------------------------------------------------
void ServoGetGain2(byte addr, short int * kp, short int * kd, short int * ki,
				  short int * il, byte * ol, byte * cl, short int * el,
                  byte * sr, byte * dc, byte * sm)
{