/* (C) 2006 Julian von Mendel
	2012 corrections by W. Haslbeck */

#include "config.h"
#include "lib.h"
#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h>
#include <util/delay.h>

/* GENERAL */

inline void pause(uint16_t ms)
{ /* pauses the program for the given time;
   * ms: time in milliseconds (0 - 65535)
   */

    uint16_t i;

    for(i=0; i < ms/63; i++) _delay_ms(63);
    _delay_ms(ms%63);
}


/* IOs */

uint8_t io_info(uint8_t ionr, uint8_t var)
{ /* returns the requested variable for the pin io_nr;
   * io_nr: pin
   * var:   0 <portnumber>
   *        1 returns number of ADC-port, if it isn't an ADC-port, 255 is returned
   *        2 returns number of servo, if it isn't a defined servo, 255 is returned
   */

    switch(var)
    {
        case 0:
            switch(ionr)
            {
                case 1 : case 9 : case 12: return 6;
                case 2 : case 14: case 23: return 0;
                case 3 : case 15: case 24: return 1;
                case 4 : case 16: case 25: return 2;
                case 5 : case 17: case 26: return 3;
                case 6 : case 18: case 27: return 4;
                case 11: case 19: case 28: return 5;
                case 10: case 13:          return 7;
            }
        break;
        case 1:
            if (ionr >= 23 && ionr <= 28)
                return ionr-23;
        break;
        case 2:
            if (ionr == 255) return 255;
            else if (ionr == SERVO0) return 0;
            else if (ionr == SERVO1) return 1;
            else if (ionr == SERVO2) return 2;
            else if (ionr == SERVO3) return 3;
            else if (ionr == SERVO4) return 4;
            else if (ionr == SERVO5) return 5;
            else if (ionr == SERVO6) return 6;
            else if (ionr == SERVO7) return 7;
            else if (ionr == SERVO8) return 8;
            else if (ionr == SERVO9) return 9;
    }
    return 255;
}

void io_set_(volatile uint8_t *ddr, volatile uint8_t *port, uint8_t pinnr, uint8_t status)
{ /* sets an io;
   * status:
   *            0 deactivated output
   *            1 activated output
   *            2 output
   *            3 input with activated pullup
   *            4 input with deactivated pullup
   */

    switch(status)
    {
        case 0:
            *ddr  |=  (1 << pinnr);
            *port &= ~(1 << pinnr);
        break;
        case 1:
        default:
            *ddr  |=  (1 << pinnr);
            *port |=  (1 << pinnr);
        break;
        case 2:
            *ddr  |=  (1 << pinnr);
        break;
        case 3:
            *ddr  &= ~(1 << pinnr);
            *port |=  (1 << pinnr);
        break;
        case 4:
            *ddr  &= ~(1 << pinnr);
            *port &= ~(1 << pinnr);
    }
}

void io_set(uint8_t ionr, uint8_t status)
{ /* sets an io;
   * ionr:      pin
   * status:
   *            0 deactivated output
   *            1 activated output
   *            2 output
   *            3 input with activated pullup
   *            4 input with deactivated pullup
   */

    if (ionr == 1 || (ionr >= 23 && ionr <= 28))
        io_set_(&DDRC,&PORTC,io_info(ionr,0),status);
    else if ((ionr >= 2 && ionr <= 6) || (ionr >= 11 && ionr <= 13))
        io_set_(&DDRD,&PORTD,io_info(ionr,0),status);
    else if ( (ionr >= 14 && ionr <= 19) || (ionr >= 9 && ionr <= 10))
        io_set_(&DDRB,&PORTB,io_info(ionr,0),status);
}

uint8_t io_get(uint8_t ionr)
{ /* checks the status of the given input pin
   * io_nr: pin
   */

    if (ionr == 1 || (ionr >= 23 && ionr <= 28))
        return io_get_(&PINC, io_info(ionr,0));
    else if ((ionr >= 2 && ionr <= 6) || (ionr >= 11 && ionr <= 13))
        return io_get_(&PIND, io_info(ionr,0));
    else if ((ionr >= 14 && ionr <= 19) || (ionr >= 9 && ionr <= 10))
        return io_get_(&PINB, io_info(ionr,0));
    return 255;
} 

inline uint8_t io_get_(volatile uint8_t *pin, uint8_t pinnr)
{ /* checks the status of the given input pin
   */

    if (*pin & (1 << pinnr)) return 0;
    return 1;
}

/* ADC */

uint16_t adc(uint8_t ionr)
{ /* checks the input of the given adc-port (10 bit);
   * io_nr: pin
   */

    uint8_t admux = io_info(ionr,1);
    if (admux == 255) return(65535);

    ADCSRA  =  (1<<ADEN)  | (1<<ADPS1) | (1<<ADPS0);
    ADMUX   =  admux;
    ADMUX  |=  (1<<REFS1) | (1<<REFS0);
    ADCSRA |=  (1<<ADSC);
    while      (ADCSRA    & (1<<ADSC));
    uint16_t val     = ADCW;
    ADCSRA &= ~(1<<ADEN);
    return val;
}


