/*
 * Copyright © 2008 Stéphane Raimbault <stephane.raimbault@gmail.com>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>

#include "modbus.h"

/* Tests based on PI-MBUS-300 documentation */
#define SLAVE_1     0x01
#define SLAVE_2     0x02
#define USE_SLAVE_2 0

#define INTER_FRAME_DELAY   1750

#define DEFAULT_BAUD        57600L

const uint16_t UT_COIL_STATUS_ADDRESS = 0x0;
const uint16_t UT_COIL_STATUS_NB_POINTS = 0x08;
const uint8_t UT_COIL_STATUS_TAB[] = { 0xCD, 0x6B, 0xB2, 0x0E, 0x1B }; 

const uint16_t UT_INPUT_STATUS_ADDRESS = 0xC4;
const uint16_t UT_INPUT_STATUS_NB_POINTS = 0x16;
const uint8_t UT_INPUT_STATUS_TAB[] = { 0xAC, 0xDB, 0x35 };

const uint16_t UT_HOLDING_REGISTERS_ADDRESS = 0x6B;
const uint16_t UT_HOLDING_REGISTERS_NB_POINTS = 0x3;
const uint16_t UT_HOLDING_REGISTERS_TAB[] = { 0x022B, 0x0000, 0x0064 };

const uint16_t UT_INPUT_REGISTERS_ADDRESS = 0x08;
const uint16_t UT_INPUT_REGISTERS_NB_POINTS = 0x1;
const uint16_t UT_INPUT_REGISTERS_TAB[] = { 0x000A };

void turnOffCoils(modbus_param_t* mb_param);
void readSerial(modbus_param_t* mb_param, int port);
void writeSerial(modbus_param_t* mb_param, int port, int cnt);


