#include "uart_cmd_srv.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
 
#include "epwm_drv.h"

static bool cmd_match_with_arg(const volatile char *buf, const char *prefix, const char **arg_out)
{
  size_t len = strlen(prefix);
  for (size_t i = 0; i < len; i++)
  {
    if (buf[i] != prefix[i])
      return false;
  }
  if (buf[len] == ' ')
  {
    *arg_out = (const char *)&buf[len + 1];
    return true;
  }
  return false;
}

static int32_t cmd_parse_int_list(const char *arg, int32_t *out, int32_t max_count)
{
  int32_t count = 0;
  char *end = NULL;
  const char *p = arg;

  while (*p != '\0' && count < max_count)
  {
    while (*p == ' ')
      p++;

    if (*p == '\0')
      break;

    out[count] = strtol(p, &end, 10);
    if (end == p)
      return -1;

    count++;
    p = end;
  }

  while (*p == ' ')
    p++;

  if (*p != '\0')
    return -1;

  return count;
}

static bool cmd_parse_int_args(const char *arg, int32_t *out, int32_t expect_count,
                               const char *usage_msg)
{
  int32_t parsed = cmd_parse_int_list(arg, out, expect_count);

  if (parsed != expect_count)
  {
    uart_send_msg("%s", usage_msg);
    return false;
  }
  return true;
}

static void cmd_handle_val(const char *arg)
{
  int32_t values[8] = { 0 };
  int32_t parsed = cmd_parse_int_list(arg, values, 8);

  if (parsed <= 0)
  {
    uart_send_msg("ERR usage: val n1 n2 ...\r\n");
    return;
  }

  for (int32_t i = 0; i < parsed; i++)
  {
    uart_send_msg("%d\r\n", values[i]);
  }
}

void uart_cmd_process(void)
{
  if (!uart_is_frame_ready())
    return;

  const char *arg = NULL;
  char *cmd = (char *)g_uart_rx_buf;

    if (strcmp(cmd, "help") == 0)
  {
    uart_send_msg("Commands:\r\n");
    uart_send_msg("  help       - show this help\r\n");
    uart_send_msg("  fw_ver     - show firmware version\r\n");
    uart_send_msg("  pwm_en 1/0    - enable/disable PWM output\r\n");
    uart_send_msg("  epwm_set_duty d1 d2 d3 - set duty for 3 channels\r\n");
    uart_send_msg("  val n1 n2 ... - parse integer values\r\n");
  }
  else if (strcmp(cmd, "fw_ver") == 0)
  {
    uart_send_msg("FW: " FW_VERSION "\r\n");
  }
  else if (cmd_match_with_arg(cmd, "pwm_en", &arg))
  {
    if (arg[0] == '1')
    {
      epwm_en(1);
      uart_send_msg("OK pwm on\r\n");
    }
    else if (arg[0] == '0')
    {
      epwm_en(0);
      uart_send_msg("OK pwm off\r\n");
    }
    else
    {
      uart_send_msg("ERR usage: pwm_en 1/0\r\n");
    }
  }
  else if (cmd_match_with_arg(cmd, "epwm_set_duty", &arg))
  {
    int32_t values[3] = { 0 };
    if (cmd_parse_int_args(arg, values, 3,
                           "ERR usage: epwm_set_duty duty1 duty2 duty3\r\n"))
    {
      epwm_set_duty(values[0], values[1], values[2]);
      uart_send_msg("OK epwm_set_duty: %d %d %d\r\n", values[0], values[1], values[2]);
    }
  }
  else if (cmd_match_with_arg(cmd, "val", &arg))
  {
    cmd_handle_val(arg);
  }
  else
  {
    uart_send_msg("ERR unknown cmd\r\n");
  }

  /* Reset for next command */
  uart_clear_rx_frame();
}