#include "marvelmind_boxie_api.h"
#ifdef WIN32
#include <windows.h>
#else
#include <dlfcn.h>
#include <string.h>
#endif

#ifdef WIN32
HINSTANCE mmLibrary;
#else
void* mmLibrary;
#endif

typedef bool __attribute__((stdcall)) (*pt_mm_api_version)(void *pdata);
static pt_mm_api_version pmm_api_version= NULL;
bool mmAPIVersion(uint32_t *version) {
  if (pmm_api_version == NULL)
    return false;

  uint8_t buf[8];
  bool res= (*pmm_api_version)(&buf[0]);

  *version= *((uint32_t *) &buf[0]);

  return res;
}

typedef bool __attribute__((stdcall)) (*pt_mm_get_last_error)(void *pdata);
static pt_mm_get_last_error pmm_get_last_error= NULL;
bool mmGetLastError(uint32_t *error) {
  if (pmm_get_last_error == NULL)
    return false;

  uint8_t buf[8];
  bool res= (*pmm_get_last_error)(&buf[0]);

  *error= *((uint32_t *) &buf[0]);

  return res;
}

typedef bool __attribute__((stdcall)) (*pt_mm_open_port)(void);
static pt_mm_open_port pmm_open_port= NULL;
bool mmOpenPort() {
  if (pmm_open_port == NULL)
    return false;

  return (*pmm_open_port)();
}

//////

typedef bool __attribute__((stdcall)) (*pt_mm_open_port_by_name)(void *pdata);
static pt_mm_open_port_by_name pmm_open_port_by_name= NULL;
bool mmOpenPortByName(char *portName) {
  if (pmm_open_port_by_name == NULL)
    return false;

  uint8_t buf[255];
  uint8_t i;
  for(i=0;i<255;i++) {
    buf[i]= portName[i];
    if (buf[i] == 0)
        break;
  }

  return (*pmm_open_port_by_name)(&buf[0]);
}

//////

typedef void __attribute__((stdcall)) (*pt_mm_close_port)(void);
static pt_mm_close_port pmm_close_port= NULL;
void mmClosePort() {
  if (pmm_close_port == NULL)
    return;

  (*pmm_close_port)();
}

//////

typedef bool __attribute__((stdcall)) (*pt_mm_get_version_and_id)(uint8_t address, void *pdata);
static pt_mm_get_version_and_id pmm_get_version_and_id= NULL;
bool mmGetVersionAndId(uint8_t address, MarvelmindDeviceVersion *mmDevVersion) {
  if (pmm_get_version_and_id == NULL)
    return false;

  uint8_t buf[128];
  bool res= (*pmm_get_version_and_id)(address, &buf[0]);

  mmDevVersion->fwVerMajor= buf[0];
  mmDevVersion->fwVerMinor= buf[1];
  mmDevVersion->fwVerMinor2= buf[2];
  mmDevVersion->fwVerDeviceType= buf[3];
  mmDevVersion->fwOptions= buf[4];

  mmDevVersion->cpuId= *((uint32_t *) &buf[5]);

  return res;
}

//////

typedef bool __attribute__((stdcall)) (*pt_mm_get_devices_list)(void *pdata);
static pt_mm_get_devices_list pmm_get_devices_list= NULL;
bool mmGetDevicesList(MarvelmindDevicesList *mmDevices) {
  if (pmm_get_devices_list == NULL)
    return false;

  uint8_t buf[(MM_MAX_DEVICES_COUNT+1)*10];

  bool res= (*pmm_get_devices_list)(&buf[0]);

  if (res) {
    uint8_t i;
    MarvelmindDeviceInfo *devPtr;

    mmDevices->numDevices= buf[0];

    uint32_t ofs= 1;
    for(i=0;i<mmDevices->numDevices;i++) {
        devPtr= &mmDevices->devices[i];

        devPtr->address= buf[ofs+0];
        devPtr->isDuplicatedAddress= (bool) buf[ofs+1];
        devPtr->isSleeping= (bool) buf[ofs+2];

        devPtr->fwVerMajor= buf[ofs+3];
        devPtr->fwVerMinor= buf[ofs+4];
        devPtr->fwVerMinor2= buf[ofs+5];
        devPtr->fwVerDeviceType= buf[ofs+6];

        devPtr->fwOptions= buf[ofs+7];

        devPtr->flags= buf[ofs+8];

        ofs+= 9;
    }
  }

  return res;
}

//////

