/* Copyright 2003 Stefan May <smay@4finger.net>
 * this code is licensed under the GPL */
#include <stdlib.h>
#include <stdio.h>
#include <avr/io.h>
#include <avr/twi.h>
#include <avr/pgmspace.h>
#include <inttypes.h>
#include <ctype.h>
#include "console.h"

uint8_t SERVO = 0xC2;

void twi_init(void)
{
	/* initialize twi timing registers */
	TWSR = 0x00;
	TWBR = (CLK / 100000 - 16ul) / 2;
	/* enable pull ups for sda and scl */
	sbi(PORTC, PC4); /* set PC4 and PC5 */
	sbi(PORTC, PC5);
	cbi(DDRC, DDC4); /* clear DDC4 and DDC5 */
	cbi(DDRC, DDC5);
	
}

/*  the following two functions are derived from demo-code
 *  in avr-libc documentation by Jörg Wunsch
 *  http://www.nongnu.org/avr-libc/user-manual/twi_demo.html
 */

int servo_read(uint8_t reg)
{
	uint8_t n = 0;
	int rv = 0;

restart:
	if (n++ >= 250)
		return -1;
begin:

	/* 
	 * SEND START CONDITION
	 */
	TWCR = _BV (TWINT) | _BV (TWSTA) | _BV (TWEN);
	while (!(TWCR & _BV (TWINT)));
	switch (TW_STATUS) {
		case TW_REP_START:
		case TW_START:
			break;
		case TW_MT_ARB_LOST:
			goto begin;
		default:
			return -1;
	}

	/*
	 * SEND SLAVE ADDRESS + WRITE
	 */
	TWDR = SERVO | TW_WRITE;
	TWCR = _BV (TWINT) | _BV (TWEN);
	while (!(TWCR & _BV (TWINT)));
	switch (TW_STATUS) {
		case TW_MT_SLA_ACK:
			break;
		case TW_MT_SLA_NACK:
			goto restart;
		case TW_MT_ARB_LOST:
			goto begin;
		default:
			goto error;
	}

	/*
	 * SEND REGISTER ADDRESS
	 */
	TWDR = reg;
	TWCR = _BV (TWINT) | _BV (TWEN);
	while (!(TWCR & _BV (TWINT)));
	switch (TW_STATUS) {
		case TW_MT_DATA_ACK:
			break;
		case TW_MT_DATA_NACK:
			goto quit;
		case TW_MT_ARB_LOST:
			goto begin;
		default:
			goto error;
	}

	/*
	 * SEND REPEATED START CONDITION
	 */
	TWCR = _BV (TWINT) | _BV (TWSTA) | _BV (TWEN);
	while (!(TWCR & _BV (TWINT)));
	switch (TW_STATUS) {
		case TW_START:
		case TW_REP_START:
			break;
		case TW_MT_ARB_LOST:
			goto begin;
		default:
			goto error;
	}

	/* 
	 * SEND SLAVE ADDRESS + READ
	 */
	TWDR = SERVO | TW_READ;
	TWCR = _BV (TWINT) | _BV (TWEN);
	while (!(TWCR & _BV (TWINT)));
	switch (TW_STATUS) {
		case TW_MR_SLA_ACK:
			break;
		case TW_MR_SLA_NACK:
			goto quit;
		case TW_MR_ARB_LOST:
			goto begin;
		default:
			goto error;
	}

	/*
	 * RECEIVE REGISTER VALUE
	 */
	TWCR = _BV (TWINT) | _BV (TWEN);
	while (!(TWCR & _BV (TWINT)));
	switch (TW_STATUS) {
		case TW_MR_DATA_NACK:
		case TW_MR_DATA_ACK:
			rv = TWDR;
			break;
		default:
			goto error;
	}
quit:
	/*
	 * SEND STOP CONDITION
	 */
	TWCR = _BV (TWINT) | _BV (TWSTO) | _BV (TWEN);
	return rv;

error:
	rv = -1;
	goto quit;
}

