#include <avr/io.h>
#include <stdio.h>

#include "kraftsensor.h"
#include "modbus.h"
#include "modbus-master.h"

int32_t kraftsensor_value;
uint8_t kraftsensor_valid;

int32_t kraftsensor_zero_offset = 0/*197700*/;

void do_kraftsensor(){
    //static int32_t last_read;
    //static int32_t old_value;
    uint16_t m_data[4];
    //int32_t kraftsensor_read;

    /* read 2 16bit values and merge to 32bit signed integer */
    readReg(1,0,2);
    if(wait_receive(2, m_data, 100)){
        kraftsensor_valid = 0;
        printf("modbus error\n\r");
    }
    else{
        kraftsensor_valid = 1;
        int32_t tmp = (uint32_t)m_data[1]<<16;
        tmp |= m_data[0];

        //kraftsensor_value = tmp;
        /* conversion magic to milliNewton */
        kraftsensor_value = (((tmp /* + 539363*/)*9.81)/177.380)+kraftsensor_zero_offset;

        //if(abs(kraftsensor_read - old_value) > 10000){
        //    if(abs(last_read - kraftsensor_read) > 10000){
        //        kraftsensor_value = old_value;
        //        //printf("delta: %ld\tvalue:%ld\n", kraftsensor_read - old_value, kraftsensor_read);
        //        //printf("spike\n");
        //    }
        //    else{
        //        kraftsensor_value = kraftsensor_read;
        //        //printf("jump\n");
        //    }
        //}
        //else{
        //    kraftsensor_value = kraftsensor_read;
        //}
        //last_read = kraftsensor_read;
        //old_value = kraftsensor_value;
    }

}