#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdint.h>
#include <stdio.h>
#include "avrIOhelper/io-helper.h"

#include "taenzer.h"
#include "abzug.h"

volatile uint16_t windings = 0;
volatile uint16_t windings_wakeup = 0;

volatile uint8_t trans_state = 0;

int32_t spule_trans_pos = 0;
uint8_t spule_trans_homed = 0;

#define TRANS_ROT_FACTOR 0.14

void timer1_init()
{
    TCCR1A |= (1<<COM1A1);
    TCCR1B |= _BV(WGM13);

    ICR1 = 100;
    OCR1A = 50;

    DDRB |= 1 << 5;
    TIMSK1 |= 1<<TOIE1;
}

void timer5_init()
{
    TCCR5A |= (1<<COM5C1);
    TCCR5B |= _BV(WGM53);

    ICR5 = 1000;
    OCR5C = 500;

    DDRL |= 1 << 5;
    TIMSK5 |= 1<<TOIE5;
}

static void spule_onoff(uint8_t state){
    if(state){
        TCCR5B |= _BV(CS51); // ROTATION
        TCCR1B |= _BV(CS11); // TRANSLATION
    }
    else{
        TCCR5B &= ~(_BV(CS51));
        TCCR1B &= ~(_BV(CS11));
    }
}

void set_spooling_speed(uint16_t speed){
    ICR5=speed;
    OCR5C = ICR5/2;

    ICR1 = ICR5/TRANS_ROT_FACTOR;
    OCR1A = ICR1/2;
}

void do_spule(){
    // Translatoric axis homeing code
    if(read_Input(IN_SPULE_HOME, LEVEL) && spule_trans_homed == 0){
        spule_trans_homed = 1;
        spule_trans_pos = 0;

        TCCR1B &= ~(_BV(CS11));
        set_Output(MOTOR_TRANS_DIR, 0); // direction: back

        ICR1 = ICR5/TRANS_ROT_FACTOR;
        OCR1A = ICR1/2;
    }

    // if not homed goto home
    if(!spule_trans_homed){
        ICR1 = 100;
        OCR1A = 50;
        set_Output(MOTOR_TRANS_DIR, 1); // direction: front
        TCCR1B |= _BV(CS11); //TURN ON
    }

    // manual forwarding if button is held
    else if(!get_abzug_state() || (get_abzug_state() && read_Input(IN_BREMSE_STATE, LEVEL)) ){
        if(read_Input(BTN_WICKELN_EIN, LEVEL)){
            set_spooling_speed(300);
            spule_onoff(1);
        }
        else
            spule_onoff(0);
    }

    // normal operation
    else{
        /* speed regulation - keep taenzer at 10% */
        float p = 100.0/(int32_t)(taenzer_state.pos/1000);
        p-=1;
        p/=2;
        p+=1;
        //tmp = (int32_t)(taenzer_state.pos/10000);
        //printf("%ld\n", tmp);

        //printf("temp1: %d\n", tmp);
        //TODO fix bounds
        //if(tmp < -7500/abzug_speed/2)
        //    tmp = -7500/abzug_speed/2;
        //printf("temp2: %d\n", tmp);
        if(p < 0.5)
            p = 0.5;
        if(p > 2)
            p = 2;

        uint16_t base_speed = (14000/abzug_speed);
        uint16_t ctrl_speed = base_speed * p;

        if(ctrl_speed <= 70)
            ctrl_speed = 70;

        ICR5 = ctrl_speed;
        OCR5C = ICR5/2;

        if(trans_state != 4)
            ICR1 = ICR5/TRANS_ROT_FACTOR;
        else
            ICR1 = 0.5*(ICR5/TRANS_ROT_FACTOR);
        OCR1A = ICR1/2;

        if (read_Input(BTN_WICKELN_EIN, RISING) && !read_Input(IN_BREMSE_STATE, LEVEL)) {
            spule_onoff(1);
        }
        if (read_Input(BTN_WICKELN_AUS, RISING)) {
            spule_onoff(0);
        }
    }
    if (read_Input(BTN_INIT, RISING)) {
        spule_trans_homed = 0;
        taenzer_state.homed = 0;
        taenzer_state.active = 0;
        windings = 0;
        windings_wakeup = 0;
    }

    if (read_Input(IN_BREMSE_STATE, FALLING)) {
        printf("draußen\n");
        spule_onoff(1);
    }

    if (read_Input(IN_BREMSE_STATE, RISING)) {
        printf("drinne\n");
        spule_onoff(0);
    }

    //PORTH |= (1<<5);
}

ISR(TIMER1_OVF_vect) {
    //PORTH &= ~(1<<5);
    //if(ioHelperReadBit(outStates, MOTOR_TRANS_DIR)){
    if(PORTB & (1<<6)){
        spule_trans_pos -= 1;
    }
    else{
        spule_trans_pos += 1;
    }
    //TODO keep track if position stays in bounds
    //PORTH |= (1<<5);
}

ISR(TIMER5_OVF_vect) {
    //PORTH &= ~(1<<5);
    static uint16_t steps = 0;
    steps++;
    if(steps == 25000){
        windings++;
        steps=0;
        printf("windungen: %d\t", windings);
        printf("trans pos: %ld\n", spule_trans_pos);
        printf("speed %d\n", ICR1);
    }

    if(windings == windings_wakeup){
        TIMSK1 |= 1<<TOIE1;
    }

    uint8_t windings_on_layer = windings % 25;

    if(windings_on_layer == 0 && steps == 0){
        trans_state = 1;
        ICR1 = ICR5/TRANS_ROT_FACTOR;
        OCR1A = ICR1/2;
        set_Output(MOTOR_TRANS_DIR, TOGGLE);
        printf("toggle at pos: %ld\n", spule_trans_pos);
        printf("speed %d\n", ICR1);
    }
    if(windings_on_layer == 1 && steps == 0){
        trans_state = 2;
        printf("nachlauf aufbauen\n");
        TCCR1B &= ~(_BV(CS11));
    }
    if(windings_on_layer == 3 && steps == 0){
        trans_state = 3;
        TCCR1B |= _BV(CS11);
        printf("done\n");
        printf("speed %d\n", ICR1);
    }
    if(windings_on_layer == 21 && steps == 0){
        trans_state = 4;
        ICR1 = 0.5*(ICR5/TRANS_ROT_FACTOR);
        OCR1A = ICR1/2;
        printf("nachlauf abbauen\n");
        printf("speed %d\n", ICR1);
    }
    //;PORTH |= (1<<5);
}