#include <avr/io.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include "serial.h"
#include "writepin.h"
#include "train.h"
#include "eeprom.h"
#include "bitrep.h"

#define MAX_TRAINS 16
#define COMMAND_BUFFER_SIZE 64
#define SNPRINTF_BUFFER_SIZE 64
#define EPPROM_SIZE 1024

char buffer[SNPRINTF_BUFFER_SIZE];

uint16_t storedTrains = 0;
Train trains[MAX_TRAINS];

bool autoff = true;
bool powerIsOn = true;

#define TICK_MAX 255
volatile uint8_t tick = 0;

volatile bool resendEvent = false;

ISR(TIMER0_OVF_vect) 
{
    ++tick;
    if(tick  == 0) resendEvent = true;
    if((tick & 0b00000111) < 1) Train::setOutput(Train::HIGH);
    else Train::setOutput(Train::LOW);
}

void timer0InterruptEnable(const bool enable)
{
     if(enable) TIMSK0 = 0b00000001;
     else TIMSK0 = 0b00000000;
}

void save_state()
{
    cli();
    EEPROM_write_char( 0, storedTrains );
    EEPROM_write_char( 1, autoff );
    for(uint16_t i = 0; i < storedTrains; i++)
    {
        EEPROM_write_char( i*2+32, trains[i].getAddress());
        EEPROM_write_char( i*2+32+1, trains[i].getProtocol());
    }
    sei();
}

void restore_state()
{
    storedTrains = EEPROM_read_char(0);
    autoff = EEPROM_read_char(1);
    if(storedTrains > MAX_TRAINS ) 
    {
        for(uint16_t i = 0; i < EPPROM_SIZE; i++) EEPROM_write_char(i, 0);
        storedTrains = 0;
    }
    else for(uint8_t i = 0; i < storedTrains; i++)
    {
        trains[i].setAddress(EEPROM_read_char(32+i*2)); 
        trains[i].setProtocol(EEPROM_read_char(32+1+i*2));
    }
}

inline static void printHelp(Serial* serial)
{
    serial->write_p(PSTR("Available Commands: \n\
                help                       :  Show this prompt.\n\
                train add [address]        :  Add train.\n\
                train delete               :  Delete last train.\n\
                train list                 :  Print list of saved trains.\n\
                train [nn] stop            :  Stop nth train.\n\
                train [nn] speed [sp]      :  Set nth train speed.\n\
                train [nn] function [x]    :  Toggle x'ed fuction on train n.\n\
                train [nn] reverse         :  Reverse train n.\n\
                stop                       :  stop all trains\n\
                power off                  :  power off the rail\n\
                power on                   :  power on the rail\n\
                power auto                 :  power off the rail when no trains are moveing\n\
                dump                       :  prints epprom contence\n\
                erase                      :  Erase epprom.\n"));
}


