#include <avr/io.h>
#include <stdint.h>
#include <stdlib.h>
#include <util/delay.h>
#include <stdio.h>
#include <avr/interrupt.h>

#include "kraftsensor.h"
#include "modbus.h"
#include "common.h"

int32_t kraftsensor_value;
uint8_t kraftsensor_valid;

int32_t kraftsensor_zero_offset = 0/*197700*/;

void timer2_init()
{
    TCCR2A = (1<<WGM21); //TIMER0 SET-UP: CTC MODE
    TCCR2B|=(1<<CS21); //prescaler 8
    OCR2A = 200; 
	TIMSK2|=(1<<OCIE2A);
}

void kraftsensor_init(){
    modbusSetAddress(1); //better set this to sth.
	modbusInit();
    timer2_init(); // modbus tick timer
}

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

}

uint8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout){

    uint8_t breaker = timeout;
    while(!receiveOkay && breaker) { //wait for client response, time out after 1s
        breaker--;
        _delay_ms(1);
        if(breaker==0)
            return -1;
    }

    if(receiveOkay) { //if this fails, there was either no response or a crc error
        if(rxbuffer[1]&0x80) { //client responded with an error code
            //handle the error
            return -1;
        }
        else {
            for(uint8_t x=0;x<len;x++) { //rxbuffer[2] should be 8 (4 registers => 8 bytes). You might want to check this at this point.
                dest[x]=(rxbuffer[3+x*2]<<8)+rxbuffer[4+x*2]; //do sth with the acquired data.
            }
        }

    }
    return 0;
}

void readReg(uint8_t slaveid, uint16_t address, uint8_t amount) {
    _delay_ms(2);
    rxbuffer[0]=slaveid;
    modbusSetAddress(slaveid);
    rxbuffer[1]=0x03;
    rxbuffer[2]=(address>>8)&0xFF;
    rxbuffer[3]=address&0xFF;
    rxbuffer[4]=0x00;
    rxbuffer[5]=amount;
    modbusSendMessage(5);
}

void writeReg(uint8_t slaveid, uint16_t address, uint16_t value) {
    _delay_ms(2);
    rxbuffer[0]=slaveid;
    modbusSetAddress(slaveid);
    rxbuffer[1]=0x06;
    rxbuffer[2]=(address>>8)&0xFF;
    rxbuffer[3]=address&0xFF;
    rxbuffer[4]=(value>>8)&0xFF;;
    rxbuffer[5]=value&0xFF;
    modbusSendMessage(5);
}

ISR(TIMER2_COMPA_vect) { //this ISR is called 9765.625 times per second
    //PORTH &= ~(1<<5);
	modbusTickTimer();
    //PORTH |= (1<<5);
}