int main(int argc, char* argv[])
{
    uint8_t *tab_rp_status;
    uint16_t *tab_rp_registers;
    modbus_param_t mb_param;
    uint8_t value1, value2;
    int nb_points;
    int ret;
    int loop_cnt = 0;
    int tx_cnt = 0;
    long baud;
    int err_cnt = 0;

    uint8_t coils[8];
    char buff[32];

    time_t timeNow;
    struct tm *startTm;
    
    /* Get start time of auto */
    timeNow = time(NULL); 
    startTm = localtime(&timeNow);
    printf("Modbus test started on: %s\n", asctime(startTm));

    if ( argc < 2)
    {
        printf("Usage: mtest port [baud]\n");
        printf("Default baud is %ld\n\n", DEFAULT_BAUD);
        printf("Examples:\n");
        printf("./mtest ttyS0        #for Linux\n");
        printf("./mtest com1         #for Cygwin\n");
        printf("\n");
        printf("./mtest ttyS0 38400  #for Linux with non default baud\n");
        return -1;
    }

    /* RTU parity : none, even, odd */
    sprintf(buff, "/dev/%s", argv[1]);

    baud = DEFAULT_BAUD;
    if ( argc == 3)
    {
        baud = atol(argv[2]);
        switch ( baud )
        {
            case 9600:   break;
            case 19200:  break;
            case 38400:  break;
            case 57600:  break;
            case 115200: break;
            default: 
                printf("Invalid baud: %ld\n", baud);
                printf("Supported bauds: 9600, 19200, 38400, 57600, 115200\n");
                return -1;
        }
    }

    modbus_init_rtu(&mb_param, buff, baud, "none", 8, 1);

    if (modbus_connect(&mb_param) == -1) 
    {
        printf("ERROR Connection failed\n");
        exit(1);
    }

    /* Allocate and initialize the memory to store the status */
    nb_points = (UT_COIL_STATUS_NB_POINTS > UT_INPUT_STATUS_NB_POINTS) ? 
        UT_COIL_STATUS_NB_POINTS : UT_INPUT_STATUS_NB_POINTS;
        
    tab_rp_status = (uint8_t *) malloc(nb_points * sizeof(uint8_t));
    memset(tab_rp_status, 0, nb_points * sizeof(uint8_t));
        

    /* Allocate and initialize the memory to store the registers */
    nb_points = (UT_HOLDING_REGISTERS_NB_POINTS > 
                 UT_INPUT_REGISTERS_NB_POINTS) ?
                 UT_HOLDING_REGISTERS_NB_POINTS : UT_INPUT_REGISTERS_NB_POINTS;
    
    tab_rp_registers = (uint16_t *) malloc(nb_points * sizeof(uint16_t));
    memset(tab_rp_registers, 0, nb_points * sizeof(uint16_t));


    printf("** UNIT TESTING **\n");

    printf("\nTEST WRITE/READ:\n");

    /** COIL STATUS **/

    /* Single */
    printf("1/2 force_single_coil (ID 0x01): ");
    ret = force_single_coil(&mb_param, SLAVE_1, UT_COIL_STATUS_ADDRESS, ON);
    usleep(INTER_FRAME_DELAY);

    if (ret == 1) 
    {
        printf("OK\n");
    } 
    else 
    {
        printf("FAILED\n");
        goto close;
    }

#if USE_SLAVE_2
    printf("1/2 force_single_coil (ID 0x02): ");
    ret = force_single_coil(&mb_param, SLAVE_2, UT_COIL_STATUS_ADDRESS, ON);
    usleep(INTER_FRAME_DELAY);
    if (ret == 1) 
    {
        printf("OK\n");
    } 
    else 
    {
        printf("FAILED\n");
        goto close;
    }
#endif

    printf("2/2 read_coil_status (ID 0x01): ");
    ret = read_coil_status(&mb_param, SLAVE_1, UT_COIL_STATUS_ADDRESS, 1, 
                           tab_rp_status);
    usleep(INTER_FRAME_DELAY);
    
    if (ret != 1) 
    {
        printf("FAILED (nb points %d)\n", ret); 
        goto close;
    }

    if (tab_rp_status[0] != ON) 
    {
        printf("FAILED (%0X = != %0X)\n", tab_rp_status[0], ON);
        goto close;
    }
    
    printf("OK\n");

#if USE_SLAVE_2
    printf("2/2 read_coil_status (ID 0x02): ");
    ret = read_coil_status(&mb_param, SLAVE_2, UT_COIL_STATUS_ADDRESS, 1, 
                           tab_rp_status);
    usleep(INTER_FRAME_DELAY);
    
    if (ret != 1) 
    {
        printf("FAILED (nb points %d)\n", ret); 
        goto close;
    }

    if (tab_rp_status[0] != ON) 
    {
        printf("FAILED (%0X = != %0X)\n", tab_rp_status[0], ON);
        goto close;
    }
#endif    

    printf("OK\n");
    /* End single */

    /* force all coils off */
    turnOffCoils(&mb_param);

    /* turn on serial xcvrs */
    force_single_coil(&mb_param, SLAVE_1, 11, ON);
    usleep(INTER_FRAME_DELAY);
    force_single_coil(&mb_param, SLAVE_1, 12, ON);
    usleep(INTER_FRAME_DELAY);
    force_single_coil(&mb_param, SLAVE_1, 13, ON);
    usleep(INTER_FRAME_DELAY);

#if USE_SLAVE_2
    force_single_coil(&mb_param, SLAVE_2, 11, ON);
    usleep(INTER_FRAME_DELAY);
    force_single_coil(&mb_param, SLAVE_2, 12, ON);
    usleep(INTER_FRAME_DELAY);
    force_single_coil(&mb_param, SLAVE_2, 13, ON);
    usleep(INTER_FRAME_DELAY);
#endif

    for (;;)
    {
        /* read the coils */
        ret = read_coil_status(&mb_param, SLAVE_1, UT_COIL_STATUS_ADDRESS, 
                               UT_COIL_STATUS_NB_POINTS, coils);

        usleep(INTER_FRAME_DELAY);
        
        if ( ret == UT_COIL_STATUS_NB_POINTS )
        {
            value1 = get_byte_from_bits(coils, 0, 8);

            /* inc the byte */
            ++value1;

            /* covert the byte into an array of bits */
            set_bits_from_byte(coils, 0, value1);

            force_multiple_coils(&mb_param, SLAVE_1, UT_COIL_STATUS_ADDRESS, 
                                 UT_COIL_STATUS_NB_POINTS, coils);

            usleep(INTER_FRAME_DELAY);
        }
        else
        {
            
            timeNow = time(NULL); 
            startTm = localtime(&timeNow);
            
            printf("Failed to read ID_1, zeroing coil value1: %s", 
                   asctime(startTm)); 
            value1 = 0;
            
            /* wait for a couple of seconds before continuing */
            sleep(2);
        }

#if USE_SLAVE_2
        ret = read_coil_status(&mb_param, SLAVE_2, UT_COIL_STATUS_ADDRESS, 
                               UT_COIL_STATUS_NB_POINTS, coils);

        usleep(INTER_FRAME_DELAY);
        
        if ( ret == UT_COIL_STATUS_NB_POINTS )
        {
            /* conver the bits to a byte */
            value2 = get_byte_from_bits(coils, 0, 8);

            /* inc the byte */
            ++value2;

            /* covert the byte into an array of bits */
            set_bits_from_byte(coils, 0, value2);

            /* write the coils back */
            force_multiple_coils(&mb_param, SLAVE_2, UT_COIL_STATUS_ADDRESS, 
                                 UT_COIL_STATUS_NB_POINTS, coils);

            usleep(INTER_FRAME_DELAY);
        }
        else
        {
            timeNow = time(NULL); 
            startTm = localtime(&timeNow);
            
            printf("Failed to read ID_2, zeroing coil value2: %s",
                   asctime(startTm)); 
            value2 = 0;
            
            /* wait for a couple of seconds before continuing */
            sleep(2);
        }

        /* clear all coils if they get out of sync */
        if ( value1 != value2 )
        {
            turnOffCoils(&mb_param);

            timeNow = time(NULL); 
            startTm = localtime(&timeNow);
            
            printf("Coils out of sync (ID_1 = 0x%02X, ID_2 = 0x%02X): %s", 
                   (int)value1, (int)value2, asctime(startTm));
            printf("Clearing all coils: %s", asctime(startTm));

            /* increment error counter */
            ++err_cnt;
            printf("Error count %d\n", err_cnt);
            
            /* wait for a couple of seconds before continuing */
            sleep(2);
        }
        else
        {
            /* decrement error counter if it is greater than zero */
            if ( err_cnt > 0 )
                --err_cnt;
        }
#endif

        readSerial(&mb_param, 0);
        readSerial(&mb_param, 1);
        readSerial(&mb_param, 2);


        if ( ++loop_cnt > 20 )
        {
            loop_cnt = 0;
            writeSerial(&mb_param, 0, ++tx_cnt);
            writeSerial(&mb_param, 1, tx_cnt);
            writeSerial(&mb_param, 2, tx_cnt);
        }

#define ERR_COUNT_MAX   10
        if ( err_cnt > ERR_COUNT_MAX )
        {
            timeNow = time(NULL); 
            startTm = localtime(&timeNow);
            
            printf("Error count exceed maximum of %d.\n", ERR_COUNT_MAX);
            printf("Exiting program at %s", asctime(startTm));
            goto close;
        }

        /* flush standard out so you can see output when the program is 
        running in the background */
        fflush(stdout);
    }

close:
    /* Free the memory */
    free(tab_rp_status);                                           
    free(tab_rp_registers);

    /* Close the connection */
    modbus_close(&mb_param);
        
    return 0;
}


