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


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;

}

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){
        set_Output(MOTOR_TRANS_DIR, 1); // direction: front
        TCCR1B |= _BV(CS11); //TURN ON
    }
    else{
        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_TAENZER_START, RISING)) {
            ICR5-=15;
            OCR5C = ICR5/2;

            ICR1 = ICR5/0.7;
            OCR1A = ICR1/2;
        }
        if (read_Input(BTN_SPULENWECHSEL, RISING)) {
            ICR5+=15;
            OCR5C = ICR5/2;

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

        if (read_Input(BTN_INIT, RISING)) {
            spule_trans_homed = 0;
            ICR1 = 100;
            OCR1A = 50;
        }
    }
}

ISR(TIMER1_OVF_vect) {
}

ISR(TIMER5_OVF_vect) {
}