#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;

int32_t spule_trans_pos = 0;
uint8_t spule_trans_homed = 0;

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;
}

void do_spule(){

    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/0.7;
        OCR1A = ICR1/2;
    }

    if(!spule_trans_homed){
        ICR1 = 100;
        OCR1A = 50;
        set_Output(MOTOR_TRANS_DIR, 1); // direction: front
        TCCR1B |= _BV(CS11); //TURN ON
    }
    else{
        /* speed regulation - keep taenzer at 10% */
        int32_t tmp = (100 - (int32_t)taenzer_state.pos/1000)*10;

        if(tmp < -75000/abzug_speed/2)
            tmp = -75000/abzug_speed/2;

        ICR5=75000/abzug_speed + tmp;
        OCR5C = ICR5/2;

        ICR1 = ICR5/0.7;
        OCR1A = ICR1/2;

        if (read_Input(BTN_WICKELN_EIN, RISING)) {
            TCCR5B |= _BV(CS51); //TURN ON
            TCCR1B |= _BV(CS11); //TURN ON
        }
        if (read_Input(BTN_WICKELN_AUS, RISING)) {
            TCCR5B &= ~(_BV(CS51));
            TCCR1B &= ~(_BV(CS11));
        }

        if (read_Input(BTN_INIT, RISING)) {
            spule_trans_homed = 0;
            taenzer_state.homed = 0;
            taenzer_state.active = 0;
            windings = 0;
            windings_wakeup = 0;
        }
    }
}

ISR(TIMER1_OVF_vect) {
    if(ioHelperReadBit(outStates, MOTOR_TRANS_DIR)){
        spule_trans_pos -= 1;
    }
    else{
        spule_trans_pos += 1;
    }
    //TODO keep track if position stays in bounds
}

ISR(TIMER5_OVF_vect) {
    static uint16_t steps = 0;
    steps++;
    if(steps == 5000){
        windings++;
        steps=0;
        printf("windungen: %d\n", windings);
    }

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

    if(windings % 25 == 0 && steps == 0){
        ICR1 = ICR5/0.7;
        OCR1A = ICR1/2;
        set_Output(MOTOR_TRANS_DIR, TOGGLE);
        printf("toggle\n");
    }
    if(windings % 25 == 1 && steps == 0){
        printf("nachlauf aufbauen ---");
        TCCR1B &= ~(_BV(CS11));
    }
    if(windings % 25 == 3 && steps == 0){
        TCCR1B |= _BV(CS11);
        printf("done\n");
    }
    if(windings % 25 == 21 && steps == 0){
        ICR1 = 0.5*(ICR5/0.7);
        OCR1A = ICR1/2;
        printf("nachlauf abbauen\n");
    }
}