//---------------------------------------------------------------------------
#ifdef WIN32
#include "stdafx.h"
#endif
#ifdef _QNX
#include <stdlib.h>
#include <i86.h>
#include "Syslog.h"
#endif

#include <stdio.h>
#pragma hdrstop

#include "PicServo.h"
//#include "picio.h"
//#include "picservo.h"
//#include "mainform.h"
//#include "sio_util.h"
//---------------------------------------------------------------------------
//Global data



//---------------------------------------------------------------------------
CPicServo::ADCMOD * CPicServo::AdcNewMod()
{
return NULL;
}

//---------------------------------------------------------------------------
//Reset all controllers with group address 'addr' (should include all modules)
BOOL CPicServo::NmcHardReset(byte addr)
{
int i;
char cstr[5];

//Send string of 0's to flush input buffers
cstr[0] = 0;
for (i=0; i<20; i++) SioPutChars(cstr, 1);

//Send out reset command string
cstr[0] = 0xAA;			//Header
cstr[1] = addr;
cstr[2] = HARD_RESET;
cstr[3] = (byte)(cstr[1] + cstr[2]);  //checksum
SioPutChars(cstr, 4);//send reset

#ifdef WIN32
Sleep(100);		//wait for reset to execute
#elseif _QNX
delay(100);
#endif

for (i=0; i<nummod; i++) 
	if (mod[i].p)
		delete mod[i].p;  //delete any prev. declared modules
nummod = 0;
SioError = 0;	//reset the number of errors to 0

SioChangeBaud(19200);	//Reset the baud rate to the default

SioClrInbuf();   //clear out any random crap left in buffer

return TRUE;
}

//---------------------------------------------------------------------------
//Change the baud rate of all controllers with group address 'groupaddr'
//(should include all modules) and also changes host's baud rate.
//There should be no group leader for 'addr'
BOOL CPicServo::NmcChangeBaud(byte groupaddr, unsigned int baudrate)
{
char cstr[6];

cstr[0] = 0xAA;			//Header
cstr[1] = groupaddr;
cstr[2] = 0x10 | SET_BAUD;
switch (baudrate) {
	case 19200:		cstr[3] = PB19200; break;
	case 57600:		cstr[3] = PB57600; break;
	case 115200:	cstr[3] = PB115200; break;
    default:	ErrorMsgBox("Baud rate not supported - using default of 19200");
    			cstr[3] = PB19200;
                baudrate = 19200;
    }
cstr[4] = (byte)(cstr[1] + cstr[2] + cstr[3]);  //checksum
SioPutChars(cstr, 5);//send command
#ifdef WIN32
Sleep(100);		//Wait for command to execute
#elseif _QNX
delay(100);
#endif

SioChangeBaud(baudrate);	//Reset the baud rate to the default

SioClrInbuf();   //clear out any random crap left in buffer
#ifdef WIN32
Sleep(100);
#elseif _QNX
delay(100);
#endif

BaudRate = baudrate;
return true;
}

