/* (C) Copyright 2003 Stefan May <smay@4finger.net>
 * this code is licensed under the GPL */
#include <stdlib.h>
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <avr/twi.h>
//#include <avr/timer.h>
#include <avr/sleep.h>

#define SERVO_ADDRESS   0xC2
#define SERVO_VERSION   0x01
#define SERVO_MAX       20

#define REG_VERSION     0x00
#define REG_DELTA       SERVO_MAX + 0x01
#define REG_OFFSETH     SERVO_MAX + 0x02
#define REG_OFFSETL     SERVO_MAX + 0x03
#define REG_REPEAT      SERVO_MAX + 0x04

#define DEFAULT_DELTA   (CLK / 256000ul)
#define DEFAULT_OFFSET  (CLK / 1000ul)
#define DEFAULT_REPEAT  0x00

#define SERVO_REG_MAX   0x80
volatile uint8_t servo_register[SERVO_REG_MAX];

#define true  0
#define false 1
volatile uint8_t servo_rollover = false;

SIGNAL(SIG_2WIRE_SERIAL)
{
    static uint8_t reg = SERVO_REG_MAX;

    switch (TWSR) {
	    /* SLAVE RECEIVER */
	case TW_SR_SLA_ACK:
	case TW_SR_ARB_LOST_SLA_ACK:
	case TW_SR_STOP:
	case TW_ST_DATA_ACK:
	case TW_ST_DATA_NACK:
	    TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWEA) | _BV(TWIE);
	    break;
	case TW_SR_DATA_ACK:
	case TW_SR_DATA_NACK:
	    if (reg < SERVO_REG_MAX) {
		servo_register[reg] = TWDR;
		reg = SERVO_REG_MAX;
	    } else {
		reg = TWDR;
	    }
	    TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWEA) | _BV(TWIE);
	    break;
	    /* SLAVE TRANSMITTER */
	case TW_ST_SLA_ACK:
	case TW_ST_ARB_LOST_SLA_ACK:
	    if (reg < SERVO_REG_MAX) {
		TWDR = servo_register[reg];
		reg = SERVO_REG_MAX;
	    }
	    TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWIE);
	    break;
    }
}

SIGNAL(SIG_OVERFLOW1)
{
    static uint8_t which = 0;
    uint16_t servo_delay;

    /* port ausschalten */
    switch (which) {
	case 1: cbi(PORTD, 0); break;
	case 2: cbi(PORTD, 1); break;
	case 3: cbi(PORTD, 2); break;
	case 4: cbi(PORTD, 3); break;
	case 5: cbi(PORTD, 4); break;
	case 6: cbi(PORTB, 6); break;
	case 7: cbi(PORTB, 7); break;
	case 8: cbi(PORTD, 5); break;
	case 9: cbi(PORTD, 6); break;
	case 10: cbi(PORTD, 7); break;
	case 11: cbi(PORTB, 0); break;
	case 12: cbi(PORTB, 1); break;
	case 13: cbi(PORTB, 2); break;
	case 14: cbi(PORTB, 3); break;
	case 15: cbi(PORTB, 4); break;
	case 16: cbi(PORTB, 5); break;
	case 17: cbi(PORTC, 0); break;
	case 18: cbi(PORTC, 1); break;
	case 19: cbi(PORTC, 2); break;
	case 20: cbi(PORTC, 3); break;
    }

    /* nächsten servo bestimmen */
    for (which++; which <= SERVO_MAX; which++) {
	if (servo_register[which] != 0)
	    goto next;
    }
    if (servo_rollover) {
	which = 1;
	servo_rollover = false;
	goto next;
    }

    /* an dieser stelle wurde kein aktiver servo gefunden */
    TCCR1B = 0; /* STOP */
    which = 0;
    return;

  next:			/* nächsten delay laden */
    servo_delay = 0;
    servo_delay -=
	(servo_register[REG_OFFSETH] << 8) + servo_register[REG_OFFSETL];
    servo_delay -=
	(uint16_t) servo_register[which] *
	(uint16_t) servo_register[REG_DELTA];
    TCNT1 = servo_delay;

    /* port einschalten */
    switch (which) {
	case 1: sbi(PORTD, 0); break;
	case 2: sbi(PORTD, 1); break;
	case 3: sbi(PORTD, 2); break;
	case 4: sbi(PORTD, 3); break;
	case 5: sbi(PORTD, 4); break;
	case 6: sbi(PORTB, 6); break;
	case 7: sbi(PORTB, 7); break;
	case 8: sbi(PORTD, 5); break;
	case 9: sbi(PORTD, 6); break;
	case 10: sbi(PORTD, 7); break;
	case 11: sbi(PORTB, 0); break;
	case 12: sbi(PORTB, 1); break;
	case 13: sbi(PORTB, 2); break;
	case 14: sbi(PORTB, 3); break;
	case 15: sbi(PORTB, 4); break;
	case 16: sbi(PORTB, 5); break;
	case 17: sbi(PORTC, 0); break;
	case 18: sbi(PORTC, 1); break;
	case 19: sbi(PORTC, 2); break;
	case 20: sbi(PORTC, 3); break;
    }
}

SIGNAL(SIG_OVERFLOW2)
{
    if ((TCCR1B & 0x07) != 0) { /* STOP */
	servo_rollover = true;
    }
    TCCR1B = 1; /* CK */
    TCNT2 = servo_register[REG_REPEAT];
}

static void reset(void)
{
    /* clear all registers */
    uint8_t i;
    for (i = 0; i < SERVO_REG_MAX; i++)
	servo_register[i] = 0x00;

    /* initialize registers */
    servo_register[REG_VERSION] = SERVO_VERSION;
    servo_register[REG_DELTA] = DEFAULT_DELTA;
    servo_register[REG_OFFSETH] = DEFAULT_OFFSET >> 8;
    servo_register[REG_OFFSETL] = DEFAULT_OFFSET & 0xFF;
    servo_register[REG_REPEAT] = DEFAULT_REPEAT;
}

int main(void)
{
    /* initialize all registers */
    reset();

    /* initialize ports */
    DDRB = 0xFF;
    DDRC = 0xFF;
    DDRD = 0xFF;
    PORTB = 0x00;
    PORTC = 0x00;
    PORTD = 0x00;

    /* initialize twi interface */
    TWSR = 0x00;
    TWBR = (CLK / 100000 - 16ul) / 2;	// 100kHz
    TWAR = SERVO_ADDRESS;
    TWCR = _BV(TWEN) | _BV(TWEA) | _BV(TWIE);

    /* initialize timers */
    TCCR2 = 5; /* CK1024 */
    TCNT2 = servo_register[REG_REPEAT];
    TCCR1B = 0; /* STOP */
    TIMSK |= _BV(TOIE2) | _BV(TOIE1);

    /* allow interrupts */
    sei();

    /* main loop */
    set_sleep_mode(SLEEP_MODE_IDLE);
    for (;;) {
	if (servo_register[REG_VERSION] != SERVO_VERSION)
	    reset();
	sleep_mode();
    }
}