int servo_write(uint8_t reg, uint8_t value)
{
	uint8_t n = 0;
	int rv = 0;

restart:
	if (n++ >= 250)
		return -1;
begin:

	/*
	 * SEND START CONDITION
	 */
	TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);
	while (!(TWCR & _BV(TWINT)));
	switch (TW_STATUS) {
		case TW_REP_START:
		case TW_START:
			break;
		case TW_MT_ARB_LOST:
			goto begin;
		default:
			return -1;
	}

	/*
	 * SEND SLAVE ADDRESS + WRITE
	 */
	TWDR = SERVO | TW_WRITE;
	TWCR = _BV(TWINT) | _BV(TWEN);
	while (!(TWCR & _BV(TWINT)));
	switch (TW_STATUS) {
		case TW_MT_SLA_ACK:
			break;
		case TW_MT_SLA_NACK:
			goto restart;
		case TW_MT_ARB_LOST:
			goto begin;
		default:
			goto error;
	}

	/*
	 * SEND REGISTER ADDRESS
	 */
	TWDR = reg;
	TWCR = _BV(TWINT) | _BV(TWEN);
	while (!(TWCR & _BV(TWINT)));
	switch (TW_STATUS) {
		case TW_MT_DATA_ACK:
			break;
		case TW_MT_DATA_NACK:
			goto quit;
		case TW_MT_ARB_LOST:
			goto begin;
		default:
			goto error;
	}

	/*
	 * SEND REGISTER VALUE
	 */
	TWDR = value;
	TWCR = _BV(TWINT) | _BV(TWEN);
	while (!(TWCR & _BV(TWINT)));
	switch (TW_STATUS) {
		case TW_MT_DATA_NACK:
			goto error;
		case TW_MT_DATA_ACK:
			break;
		default:
			goto error;
	}

quit:
	/*
	 * SEND STOP CONDITION
	 */
	TWCR = _BV(TWINT) | _BV(TWSTO) | _BV(TWEN);
	return rv;

error:
	rv = -1;
	goto quit;
}

int get_hexdigit(uint8_t* value) {

	int input;
	uint8_t temp;

	/* get digit */
	input = getchar();
	if(isxdigit(input)) {
		printf_P(PSTR("%c"), input);
		temp = *value;
		temp <<= 4;
		if(input < 0x61)
			temp += input - 0x30;
		else
			temp += tolower(input) - 0x61 + 0xa;
		*value = temp;
		return 1;
	}
	return 0;
}