/* UART */

inline void uart_init(uint8_t tx, uint8_t rx)
{ /* initializes UART communication;
   * tx: sending should be activated
   * rx: receiving should be activated
   */

    uint16_t baudrate;

    // Baud-Rate setzen
    baudrate = UART_BAUD_RATE/2;
    UBRRH    = (uint8_t) (UART_UBRR_CALC(baudrate,F_CPU)>>8);
    UBRRL    = (uint8_t) UART_UBRR_CALC(baudrate,F_CPU);

    // Ggf. TX und RX anschalten
    UCSRA |= (1<<U2X);
    UCSRB |= (1<<RXCIE);
    if(tx) UCSRB |= (1<<TXEN);
    if(rx) UCSRB |= (1<<RXEN);

    sei();
    pause(1);
}

void uart_putc(uint8_t c)
{ /* send one char per UART
   */

    while (!(UCSRA & (1<<UDRE)));
    UDR = c;
}

void uart_puts(uint8_t *data)
{ /* send string per UART
   */

    while(*data)
    {
        uart_putc(*data);
        data++;
    }
}

void uart_puti(uint16_t data)
{ /* send integer per UART
   */

    uint8_t buffer[7];
    uart_puts(utoa(data, buffer, 10));
}

void uart_putf(float data)
{ /* send float per UART
     BUG: unnötige Leerzeichen am Anfang?
   */

    uint8_t buffer[7];
    uart_puts(dtostrf(data, 6, 2, buffer));
}

#ifdef UART_BUFFER
    uint8_t uart_buffer[UART_BUFFER];
    int8_t  uart_count  = 0         ;

    uint8_t uart_getc(void)
    { /* output next char in cache
       */

        if (uart_count == 0) return 0;
        return uart_buffer[--uart_count];
    }

    ISR(SIG_UART_RECV)
    { /* INTERRUPT: receive char from UART and save it the buffer
       */

        if (uart_count < UART_BUFFER) uart_buffer[uart_count++] = UDR;
    }
#endif


/* SERVOS */

#ifdef SERVOS
    uint16_t servo_pulslength[20];

    inline void servo_init(void)
    { /* initialize servo control (up to 10 servos)
       */

        uint8_t  i;
        uint16_t wert;

        for(i=0;i<10;i++)
        {
            wert = MINPULS+(MAXPULS-MINPULS)/256*128;
            // calculate hightime
            servo_pulslength[i<<1]=(-1)*wert;
            // sum of low and hightime for one servo is 2ms
            servo_pulslength[(i<<1)+1] = (-1)*(TIMER_MAXPULS-wert);
        }

        TCNT1  = 0 - 16000;
        TCCR1A = 0;
        TCCR1B = 0x01;
        TIMSK |= _BV(TOIE2) | _BV(TOIE1);

        sei();
    }

    void servo_set(uint8_t ionr, uint8_t value)
    { /* set servo speed
       */

        uint16_t wert                  ;
        uint8_t index = io_info(ionr,2);

        if (index != 255)
        {
            wert = MINPULS+(MAXPULS-MINPULS)/256*value;
            // calculate hightime
            servo_pulslength[index<<1]=(-1)*wert;
            // sum of low and hightime for one servo is 2ms
            servo_pulslength[(index<<1)+1] = (-1)*(TIMER_MAXPULS-wert);
        }
    }

    ISR(SIG_OVERFLOW1)
    { /* INTERRUPT: timer for software pwm to control servos
       */

        static uint8_t servo_indexhalf = 0;

        switch(servo_indexhalf)
        {
            case 0 : io_set(SERVO0,1); break;
            case 1 : io_set(SERVO0,0); break;
            case 2 : io_set(SERVO1,1); break;
            case 3 : io_set(SERVO1,0); break;
            case 4 : io_set(SERVO2,1); break;
            case 5 : io_set(SERVO2,0); break;
            case 6 : io_set(SERVO3,1); break;
            case 7 : io_set(SERVO3,0); break;
            case 8 : io_set(SERVO4,1); break;
            case 9 : io_set(SERVO4,0); break;
            case 10: io_set(SERVO5,1); break;
            case 11: io_set(SERVO5,0); break;
            case 12: io_set(SERVO6,1); break;
            case 13: io_set(SERVO6,0); break;
            case 14: io_set(SERVO7,1); break;
            case 15: io_set(SERVO7,0); break;
            case 16: io_set(SERVO8,1); break;
            case 17: io_set(SERVO8,0); break;
            case 18: io_set(SERVO9,1); break;
            case 19: io_set(SERVO9,0); break;
        }
        TCNT1 = servo_pulslength[servo_indexhalf]; // set time for next interrupt

        servo_indexhalf++;
        if(servo_indexhalf == 20) servo_indexhalf = 0; // VALUE LOWER THAN 20 CAN DAMAGE SERVO!
    }
#endif