
#ifndef ___PLC_MOTION__HH
#define ___PLC_MOTION__HH
#include <vector>
#include <time.h>
#include "modbus.h"

#ifndef _MSC_VER
#include <unistd.h>
#endif

#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <iostream>
#include <mutex>

namespace plc_modbus
{

#define ADDRESS_LENGTH 100
#define ADDRESS_START 0
#define POSE_ADDRESS_START (30 + ADDRESS_START)
#define POSE_COUNT 10
#define UPDATE_FLAG_ADDRESS (0 + ADDRESS_START)
#define CURRENT_STAGE_ADDRESS (1 + ADDRESS_START)
#define FINISH_TRAJECTORY_ADDRESS (2 + ADDRESS_START)

#define ADDRESS_LEFT_LINEAR_CURRENT 16
#define ADDRESS_RIGHT_LINEAR_CURRENT 17
#define ADDRESS_LEFT_LINEAR_TARGET 30
#define ADDRESS_RIGHT_LINEAR_TARGET 20

class CPLCModbus
{
public:
  CPLCModbus(void *pOwnerObject = NULL);
  ~CPLCModbus();

  bool connect(const char *ip, int port);
  void disconnect();
  bool reconnect();
  bool set_slave(int slave);
  bool update_trajectory(uint16_t update_flag, uint16_t *dest);
  uint16_t get_current_stage();
  uint16_t get_finish_status();
  bool read_registers(int addr, int nb, uint16_t *dest);
  bool write_registers(int addr, int nb, uint16_t *dest);

  bool read_register(int addr, uint16_t *dest);
  bool write_register(int addr, uint16_t *dest);

  modbus_t *ctx;
  const char *ip;
  int port;

  std::mutex    mutex_;

private:
    };
};
#endif