#include "Arduino.h"
#include "modbusSlave.h"
#include "modbus.h"

modbusSlave::modbusSlave() {
}

/*
Set the Serial Baud rate.
Reconfigure the UART for 8 data bits, no parity, and 1 stop bit.
and flush the serial port.
*/
void modbusSlave::setBaud(SoftwareSerial *__serial, word baud) {
    _baud = baud;
    _serial = __serial;
    // calculate the time period for 3 characters for the given bps in ms.
    _frameDelay = 24000 / _baud;

    _serial->begin(baud);

    // defaults to 8-bit, no parity, 1 stop a bit
    // clear parity, stop bits, word length
    // UCSR0C = UCSR0C & B11000001;
    // UCSR0B = UCSR0B & B11111011;

    // Set word length to 8 bits
    // UCSR0C = UCSR0C | B00000110;

    // No parity
    // UCSR0C = UCSR0C | B00000000;

    // 1 Stop bit
    // UCSR0C = UCSR0C | B00000100;

    _serial->flush();
}

/*
Retrieve the serial baud rate
*/
word modbusSlave::getBaud(void) {
    return (_baud);
}

/*
Generates the crc for the current message in the buffer.
*/

void modbusSlave::calcCrc(void) {
    byte CRCHi = 0xFF,
            CRCLo = 0x0FF,
            Index,
            msgLen,
            *msgPtr;

    msgLen = _len - 2;
    msgPtr = _msg;

    while (msgLen--) {
        Index = CRCHi ^ *msgPtr++;
        CRCHi = CRCLo ^ _auchCRCHi[Index];
        CRCLo = _auchCRCLo[Index];
    }
    _crc = (CRCHi << 8) | CRCLo;
}

/*
  Checks the UART for query data
*/
void modbusSlave::checkSerial(void) {
    // while there is more data in the UART than when last checked
    while (_serial->available() > _len) {
        // update the incoming query message length
        _len = _serial->available();
        // Wait for 3 byte-widths of data (SOM/EOM)
// 		delayMicroseconds(RTUFRAMETIME);
        delay(_frameDelay);
        // Check the UART again
    }
}

/*
Copies the contents of the UART to a buffer
*/
void modbusSlave::serialRx(void) {
    byte i;

    // allocate memory for the incoming query message
    _msg = (byte *) malloc(_len);

    // copy the query byte for byte to the new buffer
    for (i = 0; i < _len; i++)
        _msg[i] = _serial->read();
}

/*
Generates a query reply message for Digital In/Out status update queries.
*/
void modbusSlave::getDigitalStatus(byte funcType, word startreg, word numregs) {
    // initialize the bit counter to 0
    byte bitn = 0;

    // if the function is to read digital inputs, then add 10001 to the start register
    // else add 1 to the start register
    if (funcType == READ_DI)
        startreg += 10001;
    else
        startreg += 1;

    // determine the message length
    // for each group of 8 registers, the message length increases by 1
    _len = numregs / 8;
    // if there is at least one incomplete byte's worth of data,
    // then add 1 to the message length for the partial byte.
    if (numregs % 8)
        _len++;
    // allow room for the Device ID byte, Function type byte, data byte count byte, and crc word
    _len += 5;

    // allocate memory of the appropriate size for the message
    _msg = (byte *) malloc(_len);

    // write the slave device ID
    _msg[0] = _device->getId();
    // write the function type
    _msg[1] = funcType;
    // set the data byte count
    _msg[2] = _len - 5;

    // For the quantity of registers queried
    while (numregs--) {
        // if a value is found for the current register, set number bitn of msg[3]
        // else a bit clear it
        if (_device->get(startreg))
            bitSet(_msg[3], bitn);
        else
            bitClear(_msg[3], bitn);
        // increment the bit index
        bitn++;
        // increment the register
        startreg++;
    }

    // generate the crc for the query reply and append it
    this->calcCrc();
    _msg[_len - 2] = _crc >> 8;
    _msg[_len - 1] = _crc & 0xFF;
}

