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

#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#define TAENZER_KRAFT_SETPOINT 12000
#define TAENZER_KRAFT_HYST 500


taenzer_state_t taenzer_state;
PID_VARS_INIT(regler);

taenzer_state_t taenzer_state = {
    .homed = 0,
    .pos = 0,
    .force_setpoint = TAENZER_KRAFT_SETPOINT,
    .active = 0
};

double pid(PID_vars *vars, double current_err) {

  /* current_error = setpoint - current_process_variable */

  vars->_integral_sum += current_err*(vars->_dt);

  double output = (vars->Kp)*current_err                      \
                + (vars->Ki)*(vars->_integral_sum)            \
                + (vars->Kd)*((current_err-(vars->_prev_err)) \
                              /(vars->_dt));

  vars->_prev_err = current_err;

  /* limit output within output_min and output_max */
  if (output>(vars->output_max))
    output = vars->output_max;
  else if (output<(vars->output_min))
    output = vars->output_min;

  return output;
}

extern void send_settings(void);
void do_taenzer(){

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

    if (read_Input(BTN_KRAFT_PLUS, RISING)) {
        taenzer_state.force_setpoint += 1000;
#if PLC_MQTT_ENABLED
        send_settings();
#endif
        if(taenzer_state.homed == 1 && read_Input(BTN_KRAFT_MINUS, LEVEL))
        {
            kraftsensor_zero_offset = -(kraftsensor_value-kraftsensor_zero_offset);
        }
    }
    if (read_Input(BTN_KRAFT_MINUS, RISING)) {
        taenzer_state.force_setpoint -= 1000;
#if PLC_MQTT_ENABLED
        send_settings();
#endif
        if(taenzer_state.homed == 1 && read_Input(BTN_KRAFT_PLUS, LEVEL))
        {
            kraftsensor_zero_offset = -(kraftsensor_value-kraftsensor_zero_offset);
        }
    }
    if (read_Input(BTN_TAENZER_START, RISING)) {
        taenzer_state.active = 1;
    }

    /* Force regualtion */
    if(kraftsensor_valid && taenzer_state.active && taenzer_state.homed && taenzer_state.pos >= 0){
        int32_t err = (kraftsensor_value - taenzer_state.force_setpoint);

        double pid_out = pid(&regler, err);
        int32_t out = (int32_t)pid_out;

        ICR4 = 400000/abs(out);
        OCR4A = ICR4/2;

        if(kraftsensor_valid && notaus_state == POWER_ON){
            if(out < -TAENZER_KRAFT_HYST/2  && taenzer_state.pos < 970000UL){
                set_Output(MOTOR_TAENZER_DIR, 0); // direction: down
                TCCR4B |= _BV(CS41); //TURN ON
            }
            else if(out > TAENZER_KRAFT_HYST/2 && taenzer_state.pos > 0){
                set_Output(MOTOR_TAENZER_DIR, 1); // direction: up
                TCCR4B |= _BV(CS41); //TURN ON
            }
            else{
                TCCR4B &= ~_BV(CS41); //TURN OFF
            }
        }
        else
            TCCR4B &= ~_BV(CS41); //TURN OFF
    }
    //else
    //    TCCR4B &= ~_BV(CS41); //TURN OFF
}

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

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

    ICR4 = 30;
    OCR4A = 15;

    DDRH |= 1 << 3;
}

ISR(TIMER4_OVF_vect) {
    if(ioHelperReadBit(outStates, MOTOR_TAENZER_DIR))
        taenzer_state.pos -= 1;
    else
        taenzer_state.pos += 1;
}

//ISR(TIMER4_OVF_vect) {
//}