typedef bool __attribute__((stdcall)) (*pt_mm_get_last_locations) (void *pdata);
static pt_mm_get_last_locations pmm_get_last_locations= NULL;
bool mmGetLastLocations(MarvelmindLocationsPack *posPack) {
    if (pmm_get_last_locations == NULL)
        return false;

    uint8_t buf[512];
    uint8_t i;
    bool res= (*pmm_get_last_locations)((void *) &buf[0]);

    uint16_t ofs= 0;
    MarvelmindDeviceLocation *ppos;
    for(i=0;i<MM_LOCATIONS_PACK_SIZE;i++) {
        ppos= &posPack->pos[i];

        ppos->address= buf[ofs+0];
        ppos->headIndex= buf[ofs+1];

        ppos->x_mm= *((int32_t *) &buf[ofs+2]);
        ppos->y_mm= *((int32_t *) &buf[ofs+6]);
        ppos->z_mm= *((int32_t *) &buf[ofs+10]);

        ppos->statusFlags= buf[ofs+14];
        ppos->quality= buf[ofs+15];

        ppos->reserved[0]= buf[ofs+16];
        ppos->reserved[1]= buf[ofs+17];

        ofs+= 18;
    }

    posPack->lastDistUpdated= buf[ofs++];
    for(i=0;i<5;i++) {
        posPack->reserved[i]= buf[ofs++];
    }

    posPack->userPayloadSize= buf[ofs++];
    uint8_t n= posPack->userPayloadSize;
    if (n>MM_USER_PAYLOAD_BUF_SIZE) {
        n= MM_USER_PAYLOAD_BUF_SIZE;
    }
    for(i=0;i<n;i++) {
        posPack->userPayloadBuf[i]= buf[ofs++];
    }

    return res;
}

//////

typedef bool __attribute__((stdcall)) (*pt_mm_get_last_locations2) (void *pdata);
static pt_mm_get_last_locations2 pmm_get_last_locations2= NULL;
bool mmGetLastLocations2(MarvelmindLocationsPack2 *posPack) {
    if (pmm_get_last_locations2 == NULL)
        return false;

    uint8_t buf[512];
    uint8_t i;
    bool res= (*pmm_get_last_locations2)((void *) &buf[0]);

    uint16_t ofs= 0;
    MarvelmindDeviceLocation2 *ppos;
    for(i=0;i<MM_LOCATIONS_PACK_SIZE;i++) {
        ppos= &posPack->pos[i];

        ppos->address= buf[ofs+0];
        ppos->headIndex= buf[ofs+1];

        ppos->x_mm= *((int32_t *) &buf[ofs+2]);
        ppos->y_mm= *((int32_t *) &buf[ofs+6]);
        ppos->z_mm= *((int32_t *) &buf[ofs+10]);

        ppos->statusFlags= buf[ofs+14];
        ppos->quality= buf[ofs+15];

        ppos->reserved[0]= buf[ofs+16];
        ppos->reserved[1]= buf[ofs+17];

        ppos->angle= *((uint16_t *) &buf[ofs+18]);
        ppos->angleReady= ((ppos->angle&0x1000) == 0);

        ofs+= 20;
    }

    posPack->lastDistUpdated= buf[ofs++];
    for(i=0;i<5;i++) {
        posPack->reserved[i]= buf[ofs++];
    }

    posPack->userPayloadSize= buf[ofs++];
    uint8_t n= posPack->userPayloadSize;
    if (n>MM_USER_PAYLOAD_BUF_SIZE) {
        n= MM_USER_PAYLOAD_BUF_SIZE;
    }
    for(i=0;i<n;i++) {
        posPack->userPayloadBuf[i]= buf[ofs++];
    }

    return res;
}

//////

typedef bool __attribute__((stdcall)) (*pt_mm_robot_movement) (uint8_t address, void *pdata);
static pt_mm_robot_movement pmm_robot_movement= NULL;
bool mmRobotMove(uint8_t address, MarvelmindMoveSettings *moveCtrl) {
    if (pmm_robot_movement == NULL)
        return false;

    uint8_t buf[64];
    uint8_t i;

    buf[0]= moveCtrl->moveType;
    for(i=0;i<8;i++)
        buf[1+i]= moveCtrl->reserved[i];

    return (*pmm_robot_movement)(address, (void *) &buf[0]);
}

typedef bool __attribute__((stdcall)) (*pt_mm_robot_stop) (uint8_t address);
static pt_mm_robot_stop pmm_robot_stop= NULL;
bool mmRobotStop(uint8_t address) {
    if (pmm_robot_stop == NULL)
        return false;

    return (*pmm_robot_stop)(address);
}

typedef bool __attribute__((stdcall)) (*pt_mm_get_boxie_telemetry) (uint8_t address, void *pdata);
static pt_mm_get_boxie_telemetry pmm_get_boxie_telemetry= NULL;
bool mmGetBoxieTelemetry(uint8_t address, MarvelmindBoxieTelemetry *boxieTel) {
    if (pmm_get_boxie_telemetry == NULL)
        return false;

    uint8_t buf[128];

    bool res= (*pmm_get_boxie_telemetry)(address, (void *) &buf[0]);

    memcpy(boxieTel, &buf[0], sizeof(MarvelmindBoxieTelemetry));

    return res;
}

