/**
 * \file
 * \brief Module to identify different devices based on their unique ID.
 * \author Erich Styger, erich.styger@hslu.ch
 * \license SPDX-License-Identifier: BSD-3-Clause
 * With this module individual devices are identified based on their unique ID.
 */

#include "platform.h"
#if PL_CONFIG_USE_IDENTIFY
#include "Identify.h"
#include "McuArmTools.h"
#include "McuUtility.h"

static ID_Robot_e currID = ID_ROBOT_NONE;

typedef struct {
  ID_Robot_e id;
  const unsigned char *name;
  McuArmTools_UID uuid; /* UID of K22 */
} ID_RobotDevice;

static const ID_RobotDevice idTable[] =
{ /*! \todo ADIS: Verify and update information */
  {.id=ID_ROBOT_E0,  .name=(const unsigned char*)"Robot E0",  .uuid={ {0x00,0x00,0x34,0x00,0x41,0xB5,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_E1,  .name=(const unsigned char*)"Robot E1",  .uuid={ {0x00,0x00,0x38,0x00,0x41,0xB5,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_E2,  .name=(const unsigned char*)"Robot E2",  .uuid={ {0x00,0x00,0x33,0x00,0x51,0xB4,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_E3,  .name=(const unsigned char*)"Robot E3",  .uuid={ {0x00,0x00,0x13,0x00,0x11,0xB9,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_E4,  .name=(const unsigned char*)"Robot E4",  .uuid={ {0x00,0x00,0x34,0x00,0x21,0xB7,0xCD,0x67,0x15,0x32,0x45,0x4E,0x14,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_E5,  .name=(const unsigned char*)"Robot E5",  .uuid={ {0x00,0x00,0x00,0x00,0x41,0xB5,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_E6,  .name=(const unsigned char*)"Robot E6",  .uuid={ {0xFF,0xFF,0x10,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x25,0x00,0x02,0x10} }},
  {.id=ID_ROBOT_E7,  .name=(const unsigned char*)"Robot E7",  .uuid={ {0x00,0x00,0x37,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x14,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_E8,  .name=(const unsigned char*)"Robot E8",  .uuid={ {0x00,0x00,0x19,0x00,0x11,0xB9,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_E9,  .name=(const unsigned char*)"Robot E9",  .uuid={ {0xFF,0xFF,0x0B,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x24,0x00,0x02,0x10} }},
  {.id=ID_ROBOT_E10, .name=(const unsigned char*)"Robot E10", .uuid={ {0xFF,0xFF,0x20,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x07,0x00,0x02,0x10} }},
  {.id=ID_ROBOT_E11, .name=(const unsigned char*)"Robot E11", .uuid={ {0x00,0x00,0x1F,0x00,0x11,0xB9,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_E12, .name=(const unsigned char*)"Robot E12", .uuid={ {0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x25,0x00,0x02,0x10} }},
  {.id=ID_ROBOT_E13, .name=(const unsigned char*)"Robot E13", .uuid={ {0x00,0x00,0x0D,0x00,0x21,0xB7,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_E14, .name=(const unsigned char*)"Robot E14", .uuid={ {0x00,0x00,0x0C,0x00,0x21,0xB7,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},

  {.id=ID_ROBOT_E17, .name=(const unsigned char*)"Robot E17", .uuid={ {0x00,0x00,0x2C,0x00,0x51,0xB4,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_E18, .name=(const unsigned char*)"Robot E18", .uuid={ {0xFF,0xFF,0x1E,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x07,0x00,0x02,0x10} }},
  {.id=ID_ROBOT_E27, .name=(const unsigned char*)"Robot E27", .uuid={ {0x00,0x00,0x36,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},

  {.id=ID_ROBOT_L0,  .name=(const unsigned char*)"Robot L0",  .uuid={ {0x00,0x00,0x1B,0x00,0x01,0xBA,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_L1,  .name=(const unsigned char*)"Robot L1",  .uuid={ {0xFF,0xFF,0x19,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x25,0x00,0x02,0x10} }},
  {.id=ID_ROBOT_L3,  .name=(const unsigned char*)"Robot L3",  .uuid={ {0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x0A,0x00,0x02,0x10} }},
  {.id=ID_ROBOT_L17, .name=(const unsigned char*)"Robot L17", .uuid={ {0x00,0x00,0x2F,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_L20, .name=(const unsigned char*)"Robot L20", .uuid={ {0x00,0x00,0x0A,0x00,0x21,0xB8,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},

  {.id=ID_ROBOT_R0,  .name=(const unsigned char*)"Robot R8",  .uuid={ {0x00,0x00,0x03,0x00,0x21,0xB7,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_R8,  .name=(const unsigned char*)"Robot R8",  .uuid={ {0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x25,0x00,0x02,0x10} }},
  {.id=ID_ROBOT_R9,  .name=(const unsigned char*)"Robot R9",  .uuid={ {0xFF,0xFF,0x03,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x24,0x00,0x02,0x10} }},
  {.id=ID_ROBOT_R23, .name=(const unsigned char*)"Robot R23", .uuid={ {0x00,0x00,0x29,0x00,0x51,0xB4,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_R27, .name=(const unsigned char*)"Robot R27", .uuid={ {0x00,0x00,0x30,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_R28, .name=(const unsigned char*)"Robot R28", .uuid={ {0x00,0x00,0x39,0x00,0x21,0xB8,0xCD,0x67,0x15,0x32,0x45,0x4E,0x14,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_R29, .name=(const unsigned char*)"Robot R29", .uuid={ {0x00,0x00,0x00,0x00,0x01,0xBA,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_R32, .name=(const unsigned char*)"Robot R32", .uuid={ {0x00,0x00,0x00,0x00,0x11,0xB9,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_R33, .name=(const unsigned char*)"Robot R33", .uuid={ {0x00,0x00,0x37,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_R34, .name=(const unsigned char*)"Robot R34", .uuid={ {0x00,0x00,0x35,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x14,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_R36, .name=(const unsigned char*)"Robot R36", .uuid={ {0x00,0x00,0x3A,0x00,0x41,0xB5,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_R37, .name=(const unsigned char*)"Robot R37", .uuid={ {0x00,0x00,0x1C,0x00,0xF1,0xBA,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_R44, .name=(const unsigned char*)"Robot R44", .uuid={ {0x00,0x00,0x2F,0x00,0x41,0x6A,0xD2,0x67,0x15,0x32,0x45,0x4E,0x32,0x00,0x02,0x30} }},
  {.id=ID_ROBOT_R45, .name=(const unsigned char*)"Robot R45", .uuid={ {0x00,0x00,0x12,0x00,0x11,0xB9,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
};

static ID_Robot_e IdentifyDevice(void) {
  uint8_t res;
  McuArmTools_UID id;
  unsigned int i;

  res = McuArmTools_UIDGet(&id);
  if (res==ERR_OK) {
    for(i=0; i<sizeof(idTable)/sizeof(idTable[0]); i++) {
      if (McuArmTools_UIDSame(&id, &idTable[i].uuid)) {
        return idTable[i].id; /* found it */
      }
    }
  }
  return ID_ROBOT_UNKNOWN;
}

static const ID_RobotDevice *GetDeviceDesc(ID_Robot_e id) {
  unsigned int i;

  for(i=0; i<sizeof(idTable)/sizeof(idTable[0]); i++) {
    if (idTable[i].id == id) {
      return &idTable[i]; /* found it */
    }
  }
  return NULL;
}

ID_Robot_e ID_WhichDevice(void) {
  if (currID==ID_ROBOT_NONE) {
    /* not checked ID, try to find matching robot */
    currID = IdentifyDevice();
  }
  return currID;
}

static uint8_t PrintStatus(const McuShell_StdIOType *io) {
  uint8_t buf[32];
  const ID_RobotDevice *device;

  McuShell_SendStatusStr((unsigned char*)"id", (unsigned char*)"Identify status\r\n", io->stdOut);
  device = GetDeviceDesc(ID_WhichDevice());
  if (device!=NULL) {
    McuUtility_strcpy(buf, sizeof(buf), device->name);
    McuUtility_strcat(buf, sizeof(buf), (uint8_t*)"\r\n");
    McuShell_SendStatusStr((unsigned char*)"  device", buf, io->stdOut);
  } else {
    McuShell_SendStatusStr((unsigned char*)"  device", (uint8_t*)"UNKNOWN\r\n", io->stdOut);
  }
  return ERR_OK;
}

uint8_t ID_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
  if (McuUtility_strcmp((char*)cmd, McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, "id help")==0) {
    McuShell_SendHelpStr((unsigned char*)"id", (const unsigned char*)"Group of id commands\r\n", io->stdOut);
    McuShell_SendHelpStr((unsigned char*)"  help|status", (const unsigned char*)"Print help or status information\r\n", io->stdOut);
    *handled = TRUE;
    return ERR_OK;
  } else if ((McuUtility_strcmp((char*)cmd, McuShell_CMD_STATUS)==0) || (McuUtility_strcmp((char*)cmd, "id status")==0)) {
    *handled = TRUE;
    return PrintStatus(io);
  }
  return ERR_OK;
}

void ID_Deinit(void) {
  currID = ID_ROBOT_NONE;
}

void ID_Init(void) {
  currID = ID_ROBOT_NONE;
}
#endif /* PL_CONFIG_USE_IDENTIFY */