#include <avr/io.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include "serial.h"
#include "writepin.h"
#include "WirelessRelay.h"
#include "pwm.h"
#include "encoder.h"

bool serialDispatch(Serial* serial , WirelessRelay* relayOne,  WirelessRelay* relayTow,  WirelessRelay* relayThree,  WirelessRelay* relayFour, Pwm16b* pwm)
{
    
    if(serial->dataIsWaiting())
    {
        char buffer[7];
        const int length = serial->getString(buffer, 7); 
        if(length == 4 && buffer[0] == 'S' && buffer[1] == 'T' && buffer[2] == 'A' && buffer[3] == 'T' ) return true;
        else if(length == 5)
        {
            if (buffer[0] == 'C' && buffer[1] == 'H' )
            {
                switch(buffer[2])
                {
                    case '0':
                        //writePin(&PORTB, PB3, buffer[4] == '1' ? true : false); pin is dead
                        serial->putString("OK\n");
                        break;
                    case '1':
                        writePin(&PORTB,  PB4, buffer[4] == '1' ? true : false);
                        serial->putString("OK\n");
                        break;
                    case '2':
                        writePin(&PORTB, PB5, buffer[4] == '1' ? true : false);
                        serial->putString("OK\n");
                        break;
                }
            }
           else if (buffer[0] == 'W' && buffer[1] == 'R' )
            {
                switch(buffer[2])
                {
                    case '0':
                        serial->putString("OK\n");
                        buffer[4] == '1' ? relayOne->on() : relayOne->off();
                    break;
                    case '1':
                        serial->putString("OK\n");
                        buffer[4] == '1' ? relayTow->on() : relayTow->off();
                    break;
                    case '2':
                        serial->putString("OK\n");
                        buffer[4] == '1' ? relayThree->on() : relayThree->off();
                    break;
                }
            }
            else if (buffer[0] == 'O' && buffer[1] == 'P' && buffer[2] == 'T')
            {
                 writePin(&PORTB, PB0, buffer[4] == '1' ? true : false);
                 serial->putString("OK\n");
            }
        }
        else if (length == 6 && buffer[0] == 'P' && buffer[1] == 'W' && buffer[2] == 'M')
        {
            char inNumber[2];
            inNumber[0]=buffer[4];
            inNumber[1]=buffer[5];
            pwm->setDuty(atoi(inNumber));
            serial->putString("OK\n");
        }
        else if (length == 6 && buffer[0] == 'L' && buffer[1] == 'E' && buffer[2] == 'D')
        {
            switch(buffer[3])
            {
                case '0':
                    writePin(&PORTC,  PC4, buffer[5] == '1' ? true : false);
                    serial->putString("OK\n");
                break;
                case '1':
                    writePin(&PORTC,  PC5, buffer[5] == '1' ? true : false);
                    serial->putString("OK\n");
                    break;
                case '2':
                    writePin(&PORTD,  PD4, buffer[5] == '1' ? true : false);
                    serial->putString("OK\n");
                    break;
            }
        }
    }
    return false;
}

void serialOutput(Serial* serial, Encoder* encoder, volatile unsigned char *inPort)
{
 serial->putString("VARS_");
 readPin( inPort, PC1 ) ? serial->putChar('0') : serial->putChar('1');
 readPin( inPort, PC2 ) ? serial->putChar('0') : serial->putChar('1');
 int16_t buffer = encoder->getPosition();
 serial->putString( (char*)&buffer, sizeof(buffer));
 serial->putChar('\n');
}

int main() 
{
     DDRB   = 0b11111111;
     DDRD   = 0b11111011;//(1 << PD3) | (1 << PD7) | (1 << PD3);
     DDRC   = 0x00;
     PORTC  = 0xFF;
     
     writePin(&PORTD, PD7, false);
     bool StepperEnable = false;
     
    sei();
    
    Serial serial;
    
    Pwm16b pwm;
    pwm.setDuty(0x0000);
    pwm.on();
    
    
    WirelessRelay relayOne(&PORTD, PD3,    0b1001110000000000);
    WirelessRelay relayTow(&PORTD, PD3,    0b1001101000000000);
    WirelessRelay relayThree(&PORTD, PD3, 0b1001100100000000);
    WirelessRelay relayFour(&PORTD, PD3,   0b1001100010000000);
    
    Encoder encoder(&PINC, PC3, PC4);
    
    serial.putString("CNCextension v1.2 starting\n");
    
    
    while(true)
    {
        encoder.poll();
        if (serialDispatch(&serial, &relayOne, &relayTow, &relayThree, &relayFour, &pwm)) serialOutput(&serial, &encoder, &PINC );
       
        writePin(&PORTB, PB2, ( PORTB & (1 << PB3) || PORTB & (1 << PB4) || PORTB & (1 << PB5)) );
        
        //char buffer[16];
        //sprintf(buffer, "ENC_%05d\n", encoder.getPosition());
        //serial.putString(buffer);

        
        if(!readPin(&PIND, PD2) && StepperEnable == false)
        {
            bool abort = false;
            for (int i = 0; i < 100; i++) 
            {
                if(readPin(&PIND, PD2)) abort = true;
                _delay_ms(10);
            }
            if(!abort)
            {
                StepperEnable = true;
                writePin(&PORTD, PD7, true);
                //serial.putString("Enabeling Steppers\n");
            }
        }
        else if( readPin(&PIND, PD2) && StepperEnable == true  )
        {
            bool abort = false;
            for (int i = 0; i < 10; i++) 
            {
                if(!readPin(&PIND, PD2)) abort = true;
                _delay_ms(5);
            }
            if(!abort)
            {
                StepperEnable = false;
                writePin(&PORTD, PD7, false);
                //serial.putString("Disabeling Steppers\n");
            }
        }
        
    }

    return 0; //master interupt.
}