typedef bool __attribute__((stdcall)) (*pt_mm_get_boxie_lidars) (uint8_t address, void *pdata);
static pt_mm_get_boxie_lidars pmm_get_boxie_lidars= NULL;
bool mmGetBoxieLidars(uint8_t address, MarvelmindBoxieLidars *boxieLidars) {
    if (pmm_get_boxie_lidars == NULL)
        return false;

    uint8_t buf[128];

    bool res= (*pmm_get_boxie_lidars)(address, (void *) &buf[0]);

    memcpy(boxieLidars, &buf[0], sizeof(MarvelmindBoxieLidars));

    return res;
}

typedef bool __attribute__((stdcall)) (*pt_mm_get_boxie_location) (uint8_t address, void *pdata);
static pt_mm_get_boxie_location pmm_get_boxie_location= NULL;
bool mmGetBoxieLocation(uint8_t address, MarvelmindBoxieLocation *boxiePos) {
    if (pmm_get_boxie_location == NULL)
        return false;

    uint8_t buf[128];

    bool res= (*pmm_get_boxie_location)(address, (void *) &buf[0]);

    memcpy(boxiePos, &buf[0], sizeof(MarvelmindBoxieLocation));

    return res;
}

typedef bool __attribute__((stdcall)) (*pt_mm_set_robot_program_item) (uint8_t address, void *pdata);
static pt_mm_set_robot_program_item pmm_set_robot_program_item= NULL;
bool mmSetRobotProgramItem(uint8_t address, MarvelmindRobotProgramItem *progItem) {
    if (pmm_set_robot_program_item == NULL)
        return false;

    uint8_t buf[64];

    buf[0]= progItem->itemIndex;
    buf[1]= progItem->totalItemsNum;

    buf[2]= progItem->opCode;
    *((int16_t *) &buf[3])= progItem->param1;
    *((int16_t *) &buf[5])= progItem->param2;
    *((int16_t *) &buf[7])= progItem->param3;

    return (*pmm_set_robot_program_item)(address, (void *) &buf[0]);
}

typedef bool __attribute__((stdcall)) (*pt_mm_set_robot_command) (uint8_t address, void *pdata);
static pt_mm_set_robot_command pmm_set_robot_command= NULL;
bool mmSetRobotCommand(uint8_t address, MarvelmindRobotCommand *robotCommand) {
    if (pmm_set_robot_command == NULL)
        return false;

    uint8_t buf[64];

    buf[0]= robotCommand->commandId;

    *((int16_t *) &buf[1])= robotCommand->param1;
    *((int16_t *) &buf[3])= robotCommand->param2;
    *((int16_t *) &buf[5])= robotCommand->param3;

    return (*pmm_set_robot_command)(address, (void *) &buf[0]);
}

//////

typedef bool __attribute__((stdcall)) (*pt_mm_device_is_modem)(uint8_t deviceType);
static pt_mm_device_is_modem pmm_device_is_modem= NULL;
bool mmDeviceIsModem(uint8_t deviceType) {
  if (pmm_device_is_modem == NULL)
    return false;

  return (*pmm_device_is_modem)(deviceType);
}

typedef bool __attribute__((stdcall)) (*pt_mm_device_is_beacon)(uint8_t deviceType);
static pt_mm_device_is_beacon pmm_device_is_beacon= NULL;
bool mmDeviceIsBeacon(uint8_t deviceType) {
  if (pmm_device_is_beacon == NULL)
    return false;

  return (*pmm_device_is_beacon)(deviceType);
}

typedef bool __attribute__((stdcall)) (*pt_mm_device_is_hedgehog) (uint8_t deviceType);
static pt_mm_device_is_hedgehog pmm_device_is_hedgehog= NULL;
bool mmDeviceIsHedgehog(uint8_t deviceType) {
  if (pmm_device_is_hedgehog == NULL)
    return false;

  return (*pmm_device_is_hedgehog)(deviceType);
}

typedef bool __attribute__((stdcall)) (*pt_mm_device_is_robot) (uint8_t deviceType);
static pt_mm_device_is_robot pmm_device_is_robot= NULL;
bool mmDeviceIsRobot(uint8_t deviceType) {
  if (pmm_device_is_robot == NULL)
    return false;

  return (*pmm_device_is_robot)(deviceType);
}

//////