void modbusSlave::getAnalogStatus(byte funcType, word startreg, word numregs) {
    word val;
    word i = 0;

    // if the function is to read analog inputs, then add 30001 to the start register
    // else add 40001 to the start register
    if (funcType == READ_AI)
        startreg += 30001;
    else
        startreg += 40001;

    // calculate the query reply message length
    // for each register queried add 2 bytes
    _len = numregs * 2;
    // allow room for the Device ID byte, Function type byte, data byte count byte, and crc word
    _len += 5;

    // allocate memory for the query response
    _msg = (byte *) malloc(_len);

    // write the device ID
    _msg[0] = _device->getId();
    // write the function type
    _msg[1] = funcType;
    // set the data byte count
    _msg[2] = _len - 5;

    // for each register queried
    while (numregs--) {
        // retrieve the value from the register bank for the current register
        val = _device->get(startreg + i);
        // write the high byte of the register value
        _msg[3 + i * 2] = val >> 8;
        // write the low byte of the register value
        _msg[4 + i * 2] = val & 0xFF;
        // increment the register
        i++;
    }

    // generate the crc for the query reply and append it
    this->calcCrc();
    _msg[_len - 2] = _crc >> 8;
    _msg[_len - 1] = _crc & 0xFF;
}

void modbusSlave::setStatus(byte funcType, word reg, word val) {
    // Set the query response message length
    // Device ID byte, Function byte, Register byte, Value byte, CRC word
    _len = 8;
    // allocate memory for the message buffer.
    _msg = (byte *) malloc(_len);


    // write the device ID
    _msg[0] = _device->getId();
    // if the function type is a digital written
    if (funcType == WRITE_DO) {
        // Add 1 to the register value and set it's value to val
        _device->set(reg + 1, val);
        // write the function type to the response message
        _msg[1] = WRITE_DO;
    } else {
        // else add 40001 to the register and set its value to val
        _device->set(reg + 40001, val);

        // write the function type of the response message
        _msg[1] = WRITE_AO;
    }

    // write the register number high byte value
    _msg[2] = reg >> 8;
    // write the register number low byte value
    _msg[3] = reg & 0xFF;
    // write the control value's high byte
    _msg[4] = val >> 8;
    // write the control value's low byte
    _msg[5] = val & 0xFF;

    // calculate the crc for the query reply and append it.
    this->calcCrc();
    _msg[_len - 2] = _crc >> 8;
    _msg[_len - 1] = _crc & 0xFF;
}

void modbusSlave::run(void (*callback)()) {

    byte deviceId;
    byte funcType;
    word field1;
    word field2;

    int i;

    // initialize message length
    _len = 0;

    // check for data in the reception buffer
    this->checkSerial();

    // if there is nothing in the reception buffer, bail.
    if (_len == 0) {
        return;
    }

    // retrieve the query message from the serial uart
    this->serialRx();

    // if the message id is not 255, and
    //   device id does not match bail
    if ((_msg[0] != 0xFF) &&
        (_msg[0] != _device->getId())) {
        return;
    }
    // calculate the checksum of the query message minus the checksum it came with.
    this->calcCrc();

    // if the checksum does not match, ignore the message
    if (_crc != ((_msg[_len - 2] << 8) + _msg[_len - 1]))
        return;

    // copy the function type from the incoming query
    funcType = _msg[1];

    // copy field 1 from the incoming query
    field1 = (_msg[2] << 8) | _msg[3];

    // copy field 2 from the incoming query
    field2 = (_msg[4] << 8) | _msg[5];

    // free the allocated memory for the query message
    free(_msg);
    // reset the message length;
    _len = 0;

    // generate query response based on a function type
    switch (funcType) {
        case READ_DI:
            this->getDigitalStatus(funcType, field1, field2);
            break;
        case READ_DO:
            this->getDigitalStatus(funcType, field1, field2);
            break;
        case READ_AI:
            this->getAnalogStatus(funcType, field1, field2);
            break;
        case READ_AO:
            this->getAnalogStatus(funcType, field1, field2);
            break;
        case WRITE_DO:
            this->setStatus(funcType, field1, field2);
            break;
        case WRITE_AO:
            this->setStatus(funcType, field1, field2);
            break;
        default:
            return;
    }

    // if a reply was generated
    if (_len) {
        int i;
        // send the reply to the serial UART
        // Senguino doesn't support a bulk serial write command....
        for (i = 0; i < _len; i++)
            _serial->write(_msg[i]);
        // free the allocated memory for the reply message
        free(_msg);
        // reset the message length
        _len = 0;
    }
    if (callback != nullptr) {
        callback();
    }
}
