#ifdef __USB_COMM__
#include "app_factory_cdc_comm.h"
#include "plat_types.h"
#include "stdbool.h"
#include "stdint.h"
#include "stdio.h"
#include "string.h"
#include "sys_api_cdc_comm.h"
#include "tool_msg.h"

static enum PARSE_STATE parse_state;
static struct message_t recv_msg;
static struct message_t send_msg = {
    {
        PREFIX_CHAR,
    },
};

static unsigned char check_sum(unsigned char *buf, unsigned char len) {
  int i;
  unsigned char sum = 0;

  for (i = 0; i < len; i++) {
    sum += buf[i];
  }

  return sum;
}

int send_reply(const unsigned char *payload, unsigned int len) {
  int ret = 0;

  if (len + 1 > sizeof(send_msg.data)) {
    TRACE(1, "Packet length too long: %u", len);
    return -1;
  }

  send_msg.hdr.type = recv_msg.hdr.type;
  send_msg.hdr.seq = recv_msg.hdr.seq;
  send_msg.hdr.len = len;
  memcpy(&send_msg.data[0], payload, len);
  send_msg.data[len] =
      ~check_sum((unsigned char *)&send_msg, MSG_TOTAL_LEN(&send_msg) - 1);

  ret = send_data((unsigned char *)&send_msg, MSG_TOTAL_LEN(&send_msg));

  return ret;
}

static void reset_parse_state(unsigned char **buf, size_t *len) {
  parse_state = PARSE_HEADER;
  memset(&recv_msg.hdr, 0, sizeof(recv_msg.hdr));

  *buf = (unsigned char *)&recv_msg.hdr;
  *len = sizeof(recv_msg.hdr);
}

static enum ERR_CODE check_msg_hdr(void) {
  enum ERR_CODE errcode = ERR_NONE;

  switch (recv_msg.hdr.type) {
  case TYPE_SYS:
    if (recv_msg.hdr.len != 1 && recv_msg.hdr.len != 5) {
      // TRACE(1,"SYS msg length error: %u", recv_msg.hdr.len);
      errcode = ERR_LEN;
    }
    break;
  case TYPE_READ:
    if (recv_msg.hdr.len != 4) {
      // TRACE(1,"READ msg length error: %u", recv_msg.hdr.len);
      errcode = ERR_LEN;
    }
    break;
  case TYPE_WRITE:
    if (recv_msg.hdr.len <= 4 || recv_msg.hdr.len > 20) {
      // TRACE(1,"WRITE msg length error: %u", recv_msg.hdr.len);
      errcode = ERR_LEN;
    }
    break;
  default:
    break;
  }

  if (errcode == ERR_NONE && recv_msg.hdr.len + 1 > sizeof(recv_msg.data)) {
    errcode = ERR_LEN;
  }

  return errcode;
}

static enum ERR_CODE handle_sys_cmd(enum SYS_CMD_TYPE cmd, unsigned char *param,
                                    unsigned int len) {
  unsigned char cret[5];
  unsigned int bootmode;

  cret[0] = ERR_NONE;

  if (cmd == SYS_CMD_SET_BOOTMODE || cmd == SYS_CMD_CLR_BOOTMODE) {
    if (len != 4) {
      TRACE(2, "Invalid SYS CMD len %u for cmd: 0x%x", len, cmd);
      return ERR_DATA_LEN;
    }
  } else {
    if (len != 0) {
      TRACE(2, "Invalid SYS CMD len %u for cmd: 0x%x", len, cmd);
      return ERR_DATA_LEN;
    }
  }

  switch (cmd) {
  case SYS_CMD_REBOOT: {
    TRACE(0, "--- Reboot---");
    send_reply(cret, 1);
    system_reboot();
    break;
  }
  case SYS_CMD_SHUTDOWN: {
    TRACE(0, "--- Shutdown ---");
    send_reply(cret, 1);
    system_shutdown();
    break;
  }
  case SYS_CMD_SET_BOOTMODE: {
    TRACE(0, "--- Set bootmode ---");
    memcpy(&bootmode, param, 4);
    system_set_bootmode(bootmode);
    send_reply(cret, 1);
    break;
  }
  case SYS_CMD_CLR_BOOTMODE: {
    TRACE(0, "--- Clear bootmode ---");
    memcpy(&bootmode, param, 4);
    system_clear_bootmode(bootmode);
    send_reply(cret, 1);
    break;
  }
  case SYS_CMD_GET_BOOTMODE: {
    TRACE(0, "--- Get bootmode ---");
    bootmode = system_get_bootmode();
    memcpy(&cret[1], &bootmode, 4);
    send_reply(cret, 5);
    break;
  }
  default: {
    TRACE(1, "Invalid command: 0x%x", recv_msg.data[0]);
    return ERR_SYS_CMD;
  }
  }

  return ERR_NONE;
}

