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

taenzer_state_t taenzer_state;

void do_taenzer(){

    ///* Homing */
    //if(!taenzer_state.homed){
    //    set_Output(MOTOR_TAENZER_DIR, 1); // direction: up
    //    TCCR4B |= _BV(CS41); //TURN ON
    //}
    //if(read_Input(IN_TAENZER_HOME, LEVEL)){
    //    TCCR4B &= ~_BV(CS41); //TURN OFF
    //    taenzer_state.homed = 1;
    //}

    /* Force regualtion */
    if(kraftsensor_valid /* && taenzer_state.homed */){
        if(kraftsensor_value < 0){
            set_Output(MOTOR_TAENZER_DIR, 0); // direction: down
            TCCR4B |= _BV(CS41); //TURN ON
        }
        else if(kraftsensor_value > 10000){
            set_Output(MOTOR_TAENZER_DIR, 1); // direction: up
            TCCR4B |= _BV(CS41); //TURN ON
        }
        else
            TCCR4B &= ~_BV(CS41); //TURN OFF
    }
}

void timer4_init()
{
    TCCR4A |= (1<<COM4A1);
    TCCR4B |= _BV(WGM43);

//    TIMSK4 |= 1<<TOIE4;
//    TIMSK4 |= 1<<OCIE4A;

    ICR4 = 100;
    OCR4A = 50;

    DDRH |= 1 << 3;
}

//ISR(TIMER4_OVF_vect) {
//}

//ISR(TIMER4_OVF_vect) {
//}