/*	PONTECH Copyright 1996
 *  Default.C
 *  	This program allow the user to change the power-up
 *    value of the servo, board ID number and some other
 *		initial settings.
 */

#include <stdio.h>
#include <stdlib.h>
#include "comm.h"
#include "comm.c"	/* don't need this line if include in project */

int initPort(int commPort);

void displayCurr(void);
int setNewValue(void);

int getNumFromSV200(void);

int getBoardID(void);
int getInitServo(int servo);
int getBaudRate(void);
int getPreEnable(void);
int getSPIConfig(void);

void setBoardID(int id);
void setInitServo(int servo,int pos);
void setBaudRate(int baud);
void setPreEnable(int preEn);
void setSPIConfig(int spiConfig);

/* Global */
char numStr[10];
int baudRate, rcvWait;

int main()
{
	int port, cont = 1;

	clrscr();

	port = COM1;
	if (!initPort(port))
	{
		port = COM2;
		if (!initPort(port))
		{
			port = COM3;
			if (!initPort(port))
			{
				port = COM4;
				if (!initPort(port))
				{
					printf("SV200 Not Founded\n");
					return 0;
				}
			}
		}
	}

	serlXmitStr("BD1\n");

	printf("SV200 Founded on COM%d, %d baud\n\n", port+1, baudRate);

	while(cont)
	{
		displayCurr();
		cont = setNewValue();
		clrscr();
	}

	serlClosePort(port);
	return 1;		
}

int initPort(int commPort)
{
	char ch = ' ';
	int i;

	serlClrBuff();
	serlSetIntr(commPort);   	/* Port 0 (COM1) */

	baudRate = 9600; 					/* Start With 9600 Baud */
	rcvWait = 10;
	for (i = 0; i < 4; i++)
	{
		serlPortInit(commPort,baudRate,0,8,1);

		serlXmitStr(" \n");
		serlXmitStr("\n");
		delay(50);			/* wait until SV200 send enough info */

		while(serlAvl())
		{
			ch = serlRcv();
			if (ch == '>')
				return 1;
		}

		switch (i)
		{
		case 0:
			baudRate = 2400;
			rcvWait = 30;
			break;
		case 1:
			baudRate = 4800;
			rcvWait = 20;
			break;
		case 2:
		default:
			baudRate = 19200;
			rcvWait = 6;
			break;
		}
	}
	return 0;
}

void displayCurr(void)
{
	int i;

	serlXmitStr("BD0\n");	/* Enable Any Board */

	printf("***** Current Setting *****\n\n");

	printf(" 0)    Board ID - %d\n", getBoardID());
	for(i = 1; i <= 8; i++)
		printf("%2d)    Servo #%d - %d\n", i, i, getInitServo(i));

	printf(" 9)   Baud Rate - %d\n", getBaudRate());
	printf("10) Pre Enabled - %d (%s)\n", getPreEnable(),
																		 ((getPreEnable()==0)? "NO":"YES") );

	printf("11)  SPI Config - %d\n", getSPIConfig());

	printf("12)  -- Factory Default --\n");

	printf("\n13)  QUIT\n");

	printf("\nEnter a number to modify: ");

}

int setNewValue(void)
{
	int cmmd, value;

	scanf("%d", &cmmd);

	switch(cmmd)
	{
	case 0:
		printf( "Enter new ID number: ");
		scanf("%d", &value);
		setBoardID(value);
		break;
	case 1: case 2: case 3:	case 4:	case 5:	case 6:	case 7:	case 8:
		printf( "Enter new Servo Power-up value: ");
		scanf("%d", &value);
		setInitServo(cmmd-1, value);
		break;
	case 9:
		printf( "Select new Baud Rate\n");
		printf( " 1) 2400     2) 4800     3) 9600     4) 19200 : ");
		scanf("%d", &value);
		setBaudRate(value);
		break;
	case 10:
		printf( "Select Pre Enable on Power-up\n");
		printf( " 0) NO	  1) YES : ");
		scanf("%d", &value);
		if (value == 1)
			setPreEnable(1);
		else
			setPreEnable(0);
		break;
	case 11:
		printf( "Enter new SPI config: ");
		scanf("%d", &value);
		setSPIConfig(value);
		break;
	case 12:
		setBoardID(1); 				delay(10);
		setInitServo(1, 128); delay(10);
		setInitServo(2, 128); delay(10);
		setInitServo(3, 128); delay(10);
		setInitServo(4, 128); delay(10);
		setInitServo(5, 128); delay(10);
		setInitServo(6, 128); delay(10);
		setInitServo(7, 128); delay(10);
		setInitServo(8, 128); delay(10);
		setBaudRate(3); /* 9600 */
		setPreEnable(1); /* YES */
		setSPIConfig(0);
		break;
	case 13:
		return 0;
	}

	delay(10);
	return 1;
}

int getNumFromSV200(void)
{
	char ch, tempStr[10];
	int i=0;

	while(serlAvl())
	{
		ch = serlRcv();
		if (ch == 13)
			tempStr[i++] = '\0';
		else
			tempStr[i++] = ch;
	}
	return atoi(tempStr);
}

int getBoardID(void)
{
	serlXmitStr("RE0\n");
	delay(rcvWait);

	return getNumFromSV200();
}

int getInitServo(int servo)
{
	serlXmitStr("RE");
	itoa(servo, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
	delay(rcvWait);

	return getNumFromSV200();
}

int getBaudRate(void)
{
	int baud;

	serlXmitStr("RE9\n");
	delay(rcvWait);

	baud = getNumFromSV200();

	switch(baud)
	{
	case 200:
		baud = 2400;
		break;
	case 100:
		baud = 4800;
		break;
	case 50:
		baud = 9600;
		break;
	case 24:
		baud = 19200;
		break;
	}

	return baud;
}

int getPreEnable(void)
{
	serlXmitStr("RE10\n");
	delay(30);

	return getNumFromSV200();
}

int getSPIConfig(void)
{
	serlXmitStr("RE11\n");
	delay(rcvWait);

	return getNumFromSV200();
}


void setBoardID(int id)
{
	serlXmitStr("WE0 ");
	itoa(id, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
}

void setInitServo(int servo,int pos)
{
	serlXmitStr("WE");
	itoa(servo, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr(" ");
	itoa(pos, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
}

void setBaudRate(int baud)
{
	switch(baud)
	{
	case 1:	/* 2400 */
		baud = 200;
		break;
	case 2: /* 4800 */
		baud = 100;
		break;
	case 3: /* 9600 */
		baud = 50;
		break;
	case 4: /* 19200 */
		baud = 24;
		break;
	}

	serlXmitStr("WE9 ");
	itoa(baud, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
}

void setPreEnable(int preEn)
{
	serlXmitStr("WE10 ");
	itoa(preEn, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
}

void setSPIConfig(int spiConfig)
{
	serlXmitStr("WE11 ");
	itoa(spiConfig, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\n");
}