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


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

ISR(TIMER1_OVF_vect) {
    if(ioHelperReadBit(outStates, MOTOR_TRANS_DIR)){
        spule_trans_pos -= 1;
        if(spule_trans_homed && spule_trans_pos == 84999){
            TCCR1B &= ~(_BV(CS11));
            TIMSK5 |= 1<<TOIE5;
            printf("nachlauf aufbauen");
        }
        if(spule_trans_homed && spule_trans_pos == 4*3540){
            ICR1 = 0.5*(ICR5/0.7);
            OCR1A = ICR1/2;
            printf("nachlauf abbauen");
        }
        if(spule_trans_homed && spule_trans_pos <= 0){
            ICR1 = ICR5/0.7;
            OCR1A = ICR1/2;
            printf("front stop\n");
            set_Output(MOTOR_TRANS_DIR, 0); // direction: back
        }
    }
    else{
        spule_trans_pos += 1;
        if(spule_trans_homed && spule_trans_pos == 3540){
            TCCR1B &= ~(_BV(CS11));
            TIMSK5 |= 1<<TOIE5;
            printf("nachlauf aufbauen");
        }
        if(spule_trans_homed && spule_trans_pos == (85000 - (4*3540)) ){
            ICR1 = 0.5*(ICR5/0.7);
            OCR1A = ICR1/2;
            printf("nachlauf abbauen");
        }
        if(spule_trans_pos >= 85000){
            ICR1 = ICR5/0.7;
            OCR1A = ICR1/2;
            printf("end stop\n");
            set_Output(MOTOR_TRANS_DIR, 1); // direction: front
        }
    }

}

ISR(TIMER5_OVF_vect) {
    static int16_t cnt = 0;
    cnt++;

    if(cnt >= 2*4000){
        cnt = 0;
        TCCR1B |= _BV(CS11); //TURN ON
        TIMSK5 &= ~(1<<TOIE5);
    }
}