#include "ed_slave_int.h"
#include "main.h"
#include "slave.h"

void slave_waitack(void);

void slave_allstop(void)
{
	slave_packet1('X');
}

void slave_requeststate(void)
{
	slave_packet1('*');
}

unsigned char slave_chksend(unsigned char chk, unsigned char c)
{
	ED_SLAVE_putData(c);
	return (chk<<1)+c+(chk&0x80 ? 1: 0);
}

void slave_packet1(unsigned char b0)
{
	unsigned char chk=0;
	slave_ack = 0;
			
	chk = slave_chksend(chk, 237);
	chk = slave_chksend(chk, 5);
	chk = slave_chksend(chk, 0xc0);
	chk = slave_chksend(chk, b0);
	slave_chksend(chk, chk);

	slave_waitack();
}

void slave_packet3(unsigned char b0, unsigned char b1, unsigned char b2)
{
	unsigned char chk=0;
	slave_ack = 0;
		
	chk = slave_chksend(chk, 237);
	chk = slave_chksend(chk, 7);
	chk = slave_chksend(chk, 0xc0);
	chk = slave_chksend(chk, b0);
	chk = slave_chksend(chk, b1);
	chk = slave_chksend(chk, b2);
	slave_chksend(chk, chk);

	slave_waitack();
}

void slave_packet4(unsigned char b0, unsigned char b1, unsigned char b2, unsigned char b3)
{
	unsigned char chk=0;
	
	slave_ack=0;
	
	chk = slave_chksend(chk, 237);
	chk = slave_chksend(chk, 8);
	chk = slave_chksend(chk, 0xc0);
	chk = slave_chksend(chk, b0);
	chk = slave_chksend(chk, b1);
	chk = slave_chksend(chk, b2);
	chk = slave_chksend(chk, b3);
	slave_chksend(chk, chk);
	
	slave_waitack();
}


void slave_waitack()
{
	long starttime=sleepcounter;
	
	while (!slave_ack)
	{
		if ((sleepcounter-starttime)>SLAVE_TIMEOUT)
		{
			commerror(5);
			break;
		}
		idle();
	}

	slave_ack=0;
}


void setmotor(unsigned char port, int dirpwm)
{
	unsigned char dir=0;
	
	if (dirpwm>255)
		dirpwm=255;
	if (dirpwm<-255)
		dirpwm=-255;
	
	if (dirpwm>=0)
		dir=1;
	
	if (dirpwm<0)
	{
		dir=2;
		dirpwm=-dirpwm;
	}	
	
	slave_packet4('M', port, dir, dirpwm);
}

int getmotoractualsignedpwm(unsigned char port)
{
	int pwm=getmotoractualpwm(port);
	unsigned char dir=getmotoractualdir(port);
	
	if (dir==1)
		return pwm;
	else if (dir==2)
		return -pwm;
	return pwm;
}

unsigned char getmotoractualpwm(unsigned char port)
{
	return slavestate[13+1+port*3];
}

unsigned char getmotorgoalpwm(unsigned char port)
{
	return slavestate[13+port*3];
}

unsigned char getmotoractualdir(unsigned char port)
{
	unsigned char v;
	v=slavestate[25+port/2];
	
	if ((port&1)==0)
		return (v>>2)&0x03;
	return (v>>6)&0x03;
}

unsigned int getmotorcurrent(unsigned char port)
{
	unsigned int vi=(slavestate[27+port*2]<<8) + slavestate[27+1+port*2];
	long v=vi;

	// 1000*5.0/65535/0.18 = 0.4238
	v=(v*434)>>10;
	return v; 
}