static enum ERR_CODE handle_data(unsigned char **buf, size_t *len, int *extra) {
  enum ERR_CODE errcode = ERR_NONE;
#if 0
	uint32_t rlen = 0;
#endif
  *extra = 0;

  // Checksum
  if (check_sum((unsigned char *)&recv_msg, MSG_TOTAL_LEN(&recv_msg)) != 0xFF) {
    TRACE(0, "Checksum error");
    return ERR_CHECKSUM;
  }

  switch (recv_msg.hdr.type) {
  case TYPE_SYS: {
    TRACE_TIME(0, "------ SYS CMD ------");
    errcode = handle_sys_cmd((enum SYS_CMD_TYPE)recv_msg.data[0],
                             &recv_msg.data[1], recv_msg.hdr.len - 1);
    if (errcode != ERR_NONE) {
      return errcode;
    }
    break;
  }
  case TYPE_READ: {
    TRACE_TIME(0, "------ READ CMD ------");
#if 0
            uint32_t addr = (recv_msg.data[0] << 16) | (recv_msg.data[1] << 8) | recv_msg.data[2];
            uint8_t data[4] = {0};
            rlen = read_reg(addr, data);
            if(rlen == 0)
                return ERR_LEN;
            else {
                send_reply(data, rlen);
            }
#endif
    break;
  }
  case TYPE_WRITE: {
    TRACE_TIME(0, "------ WRITE CMD ------");
#if 0
            uint32_t addr = (recv_msg.data[0] << 16) | (recv_msg.data[1] << 8) | recv_msg.data[2];
            uint32_t wdata = (recv_msg.data[3] << 24) | (recv_msg.data[4] << 16) | (recv_msg.data[5] << 8) | recv_msg.data[6];
            uint8_t data[1] = {0};
            errcode = write_reg(addr, wdata);
            if (errcode != ERR_NONE)
                return errcode;
            else
                send_reply(data, 1);
#endif
    break;
  }

  default:
    break;
  }

  return ERR_NONE;
}

static int parse_packet(unsigned char **buf, size_t *len) {
  enum ERR_CODE errcode;
  int rlen = *len;
  unsigned char *data;
  int i;
  int extra;
  unsigned char cret;

  switch (parse_state) {
  case PARSE_HEADER:
    ASSERT(rlen > 0 && rlen <= sizeof(recv_msg.hdr), "Invalid rlen!");

    if (recv_msg.hdr.prefix == PREFIX_CHAR) {
      errcode = check_msg_hdr();
      if (errcode != ERR_NONE) {
        goto _err;
      }
      parse_state = PARSE_DATA;
      *buf = &recv_msg.data[0];
      *len = recv_msg.hdr.len + 1;
    } else {
      data = (unsigned char *)&recv_msg.hdr.prefix;
      for (i = 1; i < rlen; i++) {
        if (data[i] == PREFIX_CHAR) {
          memmove(&recv_msg.hdr.prefix, &data[i], rlen - i);
          break;
        }
      }
      *buf = &data[rlen - i];
      *len = sizeof(recv_msg.hdr) + i - rlen;
    }
    break;
  case PARSE_DATA:
    errcode = handle_data(buf, len, &extra);
    if (errcode != ERR_NONE) {
      goto _err;
    }
    // Receive next message
    reset_parse_state(buf, len);
    break;
  default:
    TRACE(1, "Invalid parse_state: %d", parse_state);
    break;
  }

  return 0;

_err:
  cancel_input();
  cret = (unsigned char)errcode;
  send_reply(&cret, 1);

  return 1;
}

void comm_loop(void) {
  int ret;
  unsigned char *buf = NULL;
  size_t len = 0;
  size_t buf_len, rlen;

_sync:
  reset_transport();
  reset_parse_state(&buf, &len);

  while (1) {
    rlen = 0;
    if (parse_state == PARSE_HEADER) {
      set_recv_timeout(default_recv_timeout_idle);
    } else {
      set_recv_timeout(default_recv_timeout_short);
    }
    buf_len = 0;

    ret = recv_data_ex(buf, buf_len, len, &rlen);
    if (ret) {
      TRACE(1, "Receiving data failed: %d", ret);
      goto _err;
    }

    if (len != rlen) {
      TRACE(2, "Receiving part of the data: expect=%u real=%u", len, rlen);
      goto _err;
    }

    ret = parse_packet(&buf, &len);
    if (ret) {
      TRACE(0, "Parsing packet failed");
      goto _err;
    }
  }

_err:
  ret = handle_error();
  if (ret == 0) {
    TRACE(0, "retry ...");
    goto _sync;
  }

  return;
}
#endif