#include <stdio.h>
#include <stdlib.h>
#include <modbus/modbus.h>
#include <errno.h>

#define SLAVE_ID 1
#define ADDR_COILS 0
#define ADDR_DISCRETE_INPUTS 0
#define ADDR_HOLDING_REGISTERS 0
#define ADDR_INPUT_REGISTERS 0

int main(){
    modbus_t *ctx;
    int rc;

    ctx = modbus_new_tcp("169.254.124.165", 5020);

    if (ctx == NULL){
        fprintf(stderr, "Unable to allocate libmodbus context\n");
        return -1;
    }

    if (modbus_connect(ctx) == -1){
        fprintf(stderr, "Connection failed: %s\n", modbus_strerror(errno));
        modbus_free(ctx);
        return -1;
    }

    modbus_set_slave(ctx, SLAVE_ID);
    
    // FC1: Read Coil Status
    uint8_t dest_bits[32];

    rc = modbus_read_bits(ctx, ADDR_COILS, 5, dest_bits);

    if (rc >= 0){
        printf("Successfully FC1 read coils: ");

        for (int i = 0; i < 5; i++)
            printf("%d ", dest_bits[i]);

        printf("\n");
    }
    
    else{
        fprintf(stderr, "Failed FC1 to read coils: %s\n", modbus_strerror(errno));
    }

    // FC2: Read Input Status
    uint8_t dest_input_bits[32];
    
    rc = modbus_read_input_bits(ctx, ADDR_DISCRETE_INPUTS, 7, dest_input_bits);

    if (rc >= 0){
        printf("Successfully FC2 read: ");

        for (int i = 0; i < 7; i++)
            printf("%d ", dest_input_bits[i]);

        printf("\n");
    }
    
    else{
        fprintf(stderr, "Failed FC2 to read: %s\n", modbus_strerror(errno));
    }

    // FC3: Read Holding Registers
    uint16_t dest_holding_registers[32];
    
    rc = modbus_read_registers(ctx, ADDR_HOLDING_REGISTERS, 3, dest_holding_registers);

    if (rc >= 0){
        printf("Successfully FC3 read: ");

        for (int i = 0; i < 3; i++)
            printf("%d ", dest_holding_registers[i]);

        printf("\n");
    }
    
    else{
        fprintf(stderr, "Failed FC3 to read: %s\n", modbus_strerror(errno));
    }

    // FC4: Read Input Registers
    uint16_t dest_input_registers[32];

    rc = modbus_read_input_registers(ctx, ADDR_INPUT_REGISTERS, 6, dest_input_registers);

    if (rc >= 0){
        printf("Successfully FC4 read: ");

        for (int i = 0; i < 6; i++)
            printf("%d ", dest_input_registers[i]);

        printf("\n");
    }
    
    else{
        fprintf(stderr, "Failed FC4 to read: %s\n", modbus_strerror(errno));
    }

    // FC5: Write Single Coil
    rc = modbus_write_bit(ctx, 5, 1);

    if (rc >= 0){
        printf("Successfully FC5 wrote value 1 to coil address 5\n");
    }
    
    else{
        fprintf(stderr, "Failed FC5 to write: %s\n", modbus_strerror(errno));
    }

    rc = modbus_write_bit(ctx, 5, 0);

    if (rc >= 0){
        printf("Successfully FC5 wrote value 0 to coil address 5\n");
    }
    
    else{
        fprintf(stderr, "Failed FC5 to write: %s\n", modbus_strerror(errno));
    }

    // FC6: Write Single Register
    rc = modbus_write_register(ctx, 2, 200);

    if (rc >= 0){
        printf("Successfully FC6 wrote value 200 to register address 2\n");
    }
    
    else{
        fprintf(stderr, "Failed FC6 to write: %s\n", modbus_strerror(errno));
    }

    rc = modbus_write_register(ctx, 2, 0);

    if (rc >= 0){
        printf("Successfully FC6 wrote value 0 to register address 2\n");
    }
    
    else{
        fprintf(stderr, "Failed FC6 to write: %s\n", modbus_strerror(errno));
    }

    // FC15: Write Multiple
    uint8_t tab_bits[5] = {1, 1, 1, 1, 1};
    rc = modbus_write_bits(ctx, 0, 5, tab_bits);

    if (rc >= 0){
        printf("Successfully FC15 wrote value 1 to coil address 0-4\n");
    }
    
    else{
        fprintf(stderr, "Failed FC15 to write: %s\n", modbus_strerror(errno));
    }

    uint8_t tab_bits_off[5] = {0, 0, 0, 0, 0};
    rc = modbus_write_bits(ctx, 0, 5, tab_bits_off);

    if (rc >= 0){
        printf("Successfully FC15 wrote value 0 to coil address 0-4\n");
    }
    
    else{
        fprintf(stderr, "Failed FC15 to write: %s\n", modbus_strerror(errno));
    }

    // FC16: Write Multiple Registers
    uint16_t tab_registers[3] = {255, 500, 300};
    rc = modbus_write_registers(ctx, 0, 3, tab_registers);

    if (rc >= 0){
        printf("Successfully FC16 wrote values to register address 0-2\n");
    }

    else{
        fprintf(stderr, "Failed FC16 to write: %s\n", modbus_strerror(errno));
    }

    uint16_t tab_registers_off[3] = {0, 0, 0};
    rc = modbus_write_registers(ctx, 0, 3, tab_registers_off);

    if (rc >= 0){
        printf("Successfully FC16 wrote values 0 to register address 0-2\n");
    }
    
    else{
        fprintf(stderr, "Failed FC16 to write: %s\n", modbus_strerror(errno));
    }

    modbus_close(ctx);
    modbus_free(ctx);

    return 0;
}