#ifndef MODBUS_H
#define MODBUS_H
#pragma once 
#include <ros/ros.h>
#include <cstring>
#include <stdint.h>
#include <iostream>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>


#define MAX_MSG_LENGTH 260

///Function Code
#define     READ_COILS        0x01
#define     READ_INPUT_BITS   0x02
#define     READ_REGS         0x03
#define     READ_INPUT_REGS   0x04
#define     WRITE_COIL        0x05
#define     WRITE_REG         0x06
#define     WRITE_COILS       0x0F
#define     WRITE_REGS        0x10

///Exception Codes

#define    EX_ILLEGAL_FUNCTION  0x01 // Function Code not Supported
#define    EX_ILLEGAL_ADDRESS   0x02 // Output Address not exists
#define    EX_ILLEGAL_VALUE     0x03 // Output Value not in Range
#define    EX_SERVER_FAILURE    0x04 // Slave Deive Fails to process request
#define    EX_ACKNOWLEDGE       0x05 // Service Need Long Time to Execute
#define    EX_SERVER_BUSY       0x06 // Server Was Unable to Accept MB Request PDU
#define    EX_NEGATIVE_ACK      0x07
#define    EX_MEM_PARITY_PROB   0x08
#define    EX_GATEWAY_PROBLEMP  0x0A // Gateway Path not Available
#define    EX_GATEWYA_PROBLEMF  0x0B // Target Device Failed to Response
#define    EX_BAD_DATA          0XFF // Bad Data lenght or Address

#define    BAD_CON              -1

/**
 * @class Modbus Operator Class
 * @brief Providing networking support and mobus operation support.
 */
class modbus {
public:
  /**
  * Main Constructor of Modbus Connector Object
  * @param host IP Address of Host
  * @param port Port for the TCP Connection
  * @return     A Modbus Connector Object
  */
  modbus(std::string ip_address, int port);

  /**
   * Destructor of Modbus Connector Object
   */
  virtual ~modbus();

  /**
   * Modbus Slave ID Setter
   * @param id  ID of the Modbus Server Slave
   */
  void modbus_set_slave_id(int id);

  /**
   * @brief Build up a Modbus/TCP Connection
   * @return   If A Connection Is Successfully Built
   */
  bool modbus_connect();

  /**
   * @brief Close the Modbus/TCP Connection
   */
  void modbus_close() const;

  /**
   * @brief Read Coil
   *        MODBUS FUNCTION 0x01
   * @param address     Reference Address
   * @param buffer      Buffer to Store Data Read from Coils
   */
  int  modbus_read_coil(int address, bool &buffer);

  /**
   * @brief Read Coils
   *        MODBUS FUNCTION 0x01
   * @param address     Reference Address
   * @param amount      Amount of Coils to Read
   * @param buffer      Buffer to Store Data Read from Coils
   */
  int  modbus_read_coils(int address, int amount, bool* buffer);

  /**
   * @brief Read Input Bits(Discrete Data)
   *        MODBUS FUNCITON 0x02
   * @param address   Reference Address
   * @param buffer    Buffer to store Data Read from Input Bits
   */
  int  modbus_read_input_bit(int address, bool &buffer);

  /**
   * @brief Read Input Bits(Discrete Data)
   *        MODBUS FUNCITON 0x02
   * @param address   Reference Address
   * @param amount    Amount of Bits to Read
   * @param buffer    Buffer to store Data Read from Input Bits
   */
  int  modbus_read_input_bits(int address, int amount, bool* buffer);

  /**
   * @brief Read Holding Register 
   *        MODBUS FUNCTION 0x03
   * @param address    Reference Address
   * @param buffer     Buffer to Store Data Read from Registers
   */
  int  modbus_read_holding_register(int address, uint16_t &buffer);

  /**
   * @brief Read Holding Registers
   *        MODBUS FUNCTION 0x03
   * @param address    Reference Address
   * @param amount     Amount of Registers to Read
   * @param buffer     Buffer to Store Data Read from Registers
   */
  int  modbus_read_holding_registers(int address, int amount, uint16_t *buffer);

  /**
   * @brief Read Input Registers
   *        MODBUS FUNCTION 0x04
   * @param address     Reference Address
   * @param amount      Amount of Registers to Read
   * @param buffer      Buffer to Store Data Read from Registers
   */
  int  modbus_read_input_registers(int address, int amount, uint16_t *buffer);


  /**
   * @brief  Write Single Coils
   *         MODBUS FUNCTION 0x05
   * @param address    Reference Address
   * @param to_write   Value to be Written to Coil
   */
  int  modbus_write_coil(int address, const bool& to_write);

  /**
   * @brief Write Single Register
   *        FUCTION 0x06
   * @param address   Reference Address
   * @param value     Value to Be Written to Register
   */
  int  modbus_write_register(int address, const uint16_t& value);

  /**
   * @brief Write Multiple Coils
   *        MODBUS FUNCTION 0x0F
   * @param address  Reference Address
   * @param amount   Amount of Coils to Write
   * @param value    Values to Be Written to Coils
   */
  int  modbus_write_coils(int address, int amount, const bool *value);

  /**
   * @brief Write Multiple Registers
   *        MODBUS FUNCION 0x10
   * @param address Reference Address
   * @param amount  Amount of Value to Write
   * @param value   Values to Be Written to the Registers
   */
  int  modbus_write_registers(int address, int amount, const uint16_t *value); 

  bool _connected{};
  bool err{};
  int err_no{};
  std::string error_msg;
  struct timeval timeout{};

private:

  /**
   * @brief Modbus Request Builder
   * @param to_send   Message Buffer to Be Sent
   * @param address   Reference Address
   * @param func      Modbus Functional Code
   */
  void modbus_build_request(uint8_t *to_send, uint address, int func) const;

  /**
   * @brief Read Request Builder and Sender
   * @param address   Reference Address
   * @param amount    Amount of Data to Read
   * @param func      Modbus Functional Code
   */
  int modbus_read(int address, uint amount, int func);

  /**
   * @brief Write Request Builder and Sender
   * @param address   Reference Address
   * @param amount    Amount of data to be Written
   * @param func      Modbus Functional Code
   * @param value     Data to Be Written
   */
  int modbus_write(int address, uint amount, int func, const uint16_t *value);

  /**
   * @brief Data Sender
   * @param to_send Request to Be Sent to Server
   * @param length  Length of the Request
   * @return        Size of the request
   */
  ssize_t modbus_send(uint8_t *to_send, int length);

  /**
   * @brief Data Receiver
   * @param buffer Buffer to Store the Data Retrieved
   * @return       Size of Incoming Data
   */
  ssize_t modbus_receive(uint8_t *buffer) const;

  /**
   * @brief Error Code Handler
   * @param msg   Message Received from the Server
   * @param func  Modbus Functional Code
   */
  void modbuserror_handle(const uint8_t *msg, int func);

  void set_bad_con();
  void set_bad_input();

// Properties
  int _socket{};
  uint _msg_id{};
  int _slaveid{};
  uint16_t PORT{};
  std::string HOST;
  struct sockaddr_in _server{};
};

#endif //MODBUS_H
