//
// Written March 2014 by John Lydic for Technologic Systems
// Used for testing the TS-Modbus ZPU device reload instructions function.
//
// Requires libmodbus 3.0.2
// Compile commandline:
// gcc mbpeekpoke.c -o mbpeekpoke -lmodbus -I./src -g
//
// Usage: Refer to usage function in the code
//

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <asm/termios.h>
#include <modbus.h>
#include <time.h>
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>

typedef enum {
    false,
    true
} bool;

typedef enum {
    fmtHex,
    fmtSignede,
    fmtUnsignede
} DisplayFormat;

modbus_t *createModbusChannel(uint8_t *aMbConnectionStr, uint32_t aBitrate, int deviceAddress, bool debug, bool ts7670);

#define MODEL_STR_7670 "7670"

#define CMD_BUF_SIZE 64
unsigned char cmdBuf[CMD_BUF_SIZE];

extern char *optarg;

#define MAXTOREAD 32
#define CONNECT_STR_LENGTH 64
#define RESET_PERSIST_REG  0x7f
#define ZPU_RESET          0x0001
#define ZPU_PERSIST_START  0x0002
#define ZPU_PERSIST_STATUS 0x0004

#define BPS300         300
#define BPS600         600
#define BPS1200       1200
#define BPS2400       2400
#define BPS4800       4800
#define BPS9600       9600
#define BPS19200     19200
#define BPS38400     38400
#define BPS57600     57600
#define BPS115200   115200
#define BPS230400   230400
#define BPS460800   460800
#define BPS1040000 1040000
#define BPS2080000 2080000
#define BPS4160000 4160000

#define BPS_FOR_COMMS       BPS1040000
#define BPS_INDEX_FOR_COMMS BIT_RATE_INDEX_1040000

uint32_t BitRates[] = {
    BPS300,
    BPS600,
    BPS1200,
    BPS2400,
    BPS4800,
    BPS9600,
    BPS19200,
    BPS38400,
    BPS57600,
    BPS115200,
    BPS230400,
    BPS460800,
    BPS1040000,
    BPS2080000,
    BPS4160000,
    0
};

bool CheckValidBitrate(uint32_t value)
{
    uint32_t *TablePtr = BitRates;

    if (value == 0)
        return false;

    while (*TablePtr++)
        if (*TablePtr == value)
            return true;

    return false;
}

void Usage()
{
    fprintf(stderr, "\nUsage: mbpeekpoke -switch parameter [OPTIOSN] ...\n"
            "Download and start a ZPU program.  Optionally persist the code\n"
            "Default parameter values in [brackets]\n"
            "  -?                     This help screen\n"
            "  -b  bit_rate           Bit rate [9600]\n" 
            "                         300, 600, 1200, 2400, 4800, 9600, 19200\n"
            "                         38400, 57600, 115200, 1000000, 4000000\n"
            "  -c  connection         IP:Port IP address:port [127.0.0.1:7350]\n" 
            "                         Serial port [/dev/ttySP2]\n"
            "  -D                     Device to access: [C]oil, [R]egister, [I]nput, [P]acket\n" 
            "  -d                     Display packet debugging information [Off]\n" 
            "  -f                     Register display output format:\n"
            "                         'X' hex, 'S' signed decimal 'U' unsigned decimal [Hex]\n" 
            "  -h                     This help screen\n"
            "  -n  number_of_items    number of items to read [1]\n"
            "  -m  Board model #      7670 with serial [any other using TCP socket connection]\n"
            "  -r  address            Read: Device @ specified address\n"
            "  -s  slave_address      hexadecimal 00-f8; 00 is broadcast [0xf7]\n"
            "  -v  data               value to write\n"
            "  -V                     Verbose mode - display additional debug information\n"
            "  -w  address            Write: Device @ specified address\n"
            " The (-w) write option requires a single (-v) variable\n"
            " The (-r) read option can read a number of items with the (-n) switch\n"
            " Both the -w & -r options may be used on the same or different addresses\n"
            " however, they must both use the same device (-D) and the write is performed first\n"
            );
    exit(0);
}