//---------------------------------------------------------------------------
//Initialize misc network variables
void CPicServo::InitVars(void)
{
int i;

mod[0].modtype = 0;   //Default to a known module type for module 0
mod[0].modver = 0;
mod[0].stat = 0;
mod[0].statusitems = 0;
mod[0].groupaddr = 0xFF;
mod[0].groupleader = false;

for (i=1; i<MAXNUMMOD; i++)
	{
	mod[i].modtype = 0xFF;
	mod[i].modver = 0;
    mod[i].stat = 0;
    mod[i].statusitems = 0;
    mod[i].groupaddr = 0xFF;
    mod[i].groupleader = false;
    }
}
//---------------------------------------------------------------------------
//Initialize the network of controllers with sequential addresses starting
//at 0.  Retunrs the number of controllers found.
int CPicServo:: NmcInit(unsigned int port, unsigned int baudrate)
{
DWORD numread;
byte addr;
char cstr[20];
char mess[40];
char name[20], id[10];	

BaudRate = baudrate;

InitVars();
_itoa( port, id, 10 ); 
#ifdef WIN32
strcpy( name, "\\\\.\\COM"); 
strcat( name, id);
#endif
#ifdef _QNX
sprintf( name, "//1/dev/ser%d", port);
#endif

SioOpen(name, 19200);    //Open with default rate of 19200
if (ComPort == INVALID_HANDLE_VALUE) return 0;

NmcHardReset(0xFF);
NmcHardReset(0xFF);

addr = 1;
while (1)
	{
    //First set the address to a unique value:
    cstr[0] = 0xAA;		//Header
    cstr[1] = 0;  		//Send to default address of 0
    cstr[2] = 0x20 | SET_ADDR;
    cstr[3] = addr;  	//Set new address sequentially
    cstr[4] = 0xFF;		//Set group address to 0xFF
    cstr[5] = (byte)(cstr[1] + cstr[2] + cstr[3] + cstr[4]);  //checksum
    SioPutChars(cstr, 6);  //Send command

    numread = SioGetChars(cstr, 2);  //get back status, cksum
    if (numread!=2) break;	//if no response, punt out of loop
    if (cstr[0] != cstr[1])
    	{
        ErrorMsgBox("Status checksum error - please reset the Network");
        SioClose();
        return(0);
        }

    //Read the device type
    cstr[0] = 0xAA;		//Header
    cstr[1] = addr;  		//Send to addr
    cstr[2] = 0x10 | READ_STAT;
    cstr[3] = SEND_ID;  //send ID only
    cstr[4] = (byte)(cstr[1] + cstr[2] + cstr[3]);  //checksum
    SioPutChars(cstr, 5);  //Send command

    numread = SioGetChars(cstr, 4);  //get back stat, type, ver, cksum
    if (numread!=4)
    	{
        ErrorMsgBox("Could not read device type");
        SioClose();
        return(0);
        }
    mod[addr].stat = cstr[0];
    mod[addr].modtype = cstr[1];
    mod[addr].modver = cstr[2];
    mod[addr].statusitems = 0;
    mod[addr].groupaddr = 0xFF;
    mod[addr].groupleader = false;
    switch (mod[addr].modtype)
    	{
        case SERVOMODTYPE: 	
			mod[addr].p = ServoNewMod(); 
			break;
		case STEPMODTYPE:	
			mod[addr].p = StepNewMod(); 
			break;
        case ADCMODTYPE: 	
			mod[addr].p = AdcNewMod(); 
			break;
        case IOMODTYPE: 	
			mod[addr].p = IoNewMod(); 
			break;
		default: SimpleMsgBox("HAHA");
        }

    addr++;		//increment the address
    }

nummod = addr-1;

if (nummod > 0)
	{
    NmcChangeBaud(0xFF, baudrate);
	if (nummod>1) sprintf(mess,"%d Modules found", nummod);
    else sprintf(mess,"%d Module found", nummod);
	ErrorMsgBox(mess);
    }
else
	{
    ErrorMsgBox("No modules found on the network.\nPlease check power and connections.");
	ComPort=INVALID_HANDLE_VALUE;
    SioClose();
    }

return(nummod);	//Return number of modules
}

#if defined(_QNX)
int CPicServo::NmcInit(SerialDevice *device, unsigned int baudrate)
{
DWORD numread;
byte addr;
char cstr[20];
char mess[40];
char name[20], id[10];	

BaudRate = baudrate;

InitVars();
ComPort = device;
SioChangeBaud(19200);
if (ComPort == INVALID_HANDLE_VALUE) return 0;

NmcHardReset(0xFF);
NmcHardReset(0xFF);

addr = 1;
while (1)
	{
    //First set the address to a unique value:
    cstr[0] = 0xAA;		//Header
    cstr[1] = 0;  		//Send to default address of 0
    cstr[2] = 0x20 | SET_ADDR;
    cstr[3] = addr;  	//Set new address sequentially
    cstr[4] = 0xFF;		//Set group address to 0xFF
    cstr[5] = (byte)(cstr[1] + cstr[2] + cstr[3] + cstr[4]);  //checksum
    SioPutChars(cstr, 6);  //Send command

    numread = SioGetChars(cstr, 2);  //get back status, cksum
    if (numread!=2) break;	//if no response, punt out of loop
    if (cstr[0] != cstr[1])
    	{
        ErrorMsgBox("Status checksum error - please reset the Network");
        SioClose();
        return(0);
        }

    //Read the device type
    cstr[0] = 0xAA;		//Header
    cstr[1] = addr;  		//Send to addr
    cstr[2] = 0x10 | READ_STAT;
    cstr[3] = SEND_ID;  //send ID only
    cstr[4] = (byte)(cstr[1] + cstr[2] + cstr[3]);  //checksum
    SioPutChars(cstr, 5);  //Send command

    numread = SioGetChars(cstr, 4);  //get back stat, type, ver, cksum
    if (numread!=4)
    	{
        ErrorMsgBox("Could not read device type");
        SioClose();
        return(0);
        }
    mod[addr].stat = cstr[0];
    mod[addr].modtype = cstr[1];
    mod[addr].modver = cstr[2];
    mod[addr].statusitems = 0;
    mod[addr].groupaddr = 0xFF;
    mod[addr].groupleader = false;
    switch (mod[addr].modtype)
    	{
        case SERVOMODTYPE: 	
			mod[addr].p = ServoNewMod(); 
			break;
		case STEPMODTYPE:	
			mod[addr].p = StepNewMod(); 
			break;
        case ADCMODTYPE: 	
			mod[addr].p = AdcNewMod(); 
			break;
        case IOMODTYPE: 	
			mod[addr].p = IoNewMod(); 
			break;
		default: SimpleMsgBox("HAHA");
        }

    addr++;		//increment the address
    }

nummod = addr-1;

if (nummod > 0)
	{
    NmcChangeBaud(0xFF, baudrate);
	if (nummod>1) sprintf(mess,"%d Modules found", nummod);
    else sprintf(mess,"%d Module found", nummod);
	ErrorMsgBox(mess);
    }
else
	{
    ErrorMsgBox("No modules found on the network.\nPlease check power and connections.");
	ComPort=INVALID_HANDLE_VALUE;
    SioClose();
    }

return(nummod);	//Return number of modules
}
#endif

