/*	PONTECH Copyright 1996
 *	Date: 6/23/1996
 *  SV_LIB.CPP
 *  	SV200 move functions use by all the examples
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <dos.h>
#include "asynch.h"
#include "asynch.cpp"	/* don't need this line if include in project */

SV200::SV200(int comPort, int baudRate, unsigned mDelay)
{
	comm = new Asynch(comPort, baudRate);	// open comm port
	comm->flushAllBufs();

	MasterDelay = mDelay;
	svSelect = 1;
	for (int i = 0; i < 8; i++)
	{
		savePosition[i] = 0;
	}
}

SV200::~SV200()
{
	delete comm;
}

void SV200::serlXmitStr(char *str)
{
	comm->outStr(str);
}

void SV200::svCmmd(char *cmmd, int param)
{
	serlXmitStr(cmmd);
	itoa(param, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\r");
}

void SV200::svCmmd2(char *cmmd, int param1, int param2)
{
	serlXmitStr(cmmd);
	itoa(param1, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr(" ");
	itoa(param2, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\r");
}

void SV200::selectBoard(int board)
{
	serlXmitStr("BD");
	itoa(board, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\r");
}

void SV200::selectServo(int servo)
{
	serlXmitStr("SV");
	itoa(servo, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\r");
	svSelect = servo;
}

void SV200::moveSelect(int position)
{
	serlXmitStr("M");
	itoa(position, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\r");
	savePosition[svSelect] = position;

	delay(MasterDelay);
}

void SV200::move(int servo, int position)
{
	serlXmitStr("SV");
	itoa(servo, numStr, 10);
	serlXmitStr(numStr);

	serlXmitStr("M");
	itoa(position, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\r");
	savePosition[servo-1] = servo;

	delay(MasterDelay);
}

void SV200::moveAll(int S1, int S2, int S3, int S4, int S5, int S6, int S7, int S8)
{
	if ((S1 >= 0) && (S1 <= 255))
	{
		serlXmitStr("SV1M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[0] = S1;
	}
	if ((S2 >= 0) && (S2 <= 255))
	{
		serlXmitStr("SV2M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[1] = S2;
	}
	if ((S3 >= 0) && (S3 <= 255))
	{
		serlXmitStr("SV3M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[2] = S3;
	}
	if ((S4 >= 0) && (S4 <= 255))
	{
		serlXmitStr("SV4M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[3] = S4;
	}
	if ((S5 >= 0) && (S5 <= 255))
	{
		serlXmitStr("SV5M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[4] = S5;
	}
	if ((S6 >= 0) && (S6 <= 255))
	{
		serlXmitStr("SV6M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[5] = S6;
	}
	if ((S7 >= 0) && (S7 <= 255))
	{
		serlXmitStr("SV7M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[6] = S7;
	}
	if ((S8 >= 0) && (S8 <= 255))
	{
		serlXmitStr("SV8M");
		itoa(S1, numStr, 10);
		serlXmitStr(numStr);
		savePosition[7] = S8;
	}

	serlXmitStr("\r");
	delay(MasterDelay);
}

void SV200::slowMove(int servo, int position,int speed)
{
	int i, change;

	serlXmitStr("SV");
	itoa(servo, numStr, 10);
	serlXmitStr(numStr);

	change = savePosition[servo-1] - position;
	if (change > 0)
	{
		for (i = 0; i < change; i++)
		{
			serlXmitStr("I-1\n");
			delay(speed);
		}
	}
	else
	{
		for (i = change; i < 0; i++)
		{
			serlXmitStr("I1\n");
			delay(speed);
		}
	}

	savePosition[servo-1] = position;

	delay(MasterDelay);
}

int SV200::getNumFromSV200(void)
{
	int i;
	unsigned char ch;
	time_t tm;
	char result[10];

	// Read the ascii string into result
	i = 0;
	do
	{
		tm = time(NULL);
		while (comm->inCount() <= 0)
		{
			if ((time(NULL) - tm) >= 2)
			{
				result[0] = 0;
				return -1;
			}
		}
		ch = comm->inCh();

		if (ch == 13)
			ch = 0;
		if (ch != 10)
			result[i++] = ch;

	} while (ch != 0);
	result[i] = '\0';

	return atoi(result);
}


unsigned char SV200::getAD(int channel)
{
	serlXmitStr("AD");
	itoa(channel, numStr, 10);
	serlXmitStr(numStr);
	serlXmitStr("\r");

	return (unsigned char) getNumFromSV200();
}

int SV200::adjADJoystick(int position)
{
	return (((position-128) * 2) + 128);
}