#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>

#include "board.h"
#include "iocomfort.h"
#include "main.h"

//calculate the OCR0A value for the given velocity in m/s
//0.0065 m/s - 1.5625 m/s
//step_val(f) based on f=f_cpu/(2*presc*(1+OCR0A))
#define step_f(v)   v*80000
#define step_val(v) F_CPU/(128*step_f(v))-1

typedef enum{false, true} bool;

static volatile uint8_t spi_shift_data;
static volatile uint32_t x_pos = 0;
static volatile uint32_t y_pos = 0;
static volatile uint32_t z_pos = 0;

static volatile bool X_STOP_q1 = 0;
static volatile bool X_STOP_q2 = 0;
static volatile bool Y_STOP_q1 = 0;
static volatile bool Y_STOP_q2 = 0;
static volatile bool Z_STOP_q1 = 0;
static volatile bool Z_STOP_q2 = 0;


ISR(INT1_vect){
    if(!X_STOP_q1)
        X_STOP_q1 = 1;
}

ISR(INT2_vect){
    if(!Y_STOP_q1)
        Y_STOP_q1 = 1;
}

ISR(INT3_vect){
    if(!Z_STOP_q1)
        Z_STOP_q1 = 1;
}

ISR(TIMER0_COMPA_vect){
    if(!(spi_shift_data & _BV(X_EN))){
        if(spi_shift_data & _BV(X_DIR)){
            x_pos++;
        }else{
            if(x_pos > 0){
                x_pos--;
            }else{
                if(!X_STOP_q2)
                    X_STOP_q2 = 1;
            }
        }
    }
    if(!(spi_shift_data & _BV(Y_EN))){
        if(spi_shift_data & _BV(Y_DIR)){
            if(y_pos > 0){
                y_pos--;
            }else{
                if(!Y_STOP_q2)
                    Y_STOP_q2 = 1;
            }
        }else{
            y_pos++;
        }
    }
    if(!(spi_shift_data & _BV(Z_EN))){
        if(spi_shift_data & _BV(Z_DIR)){
            if(z_pos > 0){
                z_pos--;
            }else{
                if(!Z_STOP_q2)
                    Z_STOP_q2 = 1;
            }
        }else{
            z_pos++;
        }
    }
}

static void init_gpio(void){
    CFG_OUTPUT(LED_STATUS);
    CFG_OUTPUT(STEP);
    CFG_PULLUP(X_STOP);
    CFG_PULLUP(Y_STOP);
    CFG_PULLUP(Z_STOP);
}

static void init_timer(void){
    OCR0A = step_val(0.05);
    TCCR0A = _BV(WGM01) | _BV(COM0A0);
    TCCR0B = _BV(CS01) | _BV(CS00);
    TIMSK0 = _BV(OCIE0A);
}

static void init_spi(void){
    CFG_INPUT(SPI_MISO);
    CFG_OUTPUT(SPI_SS);
    CFG_OUTPUT(SPI_CLK);
    CFG_OUTPUT(SPI_MOSI);
    SET_HIGH(SPI_SS);
    SPCR = _BV(SPE) | _BV(MSTR) | _BV(SPR0);
}

static void spi_push(void){
    SET_LOW(SPI_SS);
    SPDR = spi_shift_data;
    while(!(SPSR & _BV(SPIF)));
    SET_HIGH(SPI_SS);
}

static uint8_t motor(uint32_t x, uint32_t y, uint32_t z){
    static bool _x;
    static bool _y;
    static bool _z;
    if((x>x_pos) && !(X_STOP_q1)){
        spi_shift_data &= ~_BV(X_EN);
        spi_shift_data |= _BV(X_DIR);
        X_STOP_q2 = 0;
        _x = 0;
    }else if((x<x_pos) && !(X_STOP_q2)){
        spi_shift_data &= ~_BV(X_EN);
        spi_shift_data &= ~_BV(X_DIR);
        X_STOP_q1 = 0;
        _x = 0;
    }else if((x==x_pos) || ((X_STOP_q1) && !(x<x_pos)) || ((X_STOP_q2) && !(x>x_pos))){
        spi_shift_data |= _BV(X_EN);
        _x = 1;
    }
    if((y<y_pos) && !(Y_STOP_q2)){
        spi_shift_data &= ~_BV(Y_EN);
        spi_shift_data |= _BV(Y_DIR);
        Y_STOP_q1 = 0;
        _y = 0;
    }else if((y>y_pos) && !(Y_STOP_q1)){
        spi_shift_data &= ~_BV(Y_EN);
        spi_shift_data &= ~_BV(Y_DIR);
        Y_STOP_q2 = 0;
        _y = 0;
    }else if((y==y_pos) || ((Y_STOP_q1) && !(y<y_pos)) || ((Y_STOP_q2) && !(y>y_pos))){
        spi_shift_data |= _BV(Y_EN);
        _y = 1;
    }
    if((z<z_pos) && !(Z_STOP_q2)){
        spi_shift_data &= ~_BV(Z_EN);
        spi_shift_data |= _BV(Z_DIR);
        Z_STOP_q1 = 0;
        _z = 0;
    }else if((z>z_pos) && !(Z_STOP_q1)){
        spi_shift_data &= ~_BV(Z_EN);
        spi_shift_data &= ~_BV(Z_DIR);
        Z_STOP_q2 = 0;
        _z = 0;
    }else if((z==z_pos) || ((Z_STOP_q1) && !(z<z_pos)) || ((Z_STOP_q2) && !(z>z_pos))){
        spi_shift_data |= _BV(Z_EN);
        _z = 1;
    }
    return _x + _y + _z;
}

int main(void){
    init_gpio();
    init_timer();
    init_spi();
    EIMSK = _BV(INT1) | _BV(INT2) | _BV(INT3);
    EICRA = _BV(ISC11) | _BV(ISC21) | _BV(ISC31);

    sei();

    spi_shift_data = _BV(X_EN) | _BV(Y_EN) | _BV(Z_EN) | _BV(X_DIR);

    uint8_t run = 0;

    while(1){
        spi_push();
        if(pgm_read_byte(&x_array[run]) == 15000)
            SET_HIGH(LED_STATUS);
/*        if(run<(sizeof(x_array)/sizeof(uint32_t))){
            if(motor(pgm_read_byte(&x_array[run]),pgm_read_byte(&y_array[run]),0)==3)
                run++;
        }
*/    }

    return 0;
}