#ifndef __AVR_ATmega8__
    #define __AVR_ATmega8__
#endif

//#define F_CPU 12000000
#define F_CPU 3686400
//#define F_CPU 8000000
const char _ID = 0x2;
const char _ALL = 0xff;

const int STEPS = 100;
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdlib.h>
#include <math.h>
#include "Enums.h"

const float stepRatio = 1.0220;
const char STARTBYTE = 0x7E;
const int BUFFER_SIZE = 40;
char buff[BUFFER_SIZE];
volatile int buffCount=0;
volatile char receivedByte;
bool startReceived = true;
bool byteHandled = true;
volatile bool dataFinished = false;
bool forMe = false;
volatile char data = 0x0;

/*************************************************************************
    Enable RS485 transmissionin
**************************************************************************/
inline void RS485_TE()
{
    PORTD |= _BV(PD2);
}

/*************************************************************************
    Enable RS485 reception
**************************************************************************/
inline void RS485_RE()
{
    PORTD &= ~_BV(PD2);
}

/*************************************************************************
    Functions
**************************************************************************/
void SetR(unsigned char value )
{
    if (value > 255)
    {
        value = 255;
    }
    if (value >= 0)
    {
        OCR1A = 255-value;
    }
}

void SetG(unsigned char value )
{
    if (value > 255)
    {
        value = 255;
    }
    if (value >= 0)
    {
        OCR1B = 255-value;
    }
}

void SetB(unsigned char value )
{
    if (value > 255)
    {
        value = 255;
    }
    if (value >= 0)
    {
        OCR2 = 255-value;
    }
}

void USART_Transmit(short int data )
{
    //Wait for empty transmit buffer
    while ( !( UCSRA & (1<<UDRE)) )
    {
    }
    RS485_TE();
    UDR = data;
}

void SendString(char *str)
{
    while(*str)
    {
        char c = *str++;
        USART_Transmit(c);
    }
}

void SendString(int val)
{
    char* str;
    itoa(val, str, 10);
    SendString(str);
}

char xtod(char c)
{
    if (c>='0' && c<='9') return c-'0';
    if (c>='A' && c<='F') return c-'A'+10;
    if (c>='a' && c<='f') return c-'a'+10;
    return c=0;        // not Hex digit
}

int xstrtoi(char *hex)      // hex string to integer
{
    return xtod(hex[0])*16 + xtod(hex[1]);
}

int ConvertValue(int value)
{
    if (value == 0)
    {
        return 0;
    }
    else
    {
        return (int)roundf(pow(stepRatio, (value - 1) ));
    }
}


/*************************************************************************
    Interrupts
**************************************************************************/
// Data received
ISR(USART_RXC_vect)
{
    while ( !(UCSRA & (1<<RXC)) );
    if((UCSRA & (1<<FE))||(UCSRA & (1<<PE)))  // If frame error or parity error
    {
        char garbage = UDR;                          // UDR -> Garbage
    }
    else
    {
        char data = UDR;
        if (data == STARTBYTE)
        {
            startReceived = true;
        }
        else if (startReceived)
        {
            if (forMe)
            {
                buff[buffCount] = data;
                buffCount++;
                if (buffCount > BUFFER_SIZE)
                {
                    buff[0] = '\0';
                    dataFinished = false;
                    startReceived = false;
                    forMe = false;
                    buffCount = 0;
                }
            }
            else
            {
                if (data == _ID)
                {
                    forMe = true;
                    buffCount = 0;
                }
                else
                {
                    startReceived = false;
                }
            }
            if (data == '\0' || data == '\n')
            {
                dataFinished = true;
                startReceived = false;
                forMe = false;
            }
        }
    }
}

ISR(USART_TXC_vect)
{
    RS485_RE(); //RS485 is receiving
}

/*************************************************************************
    Initialize
**************************************************************************/
void PWM_Init()
{
    DDRB = (1 << PINB1) | (1 << PINB2) | (1 << PINB3);
    TCNT1 = 0x00;

    TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM12) | (1 << WGM10);
    TCCR1B = (1 << CS10);
    TCCR2 = (1 << COM21) | (1 << WGM20) | (1 << CS20);
}