void turnOffCoils(modbus_param_t* mb_param)
{
    int i;
    uint8_t coils[8];

    /* clear coils */
    for (i = 0; i < 8; i++)
        coils[i] = OFF;

    force_multiple_coils(mb_param, SLAVE_1, UT_COIL_STATUS_ADDRESS, 
                         UT_COIL_STATUS_NB_POINTS, coils);
    usleep(INTER_FRAME_DELAY);

#if USE_SLAVE_2
    force_multiple_coils(mb_param, SLAVE_2, UT_COIL_STATUS_ADDRESS, 
                         UT_COIL_STATUS_NB_POINTS, coils);
    usleep(INTER_FRAME_DELAY);
#endif
}

#define RX_REG_CNT  9
void readSerial(modbus_param_t* mb_param, int port)
{
    uint16_t reg;
    uint16_t rx_reg[RX_REG_CNT];
    char rx_char[2 * RX_REG_CNT];
    unsigned char hi_byte;
    unsigned char lo_byte;
    int i;
    int address;
    time_t timeNow;
    struct tm *rxTm;

    switch ( port )
    {
        case 0: address = 0x106; break;
        case 1: address = 0x206; break;
        case 2: address = 0x306; break;
        default: return;
    }
    
    i = read_holding_registers(mb_param, SLAVE_1, address, RX_REG_CNT, rx_reg);

    if ( i == RX_REG_CNT )
    {
        /* clear the buffer out */
        for (i = 0; i < (RX_REG_CNT * 2); ++i)
            rx_char[i] = 0;

        for (i = 1; i < RX_REG_CNT; ++i)
        {
            reg = rx_reg[i];
/*
            if ( rx_reg[i] != 0 )
                printf("rx_reg[%02hu] = 0x%04hX\n", i, rx_reg[i]);
*/
            hi_byte = (unsigned char)(reg >> 8);
            lo_byte = (unsigned char)(0x00FF & reg);
            
            if ( (hi_byte == '\r') || (hi_byte == '\n') )
                rx_char[2 * (i - 1)] = '.';
            else
                rx_char[2 * (i - 1)] = hi_byte;


            if ( (lo_byte == '\r') || (lo_byte == '\n') )
                rx_char[(2 * (i - 1)) + 1] = '.';
            else
                rx_char[(2 * (i - 1)) + 1] = lo_byte;
        }

        if ( rx_reg[0] )
        {



            printf("RX_%d[%02hu]: \"%s\" - ", port, rx_reg[0], rx_char);
            
            /* timestamp result */
            timeNow = time(NULL); 
            rxTm = localtime(&timeNow);
            printf("%s", asctime(rxTm));

        }

        return;
    }

    printf("Error: readSerial(...) failed\n");
    return;
}