void marvelmindAPILoad() {
#ifdef WIN32
  mmLibrary= LoadLibrary("dashapi.dll");

  pmm_api_version= (pt_mm_api_version ) GetProcAddress(mmLibrary, "mm_api_version");
  pmm_get_last_error= (pt_mm_get_last_error ) GetProcAddress(mmLibrary, "mm_get_last_error");

  pmm_open_port= (pt_mm_open_port ) GetProcAddress(mmLibrary, "mm_open_port");
  pmm_open_port_by_name= (pt_mm_open_port_by_name ) GetProcAddress(mmLibrary, "mm_open_port_by_name");
  pmm_close_port= (pt_mm_close_port ) GetProcAddress(mmLibrary, "mm_close_port");

  pmm_get_version_and_id= (pt_mm_get_version_and_id ) GetProcAddress(mmLibrary, "mm_get_device_version_and_id");
  pmm_get_devices_list= (pt_mm_get_devices_list ) GetProcAddress(mmLibrary, "mm_get_devices_list");

  pmm_get_last_locations= (pt_mm_get_last_locations ) GetProcAddress(mmLibrary, "mm_get_last_locations");
  pmm_get_last_locations2= (pt_mm_get_last_locations2 ) GetProcAddress(mmLibrary, "mm_get_last_locations2");

  pmm_robot_movement= (pt_mm_robot_movement ) GetProcAddress(mmLibrary, "mm_robot_movement");
  pmm_robot_stop= (pt_mm_robot_stop ) GetProcAddress(mmLibrary, "mm_robot_stop");
  pmm_get_boxie_telemetry= (pt_mm_get_boxie_telemetry ) GetProcAddress(mmLibrary, "mm_get_boxie_telemetry");
  pmm_get_boxie_lidars= (pt_mm_get_boxie_lidars ) GetProcAddress(mmLibrary, "mm_get_boxie_lidars");
  pmm_get_boxie_location= (pt_mm_get_boxie_location ) GetProcAddress(mmLibrary, "mm_get_boxie_location");
  pmm_set_robot_program_item= (pt_mm_set_robot_program_item ) GetProcAddress(mmLibrary, "mm_set_robot_program_item");
  pmm_set_robot_command= (pt_mm_set_robot_command ) GetProcAddress(mmLibrary, "mm_set_robot_command");

  pmm_device_is_modem= (pt_mm_device_is_modem ) GetProcAddress(mmLibrary, "mm_device_is_modem");
  pmm_device_is_beacon= (pt_mm_device_is_beacon ) GetProcAddress(mmLibrary, "mm_device_is_beacon");
  pmm_device_is_hedgehog= (pt_mm_device_is_hedgehog ) GetProcAddress(mmLibrary, "mm_device_is_hedgehog");
  pmm_device_is_robot= (pt_mm_device_is_robot ) GetProcAddress(mmLibrary, "mm_device_is_robot");
#else
// not WIN32
  mmLibrary = dlopen("libdashapi.so", RTLD_LAZY);

  pmm_api_version= dlsym(mmLibrary, "mm_api_version");
  pmm_get_last_error= dlsym(mmLibrary, "mm_get_last_error");

  pmm_open_port= dlsym(mmLibrary, "mm_open_port");
  pmm_open_port_by_name= dlsym(mmLibrary, "mm_open_port_by_name");
  pmm_close_port= dlsym(mmLibrary, "mm_close_port");

  pmm_get_version_and_id= dlsym(mmLibrary, "mm_get_device_version_and_id");
  pmm_get_devices_list= dlsym(mmLibrary, "mm_get_devices_list");

  pmm_get_last_locations= dlsym(mmLibrary, "mm_get_last_locations");
  pmm_get_last_locations2= dlsym(mmLibrary, "mm_get_last_locations2");

  pmm_robot_movement= dlsym(mmLibrary,  "mm_robot_movement");
  pmm_robot_stop= dlsym(mmLibrary,  "mm_robot_stop");
  pmm_get_boxie_telemetry= dlsym(mmLibrary, "mm_get_boxie_telemetry");
  pmm_get_boxie_lidars= dlsym(mmLibrary, "mm_get_boxie_lidars");
  pmm_get_boxie_location=  dlsym(mmLibrary, "mm_get_boxie_location");
  pmm_set_robot_program_item= dlsym(mmLibrary, "mm_set_robot_program_item");
  pmm_set_robot_command= dlsym(mmLibrary, "mm_set_robot_command");

  pmm_device_is_modem= dlsym(mmLibrary, "mm_device_is_modem");
  pmm_device_is_beacon= dlsym(mmLibrary, "mm_device_is_beacon");
  pmm_device_is_hedgehog= dlsym(mmLibrary, "mm_device_is_hedgehog");
  pmm_device_is_robot= dlsym(mmLibrary, "mm_device_is_robot");
#endif
}

void marvelmindAPIFree() {
#ifdef WIN32
  FreeLibrary(mmLibrary);
#else
  dlclose(mmLibrary);
#endif
}
