#include "modbus_master.h"
#include "modbus.h"
#include <util/delay.h>

#include <stdio.h>

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

    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
            printf("error\n\r");
            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);
}