//---------------------------------------------------------------------------
BOOL CPicServo:: AdcGetStat(byte addr)
{
return false;
}
//---------------------------------------------------------------------------
//Sends a command to addr and shoves the returned status data in the
//structure for module address stataddr.  If stataddr = 0, the command
//is taken as a group command with no group leader (no status returned).
//n is the number of auxilliary data bytes.
BOOL CPicServo:: NmcSendCmd(byte addr, byte cmd, char *datastr, byte n, byte stataddr)
{
byte cksum;
char mess[40];
byte outstr[20];
int i;
BOOL iostat;

//If too many SIO errors, punt
if (SioError > MAXSIOERROR) return(false);

IOBusy = true;

//Check if data is to be returned from to a known module type
if ( !( ( mod[stataddr].modtype==SERVOMODTYPE) ||
        ( mod[stataddr].modtype==ADCMODTYPE) ||
        ( mod[stataddr].modtype==IOMODTYPE) ||
		( mod[stataddr].modtype==STEPMODTYPE) ) )
	{
    sprintf(mess,"Module type %d not supported",mod[stataddr].modtype);
    ErrorMsgBox(mess);
    return (false);
    }

//Calculate the adjust command byte, calculate checksum and send the command
cksum = 0;
outstr[0] = 0xAA;	//start with header byte
outstr[1] = addr;	//add on address byte
cksum += outstr[1];
outstr[2] = (byte)(((n<<4) & 0xF0) | cmd);
cksum += outstr[2];
for (i=0; i<n; i++)
	{
    outstr[i+3] = datastr[i];
	cksum += outstr[i+3];
    }
outstr[n+3] = cksum;

SioClrInbuf();  //Get rid of any old input chars
SioPutChars((char *)outstr, n+4); //Send the command string

if (stataddr == 0)		//If a group command w/ no leader, add delay then exit
	{
#ifdef WIN32
    Sleep(60);
#elseif _QNX
    delay(1);
#endif
    IOBusy = false;
	return true;
    }

switch (mod[stataddr].modtype) {
	case SERVOMODTYPE:	iostat = ServoGetStat(stataddr); break;
	case ADCMODTYPE:	iostat = AdcGetStat(stataddr); break;
	case IOMODTYPE:		iostat = IoGetStat(stataddr); break;
	case STEPMODTYPE:	iostat = StepGetStat(stataddr); break;
    }

if (iostat == false)
	{
    SioError++;
    FixSioError(stataddr);
    }
else SioError = 0;

IOBusy = false;
return iostat;
}

