/*
		Firmware for "Raspberry Pi Servo board" Version 0.2  2013-01-20

		Copyright 2012 Mikael Johansson

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/


#define F_CPU 20000000UL	      // CPU Freq for delay.h below

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <ctype.h>
#include <stdlib.h>
#include <avr/eeprom.h>

#define USART_BPS_9600		0
#define USART_BPS_19200		1
#define USART_BPS_38400		2
#define USART_BPS_57600		3
#define USART_BPS_115200	4
#define USART_BPS_230400	5
#define USART_BPS_500000	6

// Should be enough for longest possible cmd or parameter, including one space:
#define INPUT_BUFFER_SIZE	8

void setup_usart(unsigned short speed);
unsigned short promille_to_timer(signed short promille);
void Process_handler(void);
void RX_handler(void);
void TX_handler(void);
void MSG_handler(void);

enum {
	IDLE,
	REC_CMD,
	REC_DATA
} state;

// Used to buffer input from serial port one cmd/parameter at a time.
unsigned char buf[INPUT_BUFFER_SIZE];
unsigned char bufpos = 0;

// Used to save the cmd as ascii, while parsing parameters.
unsigned char cmd[INPUT_BUFFER_SIZE];
// Used to save the parameters converted to a number while parsing.
signed short  param[8];


// Used for sending both NACK and ACK to save some precious RAM.
const unsigned char const *acknack = "NACK\r\n";
#define ACK_STR  (acknack+1)
#define NACK_STR (acknack)

#define VERSION_STR "02\r\n"

union {
	unsigned char ucAll;
	unsigned char ucAny;
	struct {
		unsigned char exec_CMD	  : 1;
		unsigned char send_ACK		: 1;
		unsigned char send_NACK		: 1;
		unsigned char send_VER		: 1;
		unsigned char run_test		: 1;
		unsigned char change_bps	: 1;
		unsigned char ignore_ws		: 1;
		unsigned char TX			: 1;
		unsigned char enable		: 1;

	};
} uFlags;


// This array contains the target servo position.
unsigned short servo_target[8] = {30000, 30000, 30000, 30000, 30000, 30000, 30000, 30000};

//This array is read by Interrupt and contains current servo position.
volatile unsigned short servo[8] = {30000, 30000, 30000, 30000, 30000, 30000, 30000, 30000};

//This array contains the how much the servo should move every ms.
unsigned char servo_step[8] = {0};

//The index of wanted uart speed, ACK is sent before speed is changed.
unsigned char bps_index = 0;


// Clear pulse to all servos
SIGNAL(SIG_TIMER1_COMPA)
{
	PORTB = 0x00;
}



// Get pulse length for current servo and set up timer1 for it and set correct pin high.
SIGNAL(SIG_TIMER0_COMPA)
{
	static volatile unsigned char current_servo = 0;  // Just used here, volatile not needed?
	
	// not needed: TCNT0 = 0x00;
	TIFR |= (1 << OCF1A);
	current_servo = (current_servo + 1) & 0x07;
	OCR1A = servo[current_servo] - 18;  // 18 is for ISR setup time, by measuring output.
	if(uFlags.enable)
	{
		PORTB |= (1 << current_servo);
	}
	TCNT1 = 0x0000;

}



int main(void)
{
	uFlags.ucAll = 0;		// Clear all flags

	// 0xFFFF can never be a real value, assume eeprom is not initialized.
	if(eeprom_read_word((void *)0) != 0xFFFF)
	{
		unsigned char i;
		for(i=0;i<8;i++)
		{
			servo[i] = eeprom_read_word((void *)(i<<1));
		}
	}


//PORTS

  DDRB = 0xFF;				// All servo pins are outputs.
  DDRD = 0x02;				// Only serial TX pin output on portD.

//TIMERS

	// 8-bit timer - interval between the eight servo pulses.
  TCCR0A = _BV(WGM01);	// CTC
  TCCR0B = _BV(CS02);   //  clk/256

	// 16-bit timer - length of pulse
  TCCR1B = _BV(CS10) | _BV(WGM12);	// CTC and clk/1.

	TIMSK |= (1 << OCIE1A) | (1 << OCIE0A);   //Enable interrupt for CompareA-match for timer 0 and 1.

	OCR0A  = 195;   // Makes pulse starting interrupt run ~8 times per 20ms  (Timer 0)

// UART

	UCSRA |= (1<<TXC);  // Clear TX complete.

	setup_usart(USART_BPS_9600);

	sei();
	uFlags.enable = 1;

	while(1)
	{
		RX_handler();
		MSG_handler();
		TX_handler();
		Process_handler();
		_delay_us(40); // (50 - (time for one loop here)). Just a guess now.
									 // Bad solution. Makes fast bps impossible.
									 // Should be removed and tickflag in Process_handler should come from some timer interrupt instead.
	}

}



void setup_usart(unsigned short speed)
{
	//disable receiver and transmitter
	UCSRB &= ~(1<<RXEN) & ~(1<<TXEN);

	switch(speed)
	{
		case USART_BPS_9600:
			UBRRL = 129;
			break;
		case USART_BPS_19200:
			UBRRL = 64;
			break;
		case USART_BPS_38400:
			UBRRL = 32;
			break;
		case USART_BPS_57600:
			UBRRL = 21;
			break;
		case USART_BPS_115200:
			UBRRL = 10;
			break;
		case USART_BPS_230400:
			UBRRL = 4;
			break;
		case USART_BPS_500000:
			UBRRL = 2;
			break;
		default:
			uFlags.send_NACK = 1;
			break;
	}

  //enable receiver and transmitter
	UCSRB |= (1<<RXEN) |(1<<TXEN);
}



//With current crystal and timer settings 20000 timer steps is 1ms pulse, 40000 timer steps is 2ms pulse.
unsigned short promille_to_timer(signed short promille)
{
	if(promille<-2500)
	  promille = -2500;
	else if(promille>1900)
		promille = 1900;

	// Less code than promille*10+30000 ?
	return (promille+promille+promille+promille+promille+promille+promille+promille+promille+promille+30000);
}



void Process_handler(void)
{
	unsigned char i;
	unsigned char tickflag = 0;
	static unsigned char twentys_of_ms;

	twentys_of_ms++;
	if(twentys_of_ms == 20)
	{
		twentys_of_ms = 0;
		tickflag = 1;
	}


	if(uFlags.run_test)
	{
		unsigned char i;
		static unsigned char dir = 1;
		static unsigned short tmp=30000;

		for(i=0;i<8;i++)
			servo[i] = tmp;

		if(tmp == 40000)
			dir = 0;
		if(tmp == 20000)
			dir = 1;

		if(dir == 1)
			tmp = tmp+1;
		else
			tmp = tmp-1;

	}


	// Promille / ms

	if(tickflag)
	{
		tickflag = 0;
		for(i=0;i<8;i++)
		{
			if(servo[i] != servo_target[i])  // Not yet at target?
			{
				//No
				if(servo_step[i] != 0)   // Supposed to update?
				{
					//Yes
					if(abs((servo_target[i] - (signed long)servo[i])) <= (signed long)servo_step[i])  // One step is bigger than remaining distance to target?
						servo[i] = servo_target[i];  // Yes - just set servo´to target, done!

					if(servo[i] != servo_target[i]) // Are we there yet?
					{
						//No - Move servo in correct direction
						if((servo_target[i] - (signed long)servo[i]) > 0)
							servo[i] += (servo_step[i]);
						else
							servo[i] -= (servo_step[i]);
					}
				}
			}

		}
	}

}



void RX_handler(void)
{
	static unsigned char n = 0;
	unsigned char rx;
	unsigned char i;

	// Have we received a new byte?
	if ( (UCSRA & (1<<RXC)) )
	{
		// Get byte from HW serial rx register..
		rx = UDR;

		// A command always start with "s" and can not contain an "s".
		if(rx == 's')
		{
			for(i=0;i<8;i++)
			{
				param[i] = 0;
			}
			n = 0;
			bufpos = 0;
			state = REC_CMD;
			uFlags.ignore_ws = 0;
		}

		if(rx == ' ' && uFlags.ignore_ws)
		{
			// Do nothing.
		}
		else if(state != IDLE)
		{
			uFlags.ignore_ws = 0;

			if(bufpos < INPUT_BUFFER_SIZE)
			{
				buf[bufpos++] = rx;

				if(rx == ' ' || rx == '\r' || rx == '\n')
				{
					buf[bufpos-1] = '\0';

					if(state == REC_CMD)
					{
						// save cmd for execution
						for(i=0;i<bufpos;i++)
						{
							cmd[i] = buf[i];
						}
						state = REC_DATA;
					}
					else // if(state == REC_DATA)
					{
						if(n<8)
						{
							// parse parameter and save for execution
							param[n++] = atoi(buf);
						}
					}

					uFlags.ignore_ws = 1;
					bufpos = 0;
				}

				if(rx == '\r' || rx == '\n')
				{
					// Run command.
					uFlags.exec_CMD = 1;
					state = IDLE;
				}

			}
			else
			{
				// Message part is to long
				bufpos = 0;
				state = IDLE;
				uFlags.send_NACK = 1;
			}
		}

	}
}



void TX_handler(void)
{
	static unsigned char *cur;

	if(uFlags.send_VER)
	{
		uFlags.send_VER = 0;
		uFlags.TX = 1;

		cur = VERSION_STR;
	}

	if(uFlags.send_ACK)
	{
		uFlags.send_ACK = 0;
		uFlags.TX = 1;

		cur = ACK_STR;
	}

	if(uFlags.send_NACK)
	{
		uFlags.send_NACK = 0;
		uFlags.TX = 1;

		cur = NACK_STR;
	}

	if(uFlags.TX)
	{
		if(*cur != '\0') //Any char to send?
		{
			// yes!
			if( UCSRA & (1<<UDRE) )
			{
				UCSRA |= (1<<TXC);	// CLEAR TXC before trying to send something. (clear by writing 1)
				UDR = *cur;					// Send!
				cur++;
			}
		}
		else
		{
			// no
			uFlags.TX = 0;
		}
	}
	else // We have nothing to send.
	{
		// Do we want to change bps?
		if(uFlags.change_bps)
		{
			if( UCSRA & (1<<TXC) ) // Transmission complete?
			{
				// We are now allowed to change BPS
				uFlags.change_bps = 0;
				setup_usart(bps_index);
				UCSRA |= (1<<TXC);	// Not needed? Probably not.
			}
		}
	}

}



void MSG_handler(void)
{
	if(uFlags.exec_CMD)
	{
		uFlags.exec_CMD = 0;
		uFlags.run_test = 0;				//Always stop servo test when receiving a valid or invalid cmd.

		if(cmd[1] == 't')														// "st" - Servo Test
		{
			uFlags.run_test = 1;
			uFlags.send_ACK = 1;
		}
		else if(cmd[1] == 'a' && cmd[2] == 'v')		// "sav" - Servo All Velocity, has 8 parameters, speed
		{
			unsigned char i;
			for(i=0;i<8;i++)
				servo_step[i] = (unsigned char)param[i];

			uFlags.send_ACK = 1;
		}
		else if(cmd[1] == 'a')												// "sa" - Servo All, has 8 parameters, position
		{
			unsigned char i;

			for(i=0;i<8;i++)
			{
				if(servo_step[i] == 0)
					servo_target[i] = servo[i] = promille_to_timer(param[i]);
				else
					servo_target[i] = promille_to_timer(param[i]);
			}

			uFlags.send_ACK = 1;
		}
		else if(((unsigned char)(cmd[1]-'0')) < 8)										// "s0", "s1" ... "s7" - Servo N, has 1 or 2 parameter, position and speed.
		{
			unsigned char tmp;
			unsigned char whatservo = (cmd[1]-'0');

			servo_target[whatservo] = promille_to_timer(param[0]);  // Position in parameter 0
			tmp = (unsigned char)param[1];													// Speed in parameter 1 (0 == as fast as possible)

			if(tmp == 0)
			{
				servo[whatservo] = servo_target[whatservo];
			}
			else
			{
				//
				servo_step[whatservo] = tmp;
			}

			uFlags.send_ACK = 1;
		}
		else if(cmd[1] == 'i' && cmd[2] == 'a')		// "sia" - Servo Init All, has 8 parameters, positions.
		{
			unsigned char i;

			for(i=0;i<8;i++)
			{
				eeprom_write_word((void *)(i<<1), promille_to_timer(param[i]));
			}
			uFlags.send_ACK = 1;
		}
		else if(cmd[1] == 'b' && cmd[2] == 'r')		// "sbr" - Servo (set) Bit Rate, has one parameter 0-6.
		{
			bps_index = (unsigned char)param[0];
			uFlags.change_bps = 1;
			uFlags.send_ACK = 1;
		}
		else if(cmd[1] == 'n')		// "sn" - Servo versioN.
		{
			uFlags.send_VER = 1;
		}
		else if(cmd[1] == 'e')
		{
			uFlags.enable = 1;
			uFlags.send_ACK = 1;
		}
		else if(cmd[1] == 'd')
		{
			uFlags.enable = 0;
			uFlags.send_ACK = 1;
		}
		else
		{
			uFlags.send_NACK = 1;
		}		
	}
}