void trainDispatch(char* inBuffer, Serial* serial)
{
    cli();
    if( strcmp(inBuffer, "add") == 0 )
    {
        char* token = strtok(NULL, " ");
        uint8_t address = 0;
        if(token != NULL) address = strtol(token, nullptr, 2 );
        if(address != 0 && storedTrains < MAX_TRAINS) 
        {
            trains[storedTrains].setAddress(address);
            
            token = strtok(NULL, " ");
            if(token != NULL && strcmp(token, "delta") == 0 )  trains[storedTrains].setProtocol(Train::M_DELTA);
            else trains[storedTrains].setProtocol(Train::M_DIGITAL);
            
            uint8_t size = snprintf(buffer, SNPRINTF_BUFFER_SIZE, "TRAIN saved! NUMBER: %u ADRESS: %s%s PROTCOL: %s\n", storedTrains, bit_rep[address >> 4], bit_rep[address & 0x0F], trains[storedTrains].getProtocol() == Train::M_DELTA ? "delta" : "digital" );
            serial->write(buffer, size);
            
            storedTrains++;
            save_state();
        }
        serial->write_p(PSTR("Usage: train add [address]"));
    }
    else if( strcmp(inBuffer, "delete") == 0)
    {
        serial->write_p(PSTR("Train: "));
        serial->write(storedTrains);
        serial->write_p(PSTR(" deleted\n"));
        storedTrains--;
    }
    else if( strcmp(inBuffer, "list") == 0 )
    {
        serial->write_p(PSTR("Trains:\n"));
        for(uint8_t i = 0; i < storedTrains; i++)
        {
            snprintf(buffer, SNPRINTF_BUFFER_SIZE, "NUMBER: %u ID: %s%s CURRENT PACKET: %x", i, bit_rep[trains[i].getAddress() >> 4], bit_rep[trains[i].getAddress() & 0x0F], trains[i].getLastPacket());
            serial->write(buffer, SNPRINTF_BUFFER_SIZE);
            serial->write_p(PSTR(" PROTOCOL: "));
            if(trains[i].getProtocol() == Train::M_DELTA)serial->write_p(PSTR("delta\n"));
            else serial->write_p(PSTR("digital\n"));
        }
        serial->putChar('\n');
    }
    else
    {
        uint8_t id = strtol(inBuffer, nullptr, 10);
        if(id < storedTrains )
        {
            if(powerIsOn == false)
            {
                powerIsOn = true;
                Train::setOutput(Train::LOW);
                _delay_ms(100);
                timer0InterruptEnable(true);
            }
            char* token = strtok(NULL, " ");
            if( token != NULL &&  strcmp(token, "speed") == 0 )
            {
                token = strtok(NULL, " ");
                trains[id].setSpeed(atoi(token));
                serial->write_p(PSTR("Set Train speed\n"));
            }
            else if(token != NULL && strcmp(token, "function") == 0 ) 
            {
                token = strtok(NULL, " ");
                char* boolToken = strtok(NULL, " ");
                if(token != NULL && boolToken !=  NULL)
                {
                    uint8_t functionId = atoi(token);
                    bool on = (strcmp(boolToken, "on") == 0);
                    trains[id].sendFunction(functionId, on );
                    serial->write_p(PSTR("Set Train function "));
                    serial->write(functionId);
                    serial->write(on ? " on\n" : " off\n");
                }
            }
            else if(token != NULL && strcmp(token, "probe") == 0 ) 
            {
                for(uint16_t j = 0; j < 1024; j++)
                {
                    trains[id].sendRaw(j);
                    snprintf(buffer, SNPRINTF_BUFFER_SIZE, "TRYING: %x\n", j);
                    serial->write(buffer, strlen(buffer));
                    sei();
                    _delay_ms(250);
                    cli();
                    
                }
            }
            else if(token != NULL && strcmp(token, "raw") == 0 ) 
            {
                token = strtok(NULL, " ");
                if(token != NULL)
                {
                    cli();
                    uint16_t i = strtol(token, nullptr, 16 );
                    snprintf(buffer, SNPRINTF_BUFFER_SIZE, "SENDING: %x\n", i);
                    serial->write(buffer, strlen(buffer));
                    for(uint8_t j = 0; j < 100; j++)
                    {
                        trains[id].sendRaw(i);
                        _delay_ms(20);
                    }
                    serial->write_p(PSTR("Finished\n"));
                    sei();
                }
            }
            else if( token != NULL && strcmp(token, "reverse") == 0 )
            {
                trains[id].reverse();
                serial->write_p(PSTR("Reversed Train\n"));
            }
            else if( token != NULL && strcmp(token, "stop") == 0 )trains[id].stop();
            else serial->write_p(PSTR("Not a valid command\n"));
        }
        else serial->write_p(PSTR("Id out of range.\n"));
    }
    sei();
}