//---------------------------------------------------------------------------
//Attempt to re-synch communications
void CPicServo:: FixSioError(byte addr)
{
int i;
char teststr[2];

//If too many errors - prompt to reset
if (SioError >= MAXSIOERROR)
	{
    ErrorMsgBox("Multiple communication errors - please the Network");
    return;
    }

//Otherwise, send out a null string to re-synch
teststr[0] = 0;
for (i=0; i<30; i++) SioPutChars(teststr, 1);
#ifdef WIN32
Sleep(100);
#elseif _QNX
delay(100);
#endif
SioClrInbuf();
}
//---------------------------------------------------------------------------
BOOL CPicServo:: NmcSetGroupAddr(byte addr, byte groupaddr, BOOL leader)
{
char cmdstr[2];

if (!(groupaddr & 0x80)) return false;  //punt if not valid group address

cmdstr[0] = addr;
cmdstr[1] = groupaddr;
if (leader) cmdstr[1] &= 0x7F;  //clear upper bit if a group leader
mod[addr].groupaddr = groupaddr;
mod[addr].groupleader = leader;
return NmcSendCmd(addr, SET_ADDR, cmdstr, 2, addr);
}
//---------------------------------------------------------------------------
BOOL CPicServo:: NmcSynchOutput(byte groupaddr, byte leaderaddr)
//Synchronous output command issued to groupaddr.  Status data goes to
//the leaderaddr.  If no leader, use leaderaddr = 0.
{
return NmcSendCmd(groupaddr, SYNCH_OUT, NULL, 0, leaderaddr);
}
//---------------------------------------------------------------------------
//Synchronous input command issued to groupaddr.  Status data goes to
//the leaderaddr.  If no leader, use leaderaddr = 0.
BOOL CPicServo:: NmcSynchInput(byte groupaddr, byte leaderaddr)
{
return NmcSendCmd(groupaddr, SYNCH_INPUT, NULL, 0, leaderaddr);
}
//---------------------------------------------------------------------------
BOOL CPicServo:: NmcNoOp(byte addr)
{
return NmcSendCmd(addr, NOP, NULL, 0, addr);
}
//---------------------------------------------------------------------------
BOOL CPicServo:: NmcReadStatus(byte addr, byte statusitems)
{
char cmdstr[2];
byte oldstat;
BOOL retval;

cmdstr[0] = statusitems;
oldstat = mod[addr].statusitems;
mod[addr].statusitems = statusitems;
retval = NmcSendCmd(addr, READ_STAT, cmdstr, 1, addr);
mod[addr].statusitems = oldstat;

return retval;
}
//---------------------------------------------------------------------------
BOOL CPicServo:: NmcDefineStatus(byte addr, byte statusitems)
{
char cmdstr[2];

cmdstr[0] = statusitems;
mod[addr].statusitems = statusitems;
return NmcSendCmd(addr, DEF_STAT, cmdstr, 1, addr);
}
//---------------------------------------------------------------------------
CPicServo::byte CPicServo:: NmcGetStat(byte addr)
{
return mod[addr].stat;
}
//---------------------------------------------------------------------------
CPicServo::byte CPicServo:: NmcGetStatItems(byte addr)
{
return mod[addr].statusitems;
}
//---------------------------------------------------------------------------
CPicServo::byte CPicServo:: NmcGetModType(byte addr)
{
return mod[addr].modtype;
}
//---------------------------------------------------------------------------
CPicServo::byte CPicServo:: NmcGetModVer(byte addr)
{
return mod[addr].modver;
}
//---------------------------------------------------------------------------
CPicServo::byte CPicServo:: NmcGetGroupAddr(byte addr)
{
return mod[addr].groupaddr;
}
//---------------------------------------------------------------------------
BOOL CPicServo:: NmcGroupLeader(byte addr)
{
return mod[addr].groupleader;
}
//---------------------------------------------------------------------------
void CPicServo:: NmcShutdown(void)
{
//int retval;

//retval = MessageBox(NULL, "Do you wish to reset the NMC network?",
//                                 "",MB_YESNO | MB_DEFBUTTON1 | MB_TASKMODAL);
//
//if (retval == IDYES)
//	{
//	if (ComPort!=INVALID_HANDLE_VALUE && ComPort!=NULL) NmcHardReset(0xFF);
//    }
//else
//	{
//    if (BaudRate != 19200)
//      	{
//      	ErrorMsgBox("Returning NMC Baud rate to 19200 (default)");
//      	NmcChangeBaud(0xFF, 19200);
//      	}
//    }
if (ComPort!=INVALID_HANDLE_VALUE && ComPort!=NULL) NmcHardReset(0xFF);

nummod = 0;    
SioClose();
}
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
CPicServo::CPicServo()
{
//char cmdstr[20];			//use global command string
SioError = 0;
IOBusy = false;
printerrors=1;
ComPort=INVALID_HANDLE_VALUE;
nummod=0; //system hasn't yet found any modules
}