void USART_Init(int baudrate)
{
    // Set baud rate
    int rate = F_CPU/16/baudrate - 1;

    // Set baud rate
    UBRRH = (short int)(rate>>8);
    UBRRL = (short int)rate;

    UCSRB=(1<<RXCIE)|(1<<RXEN)|(1<<TXCIE)|(1<<TXEN);

    // Set frame format to 8 data bits, no parity, 1 stop bit
    UCSRC = (1 << URSEL) | (1 << UCSZ0) | (1 << UCSZ1);

    RS485_RE();
}

/*************************************************************************
    Main
**************************************************************************/
int main()
{
    _delay_ms(500);
    unsigned char oldR = 0;
    unsigned char oldG = 0;
    unsigned char oldB = 0;
    unsigned char requestedRed = 0;
    unsigned char requestedGreen = 0;
    unsigned char requestedBlue = 0;
    float stepR = 0;
    float stepG = 0;
    float stepB = 0;
    int stepCount = 0;
    int delayCount = 0;
    PWM_Init();
    USART_Init(4800);
    sei();

    SendString("Device ready:");
    _delay_ms(100);
    char* id = '\0';
    itoa(_ID, id, 10);
    SendString( id );

    while (true)
    {
        if (dataFinished)
        {
            dataFinished = false;
            buffCount = 0;
            char cmd = buff[0];
            switch (cmd)
            {
                case Status:
                {
                    char str[11];
                    str[0] = STARTBYTE;
                    str[1] = _ID;
                    str[2] = 0x1;
                    char valR[3];
                    char valG[3];
                    char valB[3];
                    itoa(requestedRed, valR, 16);
                    itoa(requestedGreen, valG, 16);
                    itoa(requestedBlue, valB, 16);
                    str[3] = valR[0];
                    str[4] = valR[1];
                    str[5] = valG[0];
                    str[6] = valG[1];
                    str[7] = valB[0];
                    str[8] = valB[1];
                    str[9] = '\n';
                    str[10] = '\0';
                    SendString(str);
                    break;
                }
                case Set:
                {
                    USART_Transmit(STARTBYTE);
                    USART_Transmit(_ID);
                    SendString("ACK\n");
                    oldR = requestedRed;
                    oldG = requestedGreen;
                    oldB = requestedBlue;

                    char rStr[3];
                    char gStr[3];
                    char bStr[3];

                    rStr[0] = buff[1];
                    rStr[1] = buff[2];
                    rStr[2] = '\0';

                    gStr[0] = buff[3];
                    gStr[1] = buff[4];
                    gStr[2] = '\0';

                    bStr[0] = buff[5];
                    bStr[1] = buff[6];
                    bStr[2] = '\0';

                    int red = xstrtoi(rStr);
                    int green = xstrtoi(gStr);
                    int blue = xstrtoi(bStr);
                    requestedRed = ConvertValue( red );
                    requestedGreen = ConvertValue( green );
                    requestedBlue = ConvertValue( blue );

                    stepR = (float)(requestedRed - oldR) / (float)STEPS;
                    stepG = (float)(requestedGreen - oldG) / (float)STEPS;
                    stepB = (float)(requestedBlue - oldB) / (float)STEPS;
                    stepCount = 0;
                    break;
                }
                default:
                {
                    SendString("~NACK\n");
                    break;
                }
            }
        }

        if (stepCount < STEPS)
        {
            if (delayCount >= 4000)
            {
                delayCount = 0;
                stepCount++;
                unsigned char newR = oldR + (int)roundf(stepR * (float)stepCount);
                unsigned char newG = oldG + (int)roundf(stepG * (float)stepCount);
                unsigned char newB = oldB + (int)roundf(stepB * (float)stepCount);
                SetR(newR);
                SetG(newG);
                SetB(newB);
            }
            delayCount++;
        }
    }
}