/*
   * Copyright (C) 2017 Inria
   *
   * This file is subject to the terms and conditions of the GNU Lesser General
   * Public License v2.1. See the file LICENSE in the top level directory for more
   * details.
   */
  
  #include "feetech.h"
  #include "shell.h"
  #include "shell_commands.h"
  #include "uart_stdio.h"
  
  #include <stdio.h>
  #include <string.h>
  
  #define ARRAY_LEN(array) (sizeof(array)/sizeof(array[0]))
  
  typedef struct {
      const char *name;
      int addr;
  } reg_name_addr_t;
  
  static const reg_name_addr_t regs8[] = {
      { "ID", SCS15_ID },
      { "BAUD_RATE", SCS15_BAUD_RATE },
      { "RETURN_DELAY_TIME", SCS15_RETURN_DELAY_TIME },
      { "RETURN_LEVEL", SCS15_RETURN_LEVEL },
      { "LIMIT_TEMPERATURE", SCS15_LIMIT_TEMPERATURE },
      { "MAX_LIMIT_VOLTAGE", SCS15_MAX_LIMIT_VOLTAGE },
      { "MIN_LIMIT_VOLTAGE", SCS15_MIN_LIMIT_VOLTAGE },
      { "ALARM_LED", SCS15_ALARM_LED },
      { "ALARM_SHUTDOWN", SCS15_ALARM_SHUTDOWN },
      { "COMPLIANCE_P", SCS15_COMPLIANCE_P },
      { "COMPLIANCE_D", SCS15_COMPLIANCE_D },
      { "COMPLIANCE_I", SCS15_COMPLIANCE_I },
      { "CW_DEAD", SCS15_CW_DEAD },
      { "CCW_DEAD", SCS15_CCW_DEAD },
      { "TORQUE_ENABLE", SCS15_TORQUE_ENABLE },
      { "LED", SCS15_LED },
      { "LOCK", SCS15_LOCK },
      { "PRESENT_VOLTAGE", SCS15_PRESENT_VOLTAGE },
      { "PRESENT_TEMPERATURE", SCS15_PRESENT_TEMPERATURE },
      { "REGISTERED_INSTRUCTION", SCS15_REGISTERED_INSTRUCTION },
      { "ERROR", SCS15_ERROR },
      { "MOVING", SCS15_MOVING },
  };
  
  static const reg_name_addr_t regs16[] = {
      { "MODEL_NUMBER", SCS15_MODEL_NUMBER },
      { "VERSION", SCS15_VERSION },
      { "MIN_ANGLE_LIMIT", SCS15_MIN_ANGLE_LIMIT },
      { "MAX_ANGLE_LIMIT", SCS15_MAX_ANGLE_LIMIT },
      { "MAX_TORQUE", SCS15_MAX_TORQUE },
      { "PUNCH", SCS15_PUNCH },
      { "IMAX", SCS15_IMAX },
      { "OFFSET", SCS15_OFFSET },
      { "GOAL_POSITION", SCS15_GOAL_POSITION },
      { "GOAL_TIME", SCS15_GOAL_TIME },
      { "GOAL_SPEED", SCS15_GOAL_SPEED },
      { "PRESENT_POSITION", SCS15_PRESENT_POSITION },
      { "PRESENT_SPEED", SCS15_PRESENT_SPEED },
      { "PRESENT_LOAD", SCS15_PRESENT_LOAD },
      { "VIR_POSITION", SCS15_VIR_POSITION },
      { "CURRENT", SCS15_CURRENT },
  };
  
  static const int32_t baudrates[] = {
      1000000L,
      500000L,
      250000L,
      128000L,
      115200L,
      76800L,
      57600L,
      38400L,
  };
  
  static uint8_t feetech_buffer[128];
  static uart_half_duplex_t stream;
  
  static int parse_uart(char *arg)
  {
      unsigned uart = atoi(arg);
      if (uart >= UART_NUMOF) {
          printf("Error: Invalid UART_DEV device specified (%u).\n", uart);
          return -1;
      }
      else if (UART_DEV(uart) == UART_STDIO_DEV) {
          printf("Error: The selected UART_DEV(%u) is used for the shell!\n", uart);
          return -2;
      }
      return uart;
  }
  
  static int32_t parse_baud(char *arg)
  {
      int32_t baud = atoi(arg);
  
      for (size_t i = 0 ; i < ARRAY_LEN(baudrates) ; i++) {
          if (baud == baudrates[i]) {
              return baud;
          }
      }
  
      printf("Error: Invalid baudrate (%s)\n", arg);
      return -1;
  }
  
  static int parse_dev(char *arg)
  {
      int dev = atoi(arg);
      if (dev < 0 || 254 < dev) {
          printf("Error: Invalid device id (%s)\n", arg);
          return -1;
      }
      return dev;
  }
  
  static void parse_reg(char *arg, int *reg8, int *reg16)
  {
      *reg8 = -1;
      *reg16 = -1;
  
      for (size_t i = 0 ; i < ARRAY_LEN(regs8) ; i++) {
          if (strcmp(arg, regs8[i].name) == 0) {
              *reg8 = regs8[i].addr;
              return;
          }
      }
  
      for (size_t i = 0 ; i < ARRAY_LEN(regs16) ; i++) {
          if (strcmp(arg, regs16[i].name) == 0) {
              *reg16 = regs16[i].addr;
              return;
          }
      }
  
      printf("Error: Invalid register (%s)\n", arg);
  }
  
  void print_registers(void) {
      puts("available 8bits registers :");
      for (size_t i = 0 ; i < ARRAY_LEN(regs8) ; i++) {
          printf("\t%s\n", regs8[i].name);
      }
  
      puts("available 16bits registers :");
      for (size_t i = 0 ; i < ARRAY_LEN(regs16) ; i++) {
          printf("\t%s\n", regs16[i].name);
      }
  }
  
  static int cmd_init(int argc, char **argv) {
      int uart = -1;
      int baud = -1;
      uint32_t timeout = -1;
  
      if (argc != 3 && argc != 4) {
          printf("usage; %s <uart> <baudrate> [<timeout_us>]\n", argv[0]);
          puts("available baudrates :");
          for (size_t i = 0 ; i < ARRAY_LEN(baudrates) ; i++) {
              printf("\t%ld\n", (long int)baudrates[i]);
          }
          return 1;
      }
      /* parse parameters */
      uart = parse_uart(argv[1]);
      if (uart < 0) {
          return -1;
      }
  
      baud = parse_baud(argv[2]);
      if (baud < 0) {
          return -1;
      }
  
      if (argc == 4) {
          timeout = (uint32_t)atol(argv[3]);
          if (timeout == 0) {
              printf("Error : Invalid timeout (%s)", argv[3]);
              return -1;
          }
      }
  
      /* init */
      uart_half_duplex_params_t params = {
          .uart = uart,
          .baudrate = baud,
          .dir = UART_HALF_DUPLEX_DIR_NONE,
      };
  
      int ret = uart_half_duplex_init(&stream, feetech_buffer, ARRAY_LEN(feetech_buffer), &params);
  
      if (argc == 4) {
          stream.timeout_us = timeout;
      }
  
      if (ret == UART_HALF_DUPLEX_NODEV) {
          puts("Error: invalid UART device given");
          return -1;
      }
      if (ret == UART_HALF_DUPLEX_NOBAUD) {
          puts("Error: given baudrate is not applicable");
          return -1;
      }
      if (ret == UART_HALF_DUPLEX_INTERR) {
          puts("Error: internal error");
          return -1;
      }
      if (ret == UART_HALF_DUPLEX_NOMODE) {
          puts("Error: given mode is not applicable");
          return -1;
      }
      if (ret == UART_HALF_DUPLEX_NOBUFF) {
          puts("Error: invalid buffer given");
          return -1;
      }
  
      printf("Successfully initialized Feetech TTL bus UART_DEV(%i)\n", uart);
      return 0;
  }
  
  static int cmd_ping(int argc, char **argv) {
      int id = -1;
  
      if (argc != 2) {
          printf("usage; %s <dev_id>\n", argv[0]);
          return 1;
      }
      /* parse parameters */
      id = parse_dev(argv[1]);
      if (id < 0) {
          return -1;
      }
  
      /* ping */
      if (feetech_ping(&stream, id) == FEETECH_OK) {
          printf("Device %i responded\n", id);
      }
      else {
          printf("No response from %i\n", id);
      }
      return 0;
  }
  
  static int cmd_scan(int argc, char **argv) {
      int min = -1;
      int max = -1;
  
      if (argc == 3) {
          min = atoi(argv[1]);
          max = atoi(argv[2]);
          if (min < 0) {
              return -1;
          }
          if (max > 254) {
              return -1;
          }
          if (max < min) {
              return -1;
          }
      }
      else if (argc == 1) {
          min = 0;
          max = 254;
      }
      else {
          printf("usage; %s [<min_id> <max_id>]\n", argv[0]);
          return 1;
      }
  
      /* ping */
      puts("Scanning...");
      for (int id = min ; id < max ; id++) {
          if (feetech_ping(&stream, id) == FEETECH_OK) {
              printf("Device %i available\n", id);
          }
      }
      puts("End");
      return 0;
  }
  
  static int cmd_read(int argc, char **argv) {
      int id = -1;
      int reg8 = -1;
      int reg16 = -1;
  
      if (argc != 3) {
          printf("usage; %s <dev_id> <reg>\n", argv[0]);
          print_registers();
          return 1;
      }
      /* parse parameters */
      id = parse_dev(argv[1]);
      if (id < 0) {
          return -1;
      }
  
      parse_reg(argv[2], &reg8, &reg16);
      if (reg8 < 0 && reg16 < 0) {
          return -1;
      }
  
      /* read */
      feetech_t dev;
      feetech_init(&dev, &stream, id);
      if (reg8 >= 0) {
          uint8_t val = 0;
          int ret = feetech_read8(&dev, reg8, &val);
          if (ret != FEETECH_OK) {
              printf("Error[%i] : No response from %i\n", ret, id);
              return -1;
          }
          printf("%i\n", (int)val);
      }
      else {
          uint16_t val = 0;
          int ret = feetech_read16(&dev, reg16, &val);
          if (ret != FEETECH_OK) {
              printf("Error[%i] : No response from %i\n", ret, id);
              return -1;
          }
          printf("%i\n", (int)val);
      }
      return 0;
  }
  
  static int cmd_write(int argc, char **argv) {
      int id = -1;
      int reg8 = -1;
      int reg16 = -1;
  
      if (argc != 4) {
          printf("usage; %s <dev_id> <reg> <value>\n", argv[0]);
          print_registers();
          return 1;
      }
      /* parse parameters */
      id = parse_dev(argv[1]);
      if (id < 0) {
          return -1;
      }
  
      parse_reg(argv[2], &reg8, &reg16);
      if (reg8 < 0 && reg16 < 0) {
          return -1;
      }
  
      int val = atoi(argv[3]);
      if (val < 0) {
          return -1;
      }
  
      /* read */
      feetech_t dev;
      feetech_init(&dev, &stream, id);
      if (reg8 >= 0) {
          int ret = feetech_write8(&dev, reg8, val);
          if (ret != FEETECH_OK) {
              printf("Error[%i] : No response from %i\n", ret, id);
              return -1;
          }
          printf("Written %i at address %i\n", (int)val, reg8);
      }
      else {
          int ret = feetech_write16(&dev, reg16, val);
          if (ret != FEETECH_OK) {
              printf("Error[%i] : No response from %i\n", ret, id);
              return -1;
          }
          printf("Written %i at address %i\n", (int)val, reg16);
      }
      return 0;
  }
  
  static const shell_command_t shell_commands[] = {
      { "init", "Initialize a Feetech TTL bus with a given baudrate", cmd_init },
      { "ping", "Ping a Feetech device", cmd_ping },
      { "scan", "Find all Feetech devices between min_id and max_id", cmd_scan },
      { "read", "Read a Feetech device register", cmd_read },
      { "write", "Write in a Feetech device register", cmd_write },
      { NULL, NULL, NULL }
  };
  
  int main(void)
  {
      puts("\nManual Feetech device driver test");
      puts("===================================");
      puts("This application is intended for testing Feetech TTL bus\n");
  
      char line_buf[SHELL_DEFAULT_BUFSIZE];
      shell_run(shell_commands, line_buf, SHELL_DEFAULT_BUFSIZE);
  
      return 0;
  }