#define TX_BUFF_SZ  32
#define TX_REG_CNT  ((TX_BUFF_SZ / 2) + 1)

typedef struct 
{
    uint16_t txReq;
    char txBuff[TX_BUFF_SZ];
} TxData;
    
typedef union TxMsg
{
    TxData txData;
    uint16_t txReg[TX_REG_CNT];
} TxMsg;


void byteSwap16(char* bytes)
{
    char swap_buff[2];
    
    swap_buff[0] = bytes[0];
    swap_buff[1] = bytes[1];
    
    bytes[0] = swap_buff[1];
    bytes[1] = swap_buff[0];
}

void writeSerial(modbus_param_t* mb_param, int port, int cnt)
{
    int i;
    uint16_t reg;
    
    TxMsg txMsg;
    int address;

    switch ( port )
    {
        case 0: address = 0x186; break;
        case 1: address = 0x286; break;
        case 2: address = 0x386; break;
        default: return;
    }

    /* fill the buff to send */
    memset(txMsg.txData.txBuff, 0, TX_BUFF_SZ);
    sprintf(txMsg.txData.txBuff, "TX_%d WRITE: %d\r\n", port, cnt);
    txMsg.txData.txReq = (uint16_t)strlen(txMsg.txData.txBuff);

#if 0
    printf("TX_BUFF_SZ = %d\n", TX_BUFF_SZ);
    printf("TX_REG_CNT = %d\n", TX_REG_CNT);
    
    printf("txMsg.txData.txReq = 0x%04hX\n", txMsg.txData.txReq);
    printf("txMsg.txData.txBuff = '%s'\n", txMsg.txData.txBuff);
#endif

    for (i = 0; i < TX_REG_CNT; ++i)
    {
        /* swap the char bytes back as reading them from the union swaps them */
        if ( i > 0 )
            byteSwap16((char*)&txMsg.txReg[i]);

#if 0
        printf("txMsg.txReg[%d] = 0x%04hX\n", i, txMsg.txReg[i]);
#endif

    }

    i = preset_multiple_registers(mb_param, SLAVE_1, 
                                  address, TX_REG_CNT, txMsg.txReg);
    if ( i != TX_REG_CNT )
    {
        printf("Error: writeSerial(...): failed to write bytes\n");
        return;
    }
    
    i = read_holding_registers(mb_param, SLAVE_1, (address - 4), 1, &reg);
    if ( i != 1 )
    {
        printf("Error: writeSerial(...): failed to read queue count\n");
        return;
    }

    if ( txMsg.txData.txReq != (0x00FF & reg) )
    {
        printf("Error: writeSerial(...) failed.\n");
        printf("requested write of  %hu\n", txMsg.txData.txReq);
        printf("successfully queued %hu\n", reg);
    }

#if 0
    printf("TX_WRITE_ACCEPT = %d\n", (0x00FF & reg));
    reg >>= 8;
    printf("TX_QUEUE_FREE   = %d\n", reg);
#endif


}