typedef enum {
    INVALID,
    REGISTER,
    COIL,
    INPUT,
    PACKET
} RESOURCE;

bool cOption = false;   // Communications device string set by user
bool dOption = false;   // Display packet debugging information
bool DOption = false;   // Device has been set
bool rOption = false;   // Reading
bool vOption = false;   // Data VALUE has been entered
bool VOption = false;   // Verbose mode
bool wOption = false;   // Writing
bool mOption7670 = false;   // model: initially 7670 woth/tty direct port connection

RESOURCE theResource = INVALID;

const char *optionsList = "?b:c:D:df:F:hm:n:r:s:Vv:w:";

int main(int argc, char **argv)
{
    int aChar;
    uint16_t index;
    uint16_t writeData;
    uint16_t readBuffer[MAXTOREAD];
    uint8_t  coilBuffer[MAXTOREAD];
    uint16_t writeAddress;
    uint16_t readAddress;
    uint16_t numberToRead;
    uint32_t bitRate;
    uint8_t mbConnectionStr[CONNECT_STR_LENGTH+1];
    int slaveAddress;
    DisplayFormat fmtOut = fmtHex;

    modbus_t *mbConnection;

    int rtnStatus = 0;

    bitRate = 9600;  // default
    slaveAddress = 0xf7;
    numberToRead = 1;

    if (argc == 1)
        Usage();

    while ((aChar = getopt(argc, argv, optionsList)) != -1)
    {
        //printf("aChar: 0x%02x  Option: \'%c\'  Optarg: %s\n", aChar,  aChar, optarg);
        switch (aChar)
        {
            case 'b':  // 
                bitRate = atoi(optarg);
                if (!CheckValidBitrate(bitRate))
                {
                    fprintf(stderr,"Invalid bit rate: %d\n", bitRate);
                    return 0;
                }
                break;

            case 'c':  // 
                if (strlen(optarg) > CONNECT_STR_LENGTH)
                {
                    fprintf(stderr, "Invalid connection definition size: %s\n", optarg);
                    return 0;
                }
                cOption = true;
                strcpy(mbConnectionStr, optarg);
                break;

            case 'd':  // Set debugging
                dOption = true;
                break;

            case 'D':  // Set device
                if (strcasecmp("register", optarg) == 0 || strcmp("R", optarg) == 0)
                    theResource = REGISTER;
                else if (strcasecmp("coil", optarg) == 0 || strcmp("C", optarg) == 0)
                    theResource = COIL;
                else if (strcasecmp("input", optarg) == 0 || strcmp("I", optarg) == 0)
                    theResource = INPUT;
                else if (strcasecmp("packet", optarg) == 0 || strcmp("P", optarg) == 0)
                    theResource = PACKET;

                if (!theResource)
                {
                    fprintf(stderr, "Invalid device specified: \'%s\'\n", optarg);
                    exit(0);
                }
                DOption = true;
                break;

            case 'F':  // Set display format
            case 'f':  // Set display format
                if (strlen(optarg) > 1)
                {
                    fprintf(stderr, "Invalid display format size: %s\n", optarg);
                    return 0;
                }

                switch (optarg[0])
                {
                    case 'X':
                    case 'x':
                    default:
                        fmtOut = fmtHex;
                        break;

                    case 'S':
                    case 's':
                        fmtOut = fmtSignede;
                        break;

                    case 'U':
                    case 'u':
                        fmtOut = fmtUnsignede;
                        break;

                }
                break;

            case 'm':  // model
                if (strncmp(optarg, MODEL_STR_7670, 4))
                {
                    fprintf(stderr, "Invalid model number: %s\n", optarg);
                    return 0;
                }
                else
                    mOption7670 = true;
                break;

            case 'n':  // Number to read
                numberToRead = strtol(optarg, NULL, 16);
                if (numberToRead > MAXTOREAD)
                    numberToRead = MAXTOREAD;
                break;

            case 'r':  // read
                readAddress = strtol(optarg, NULL, 16);
                rOption = true;
                break;

            case 's':  // 
                slaveAddress = strtol(optarg, NULL, 16);
                if ((slaveAddress < 0x01) || (slaveAddress > 0xf7))
                {
                    fprintf(stderr,"Slave ID (%0x02x) must be between 0x01 and 0xf7\n", slaveAddress);
                    return 0;
                }
                break;

            case 'v':  // 
                writeData = strtol(optarg, NULL, 16);
                vOption = true;
                break;

            case 'V':  // Set verbose mode
                VOption = true;
                break;

            case 'w':  // write
                writeAddress = strtol(optarg, NULL, 16);
                if (writeAddress > 0x7e)
                {
                    fprintf(stderr,"Write Address (%0x02x) must be between 0x01 and 0xf7\n", writeAddress);
                    return 0;
                }
                wOption = true;
                break;

            case 'h':  //  Display help information
            case '?':  //  Display help information
            default:
                Usage();
        }
    }

    if ((readAddress + numberToRead) > 0x7e)
    {
        fprintf(stderr, "Read Address (0x%02x) PLUS Count (%d) must be <= 0x7e was: 0x%02x\n", readAddress, numberToRead, (readAddress + numberToRead));
        return 0;
    }

    
    if (!cOption)
    {
        if (mOption7670)
            strcpy(mbConnectionStr, "/dev/ttySP2");
        else
            strcpy(mbConnectionStr, "127.0.0.1:7350");
    }

    if (VOption)
        printf("Using Device: %s\n", mbConnectionStr);

    // Open the modbus path

    mbConnection = createModbusChannel(mbConnectionStr, bitRate, slaveAddress, dOption, mOption7670);

    //  Parameters have been entered
    //  Validate and execute

    if (!wOption && !rOption)
    {
        fprintf(stderr, "Must specifiy Read (-r) or Write (-w)\n");
        exit(0);
    }
    else if (!DOption)
    {
        fprintf(stderr, "Device not specified (-D R/C/I/P)\n");
        exit(0);
    }

    if (wOption && !vOption)
    {
        fprintf(stderr, "WRITE requires a value (-v)\n");
        exit(0);
    }

    //  Perform write operation
    if (wOption)
    {
        switch (theResource)
        {
            case REGISTER:
                rtnStatus = modbus_write_register(mbConnection, writeAddress == 0 ? MODBUS_BROADCAST_ADDRESS : writeAddress, writeData);
                if (rtnStatus < 0)
                    printf("rtnStatus: %d failed writing %04X to holding register %04X  Error: %d 0 %s\n", rtnStatus, writeData, writeAddress, errno, modbus_strerror(errno));
                else
                    printf("Value %04X written to HR %04X\n", writeData, writeAddress);
                break;

            case COIL:
                rtnStatus = modbus_write_bit(mbConnection, (int) writeAddress, (int) writeData ? 1 : 0);
                if (rtnStatus < 0)
                    printf("rtnStatus: %d failed writing %d to coil %d  Error: %d 0 %s\n", rtnStatus, writeData ? 1 : 0, writeAddress, errno, modbus_strerror(errno));
                else
                    printf("State %d written to coil %d\n", writeData ? 1 : 0, writeAddress);
                break;

            case INPUT:
                fprintf(stderr, "Cannot WRITE to input\n");
                exit(0);

            case PACKET:
                fprintf(stderr, "Write packet not yet implemented\n");
                exit(0);
        }
    }

    //  Clear the buffer
    for (index = 0; index < MAXTOREAD; index++)
        readBuffer[index] = 0;

    //  Perform read operation
    if (rOption)
    {
        switch (theResource)
        {
            case REGISTER:
                rtnStatus = modbus_read_registers(mbConnection, (int) readAddress, (int) numberToRead, &readBuffer[0]);
                if (numberToRead == 1)
                    printf("Read register 0x%02x: %04x\n", readAddress, readBuffer[0]);
                else
                {
                    printf("Read %d registers @ 0x%02x: ", numberToRead, readAddress);
                    for (index = 0; index < numberToRead; index++)
                    {
                        switch (fmtOut)
                        {
                            case fmtHex:
                                printf("0x%04x ", readBuffer[index]);
                                break;

                            case fmtSignede:
                                printf("%d ", (int16_t) readBuffer[index]);
                                break;

                            case fmtUnsignede:
                                printf("%d ", (uint16_t) readBuffer[index]);
                                break;

                        }
                    }
                    printf("\n");
                }
                break;

            case COIL:
                modbus_read_bits(mbConnection, readAddress, numberToRead, coilBuffer);
                if (numberToRead == 1)
                    printf("Read coil 0x%02x as %x\n", readAddress, coilBuffer[0]);
                else
                {
                    printf("Read coils : 0x%04: ");
                    for (index = 0; index < numberToRead; index++)
                        printf("0x%x ", coilBuffer[index] ? 1 : 0);
                    printf("\n");
                }
                break;


            case INPUT:
                modbus_read_input_registers(mbConnection, readAddress, numberToRead, readBuffer);
                if (numberToRead == 1)
                    printf("Read input 0x%02x as %x\n", readAddress, readBuffer[0]);
                else
                {
                    printf("Read inputs @ 0x%04x: ", readAddress);
                    for (index = 0; index < numberToRead; index++)
                        printf("0x%x ", readBuffer[index]);
                    printf("\n");
                }
                break;

            case PACKET:
                fprintf(stderr, "Cannot READ a packet\n");
                exit(0);
        }
    }

    modbus_close(mbConnection);
    modbus_free(mbConnection);
    return 0;
}