void main(void)
{
	uint8_t reg, value;
	int result;

	/* initialize hardware */
	console_init();
	twi_init();

	/* the main program */
	printf_P(PSTR("\n\nSERVOTESTER\n\n"));

	result = servo_read(0x00);
	if(result != -1) {
		printf_P(PSTR("servocontroller version = %02X\n\n"), result);
	}
	else {
		printf_P(PSTR("hardware failure"));
		while(1);
	}
		
reset:
	printf_P(PSTR("\n%02X> "), SERVO);

	/* get command */
	switch(getchar()) {
		case 'a': /* set controller address */
			printf_P(PSTR("address "));
			SERVO = 0;
			if(!get_hexdigit(&SERVO)) goto error;
			if(!get_hexdigit(&SERVO)) goto error;
			printf_P(PSTR("\n"));
			result = servo_read(0x00);
			if(result != -1) {
				printf_P(PSTR("servocontroller version = %02X\n\n"), result);
			}
			else {
				printf_P(PSTR("hardware failure"));
				while(1);
			}
			break;
		case 'r': /* read */
			printf_P(PSTR("read "));

			/* get register number */
			reg = 0;
			if(!get_hexdigit(&reg)) goto error;
			if(!get_hexdigit(&reg)) goto error;
			printf_P(PSTR("\n"));

			/* check register */
			if(reg > 0x7f) goto error;

			/* execute command */
			value = servo_read(reg);
			printf_P(PSTR("%02X - %02X\n"), reg, value);
			break;
		case 'c': /* clear */
			printf_P(PSTR("clear "));
			
			/* get register number */
			reg = 0;
			if(!get_hexdigit(&reg)) goto error;
			if(!get_hexdigit(&reg)) goto error;
			printf_P(PSTR("\n"));

			/* check register */
			if(reg > 0x7f) goto error;

			/* execute command */
			servo_write(reg, 0x00);
			break;
		case 'w': /* write */
			printf_P(PSTR("write "));

			/* get register number */
			reg = 0;
			if(!get_hexdigit(&reg)) goto error;
			if(!get_hexdigit(&reg)) goto error;
			printf_P(PSTR(" "));

			/* check register */
			if(reg > 0x7f) goto error;
			
			/* get value */
			value = 0;
			if(!get_hexdigit(&value)) goto error;
			if(!get_hexdigit(&value)) goto error;
			printf_P(PSTR("\n"));

			/* execute command */
			servo_write(reg, value);
			break;
		case 'd': /* dump */
			printf_P(PSTR("dump\n"));
			for(reg = 0x00; reg < 0x7F; reg++) {
				value = servo_read(reg);
				printf_P(PSTR("%02X - %02X   "), reg, value);
				switch(reg) {
					case 0x00:
						printf_P(PSTR("(version number)\n"));
						break;
					case 0x01:
					case 0x02:
					case 0x03:
					case 0x04:
					case 0x05:
					case 0x06:
					case 0x07:
					case 0x08:
					case 0x09:
					case 0x0a:
					case 0x0b:
					case 0x0c:
					case 0x0d:
					case 0x0e:
					case 0x0f:
					case 0x10:
					case 0x11:
					case 0x12:
					case 0x13:
					case 0x14:
						printf_P(PSTR("(servo %d)\n"), reg);
						break;
					case 0x15:
						printf_P(PSTR("(delta time)\n"));
						break;
					case 0x16:
						printf_P(PSTR("(offset high byte)\n"));
						break;
					case 0x17:
						printf_P(PSTR("(offset low byte)\n"));
						break;
					default:
						printf_P(PSTR("\n"));
				}
			}
			break;
		case 's': /* controller */
			printf_P(PSTR("controller "));
			
			/* get register number */
			reg = 0;
			if(!get_hexdigit(&reg)) goto error;
			if(!get_hexdigit(&reg)) goto error;
			printf_P(PSTR(" "));

			/* check register */
			if(reg < 0x01 && reg > 0x14)
				goto error;
			
			/* execute command */
			value = servo_read(reg);
			printf_P(PSTR("\n\n" 
						"+  - increment\n"
						"-  - decrement\n"
						"r  - reset to zero\n"
						"q  - quit\n\n"
				     ));
			printf_P(PSTR("\r%02X - %02X"), reg, value);
			while((result = getchar()) != 'q') {
				switch(result) {
					case '+':
						if(value < 0xFF)
							value++;
						break;
					case '-':
						if(value > 0x01)
							value--;
						break;
					case 'r':
						value = 0x00;
						break;
				}
				printf_P(PSTR("\r%02X - %02X"), reg, value);
				servo_write(reg, value);
			}
			printf_P(PSTR("\n"));
			break;
		case 'h': /* help */
			printf_P(PSTR("help\n\n"
						"a [addr]      - set twi address\n"
						"d             - dump all registers\n"
						"r [reg]       - read register\n"
						"w [reg] [val] - write register\n"
						"c [reg]       - clear register\n"
						"s [servo]     - servocontroller with inc/dev\n"
						"h             - this help\n"
						"\n  reg   [00-7F]\n"
						"  val   [00-FF]\n"
						"  servo [01-14]\n"
						"  addr  [00-FF]\n"
						"  all values in hex\n"
				));
			break;
		default:
			goto error;
	}

	/* the end */
	goto reset;
error:
	printf_P(PSTR("syntax error\n"));
	goto reset;
}