void powerDispatch(char* token, Serial* serial)
{
    if(strcmp(token, "off") == 0)
    {
        timer0InterruptEnable(false);
        Train::setOutput(Train::OFF);
        powerIsOn = false;
    }
    else if( strcmp(token, "on") == 0)
    {
        for(uint16_t i = 0; i < storedTrains; i++)
        {
            trains[i].setSpeed(0);
        }
        Train::setOutput(Train::LOW);
        timer0InterruptEnable(true);
    }
    else if(strcmp(token, "auto") == 0)
    {
        token = strtok(NULL, " ");
        if(token != NULL && strcmp(token, "on") == 0)
        {
            autoff = true;
            serial->write_p(PSTR("auto power off turned on.\n"));
            save_state();
        }
        else if(token != NULL && strcmp(token, "off") == 0)
        {
            autoff = false;
            serial->write_p(PSTR("auto power off turned off.\n"));
            save_state();
        }
        else 
        {
            serial->write_p(PSTR("argument must be \"on\" or \"off\". This feature is currently "));
            autoff ? serial->write_p(PSTR("on.\n")) : serial->write_p(PSTR("off.\n"));
        }
    }
}


void serialDispatch(Serial* serial)
{
    if(serial->dataIsWaiting())
    {
        char buffer[COMMAND_BUFFER_SIZE];
        unsigned int length = serial->getString(buffer, COMMAND_BUFFER_SIZE);
        if(length > 2)
        {
            serial->write_p(PSTR("Got: "));
            serial->putChar('\"');
            serial->write(buffer, length);
            serial->write("\"\n");
            char* token = strtok(buffer, " ");
            if(length > 4 && strcmp(token, "train") == 0)
            {
                token = strtok(NULL, " ");
                if(token != NULL)trainDispatch(token, serial);
            }
            else if(length > 4 && strncmp(token, "erase", 4) == 0)
            {
                for(uint16_t i = 0; i < EPPROM_SIZE; i++) EEPROM_write_char(i, 0);
                serial->write_p(PSTR("EEPROM erased\n"));
                storedTrains = 0;
            }
            else if(length > 3 && strcmp(token, "dump") == 0)
            {
                for(uint16_t i = 0; i < EPPROM_SIZE; i++) 
                {
                    if(i != 0) serial->putChar(',');
                    serial->write((uint16_t)EEPROM_read_char(i));
                }
                serial->putChar('\n');
            }
            else if(length > 3 && strcmp(token, "stop") == 0)
            {
                for(uint16_t i = 0; i < storedTrains; i++)
                {
                    cli();
                    trains[i].stop();
                    sei();
                }
            }
            else if(length > 3 && strcmp(token, "power") == 0)
            {
                token = strtok(NULL, " ");
                if(token != NULL)powerDispatch(token, serial);
            }
            else if(length > 3 && strcmp(token, "help") == 0)
            {
                printHelp(serial);
            }
            else
            {
                serial->putChar('\"');
                serial->write(buffer, length);
                serial->putChar('\"');
                serial->write_p(PSTR(" is not a valid command\n"));
            }
        }
    }
}

int main() 
{
    TCNT0 = 0;
    TCCR0B = (1<<CS01) /*| (1<<CS00)*/; // run timer0 with /64 scaler
    
    DDRD =  (1 << PD2) | (1 << PD3) | (1 << PD4) | (1 << PD5);
    
    restore_state();
    
    if(autoff) 
    {
        timer0InterruptEnable(false);
        Train::setOutput(Train::OFF);
    }
    else
    {
        timer0InterruptEnable(true);
        Train::setOutput(Train::LOW);
    }
    
    sei();
    
    Serial serial;

    serial.write_p(PSTR("TrainController v0.1 starting\n"));
    
    uint8_t trainToResend = 0;
    
    while(true)
    {        
        if(resendEvent && storedTrains > 0) 
        {
            timer0InterruptEnable(false);
            trains[trainToResend].resendData();
            trainToResend++;
            if(storedTrains <= trainToResend) trainToResend = 0;
            resendEvent = false;
            timer0InterruptEnable(true);
        }
        
        if(autoff) 
        {
            bool trainsRunning = false;
            for(uint16_t i = 0; i < storedTrains; i++) trainsRunning = trainsRunning || trains[i].isActive();
            if(!trainsRunning)
            {
                powerIsOn = false;
                timer0InterruptEnable(false);
                Train::setOutput(Train::OFF);
            }
        }
        serialDispatch(&serial);
    }
    return 0;
}