//
//   Create a modbus conection at the specificed bit rate and slave address
//
modbus_t *createModbusChannel(uint8_t *aMbConnectionStr, uint32_t aBitrate, int deviceAddress, bool debug, bool ts7670)
{
    modbus_t *aMbConnection;
    struct termios2 tio2;
    uint32_t theBitrate = aBitrate;
    int status, mbPortFd;
    unsigned char cmdBuf[CMD_BUF_SIZE];

    if (aBitrate > BPS115200)
        theBitrate = BPS38400;

    if (VOption)
        printf("Opening modbus connection @ %d\n", theBitrate);
    if ((aMbConnection = modbus_new_rtu(aMbConnectionStr, theBitrate, 'N', 8, 1)) == NULL)
    {
        fprintf(stderr,"Modbus port %s failed to open. Errno: %d, %s\n",
                aMbConnectionStr, errno, modbus_strerror(errno));
        return 0;
    }

    if (VOption)
        fprintf(stderr, "Opened modbus connection @ %d bps on port %s\n", aBitrate, aMbConnectionStr);

    // Set modbus post-setup options.
    modbus_set_slave(aMbConnection, deviceAddress);
    if (debug)
        modbus_set_debug(aMbConnection, 1);
    if (status  = modbus_connect(aMbConnection))
    {
        fprintf(stderr,"File %s failed to open.\n", optarg);
        fprintf(stderr,"modbus_connect failed Errno: %d, %s\n", errno, modbus_strerror(errno));
        return 0;
    }

    if (aBitrate > BPS115200)
    {
        mbPortFd = modbus_get_socket(aMbConnection); // Get the port fd
        if (VOption)
            printf("Setting %d bps using fd: %d\n", aBitrate, mbPortFd);
        tcflush(mbPortFd, TCIFLUSH);
        ioctl(mbPortFd, TCGETS2, &tio2);
        tio2.c_cflag &= ~CBAUD;
        tio2.c_cflag |= BOTHER;
        tio2.c_ospeed = tio2.c_ispeed = aBitrate;
        ioctl(mbPortFd, TCSETS2, &tio2);
    }

    if (VOption)
        printf("Modbus bit rate: %d @ Slave ID: 0x%x (%d)\n", theBitrate, deviceAddress, deviceAddress);

    // If TS-7670, set RS-485 enable logic to proper bit rate
    if (ts7670)
    {
        sprintf(cmdBuf, "tshwctl --485speed=%d", theBitrate);
        system(cmdBuf);
        if (VOption)
            printf("Setting bit rate windows with: %s\n", cmdBuf);
    }

    return aMbConnection;
}