#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "modbus.h"

#define HIGH_LOW_CYCLE_TIME 10

uint16_t holdingRegisters[4];

void timer0100us_start(void) {
	TCCR0B|=(1<<CS01); //prescaler 8
	TIMSK0|=(1<<TOIE0);
}

//Return raw ADC data
uint32_t HX711_get_data(uint8_t clk_PIN, uint8_t data_PIN, uint8_t gain_for_next_conv){
    uint32_t data = 0;
    for(uint8_t i = 0; i < 24; i++){    //Start 24bit transmission
        PORTB |= (1 << clk_PIN);        //Set clock-PIN high
        _delay_us(HIGH_LOW_CYCLE_TIME);
        /*  Shift data to free LSB. Operation placed before writing data to avoid 
            manuell shiftig after loop finsihed.*/
        data <<= 1;                     
        /*  Shift input-status of data containing bit to the
            LSB and set all other bits to 0. Add result to
            currently LSB in data-var.*/
        data |= ((PINB>>data_PIN) & 0x01); //Read and store data
        PORTB &= ~(1 << clk_PIN);        //Set clock-PIN low
        _delay_us(HIGH_LOW_CYCLE_TIME);
    }

    /*  Add extra clk-pluses to set PGA-Gain for next conversion
        1 extra pulse : Channel A Gain = 128
        2 extra pluses: Channel B Gain = 32
        3 extra pluses: Channel A Gain = 64*/
    int8_t extra_clk_pluses = 1;
    switch(gain_for_next_conv){
        case 128: 
            extra_clk_pluses = 1;
            break;
        case 64:
            extra_clk_pluses = 3;
            break;
        case 32:
            extra_clk_pluses = 2;
            break;
        default:
         extra_clk_pluses = 1;
    }
    for(uint8_t i = 0; i < extra_clk_pluses; i++){
        PORTB |= (1 << clk_PIN);    //set clock-PIN high
        _delay_us(HIGH_LOW_CYCLE_TIME);
        PORTB &= ~(1 << clk_PIN);   //set clock-PIN low
        _delay_us(HIGH_LOW_CYCLE_TIME);
    }

    /*  Convert 24bit Two's complement to 32bit Two's complement
        if MSB from 24Bit result is 1 aka negative*/
    if(data & (1L<<23)){
            data |= 0xFFL << 24;
    }
    return data;
}

void modbusGet(void) {
	if (modbusGetBusState() & (1<<ReceiveCompleted))
	{
		switch(rxbuffer[1]) {
			case fcReadHoldingRegisters:
                PORTC ^= 1 << 1;
//                _delay_ms(10);
//                PORTC &= ~(1 << 1);

                while(PINB & (1<<4)); // wait for data ready

                uint32_t tmp = HX711_get_data(3, 4, 128); 
                //int32_t tmp = -1000;
                holdingRegisters[0] = tmp & 0xFFFF;
                holdingRegisters[1] = (tmp>>16) & 0xFFFF;
				modbusExchangeRegisters(holdingRegisters,0,4);
			break;

            default: {
				modbusSendException(ecIllegalFunction);
			}
			break;
		}
	}
}

int main(void){
    DDRC |= 3 << 0;
    DDRB |= (1<<3);

    modbusSetAddress(1);
	modbusInit();
    sei();

    timer0100us_start();

    while(1){
        modbusGet();
        //PORTC ^= 1 << 0;
        //_delay_ms(100);
        //PORTC &= ~(1 << 1);
    }
}

ISR(TIMER0_OVF_vect) { //this ISR is called 9765.625 times per second
	modbusTickTimer();
}