/*
   * Copyright (C) 2016 OTA keys S.A.
   *
   * 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.
   */
  
  /**
   * @ingroup     tests
   * @{
   *
   * @file
   * @brief       main
   *
   * @author      Vincent Dupont <vincent@otakeys.com>
   *
   * @}
   */
  #include <string.h>
  #include <stdlib.h>
  #include <stdio.h>
  #include <inttypes.h>
  
  #include "shell.h"
  #include "board.h"
  #include "periph/gpio.h"
  #include "thread.h"
  
  #include "can/can.h"
  #include "can/conn/raw.h"
  #include "can/conn/isotp.h"
  #include "can/device.h"
  
  #define THREAD_STACKSIZE   (THREAD_STACKSIZE_MAIN)
  #define RECEIVE_THREAD_MSG_QUEUE_SIZE   (8)
  
  #include "timex.h"
  #define TEST_CONN_CAN_RECV_TIMEOUT (30 * US_PER_SEC)
  
  #define RCV_THREAD_NUMOF (2)
  
  #define MAX_FILTER      (16)
  
  #define CAN_MSG_RECV        0x400
  #define CAN_MSG_BIND_ISOTP  0x401
  #define CAN_MSG_RECV_ISOTP  0x402
  #define CAN_MSG_CLOSE_ISOTP 0x403
  #define CAN_MSG_SEND_ISOTP  0x404
  
  static char thread_stack[RCV_THREAD_NUMOF][THREAD_STACKSIZE];
  static kernel_pid_t receive_pid[RCV_THREAD_NUMOF];
  
  static conn_can_raw_t conn[RCV_THREAD_NUMOF];
  static struct can_filter filters[RCV_THREAD_NUMOF][MAX_FILTER];
  
  #ifdef MODULE_CAN_ISOTP
  #define ISOTP_BUF_SIZE 1024
  static uint8_t isotp_buf[RCV_THREAD_NUMOF][ISOTP_BUF_SIZE];
  
  static conn_can_isotp_t conn_isotp[RCV_THREAD_NUMOF];
  #endif
  
  static int thread_busy[RCV_THREAD_NUMOF];
  
  static void print_usage(void)
  {
      puts("test_can list");
      puts("test_can send ifnum can_id [B1 [B2 [B3 [B4 [B5 [B6 [B7 [B8]]]]]]]]");
      printf("test_can recv ifnum user_id timeout can_id1 [can_id2..can_id%d]\n", MAX_FILTER);
      puts("test_can close user_id");
  #ifdef MODULE_CAN_ISOTP
      puts("test_can bind_isotp ifnum user_id source_id dest_id");
      puts("test_can send_isotp user_id [B1 [.. [ Bn ]]]");
      puts("test_can recv_isotp user_id timeout");
      puts("test_can close_isotp user_id");
  #endif
      puts("test_can get_filter ifnum");
      puts("test_can set_bitrate ifnum bitrate [sample_point]");
      puts("test_can get_bitrate ifnum");
      puts("test_can get_counter ifnum");
      puts("test_can power_up ifnum");
      puts("test_can power_down ifnum");
  }
  
  static int _list(int argc, char **argv) {
  
      (void)argc;
      (void)argv;
  
      for (int i = 0; i < CAN_DLL_NUMOF; i++) {
          const char *name = raw_can_get_name_by_ifnum(i);
          if (name) {
              printf("CAN #%d: %s\n", i, name);
          }
          else {
              break;
          }
      }
  
      return 0;
  }
  
  static int _send(int argc, char **argv)
  {
      if (argc < 5) {
          print_usage();
          return 1;
      }
      struct can_frame frame;
      int ifnum = strtol(argv[2], NULL, 0);
      if (ifnum >= CAN_DLL_NUMOF) {
          puts("Invalid interface number");
          return 1;
      }
  
      frame.can_id = strtoul(argv[3], NULL, 16);
      frame.can_dlc = argc - 4;
      if (frame.can_dlc > 8) {
          puts("Invalid length");
          return 1;
      }
      for (int i = 0; i < frame.can_dlc; i++) {
          frame.data[i] = strtol(argv[4 + i], NULL, 16);
      }
      conn_can_raw_t conn;
      conn_can_raw_create(&conn, NULL, 0, ifnum, 0);
      int ret = conn_can_raw_send(&conn, &frame, 0);
      if (ret < 0) {
          puts("Error when trying to send");
      }
      return 0;
  }
  
  static int _receive(int argc, char **argv)
  {
      if (argc < 4) {
          print_usage();
          return 1;
      }
      int res;
      int ifnum = strtol(argv[2], NULL, 0);
      if (ifnum >= CAN_DLL_NUMOF) {
          puts("Invalid interface number");
          return 1;
      }
  
      int thread_nb = strtol(argv[3], NULL, 0);
      int filt_num = argc - 5;
      if (thread_nb >= RCV_THREAD_NUMOF) {
          printf("Invalid thread number, range=0..%d\n", RCV_THREAD_NUMOF - 1);
          return 1;
      }
      if (thread_busy[thread_nb]) {
          puts("Thread already in use");
          return 1;
      }
      if (filt_num > MAX_FILTER) {
          puts("Too many filters");
          return 1;
      }
      for (int i = 0; i < filt_num; i++) {
          filters[thread_nb][i].can_id = strtoul(argv[5 + i], NULL, 16);
          filters[thread_nb][i].can_mask = 0xffffffff;
      }
      uint32_t timeout = strtoul(argv[4], NULL, 0);
      msg_t msg;
      msg.type = CAN_MSG_RECV;
      msg.content.value = timeout;
      res = conn_can_raw_create(&conn[thread_nb], filters[thread_nb],
                                filt_num, ifnum, 0);
      if (res < 0) {
          puts("Error when setting filters");
          return 1;
      }
      thread_busy[thread_nb] = 1;
      msg_send(&msg, receive_pid[thread_nb]);
      return 0;
  }
  
  static int _close(int argc, char **argv)
  {
      if (argc < 2) {
          print_usage();
          return 1;
      }
      int thread_nb = strtol(argv[2], NULL, 0);
      if (thread_nb >= RCV_THREAD_NUMOF) {
          printf("Invalid thread number, range=0..%d\n", RCV_THREAD_NUMOF - 1);
          return 1;
      }
      conn_can_raw_close(&conn[thread_nb]);
      thread_busy[thread_nb] = 0;
      return 0;
  }
  
  #ifdef MODULE_CAN_ISOTP
  static int _bind_isotp(int argc, char **argv)
  {
      if (argc < 4) {
          print_usage();
          return 1;
      }
      int ret;
      int ifnum = strtol(argv[2], NULL, 0);
      if (ifnum >= CAN_DLL_NUMOF) {
          puts("Invalid interface number");
          return 1;
      }
  
      int thread_nb = strtol(argv[3], NULL, 0);
      if (thread_nb >= RCV_THREAD_NUMOF) {
          printf("Invalid thread number, range=0..%d\n", RCV_THREAD_NUMOF - 1);
          return 1;
      }
      if (thread_busy[thread_nb]) {
          puts("Thread already in use");
          return 1;
      }
      struct isotp_options isotp_opt;
      memset(&isotp_opt, 0, sizeof(isotp_opt));
      isotp_opt.tx_id = strtoul(argv[4], NULL, 16);
      isotp_opt.rx_id = strtoul(argv[5], NULL, 16);
  
  #ifdef MODULE_CONN_CAN_ISOTP_MULTI
      conn_can_isotp_init_slave(&conn_isotp[thread_nb], (conn_can_isotp_slave_t *)&conn_isotp[thread_nb]);
  #endif
      ret = conn_can_isotp_create(&conn_isotp[thread_nb], &isotp_opt, ifnum);
      if (ret == 0) {
          ret = conn_can_isotp_bind(&conn_isotp[thread_nb]);
      }
      if (ret < 0) {
          puts("Error when binding connection");
          return 1;
      }
      thread_busy[thread_nb] = 1;
      return 0;
  }
  
  static int _send_isotp(int argc, char **argv)
  {
      if (argc < 4) {
          print_usage();
          return 1;
      }
      int thread_nb = strtoul(argv[2], NULL, 0);
      if (thread_nb >= RCV_THREAD_NUMOF) {
          printf("Invalid thread number, range=0..%d\n", RCV_THREAD_NUMOF - 1);
          return 1;
      }
      int len = argc - 3;
      uint8_t data[len];
  
      for (int i = 0; i < len; i++) {
          data[i] = strtol(argv[3 + i], NULL, 16);
      }
  
      msg_t msg, reply;
      can_opt_t opt;
      opt.data = data;
      opt.data_len = len;
      msg.type = CAN_MSG_SEND_ISOTP;
      msg.content.ptr = &opt;
      int res = msg_send_receive(&msg, &reply, receive_pid[thread_nb]);
      if (res < 0 || (int)reply.content.value < 0) {
          puts("Error when sending");
          return 1;
      }
      return 0;
  }
  
  static int _receive_isotp(int argc, char **argv)
  {
      if (argc < 4) {
          print_usage();
          return 1;
      }
      int thread_nb = strtol(argv[2], NULL, 0);
      if (thread_nb >= RCV_THREAD_NUMOF) {
          printf("Invalid thread number, range=0..%d\n", RCV_THREAD_NUMOF - 1);
          return 1;
      }
      uint32_t timeout = strtoul(argv[3], NULL, 0);
  
      msg_t msg;
      msg.type = CAN_MSG_RECV_ISOTP;
      msg.content.value = timeout;
      msg_send(&msg, receive_pid[thread_nb]);
  
      return 0;
  }
  
  static int _close_isotp(int argc, char **argv)
  {
      if (argc < 2) {
          print_usage();
          return 1;
      }
      int thread_nb = strtol(argv[2], NULL, 0);
      if (thread_nb >= RCV_THREAD_NUMOF) {
          printf("Invalid thread number, range=0..%d\n", RCV_THREAD_NUMOF - 1);
          return 1;
      }
      conn_can_isotp_close(&conn_isotp[thread_nb]);
      thread_busy[thread_nb] = 0;
      return 0;
  }
  #endif /* MODULE_CAN_ISOTP */
  
  static int _get_filter(int argc, char **argv)
  {
      if (argc < 3) {
          print_usage();
          return 1;
      }
      int res;
      int ifnum = strtol(argv[2], NULL, 0);
      struct can_filter filters[32];
      can_opt_t opt;
      opt.data = (void *)filters;
      opt.data_len = sizeof(filters);
      opt.opt = CANOPT_RX_FILTERS;
      res = raw_can_get_can_opt(ifnum, &opt);
      if (res < 0) {
          puts("Error when reading filters");
      }
      else if (res == 0) {
          puts("No filter set");
      }
      else {
          for (unsigned int i = 0; i < res / sizeof(filters[0]); i++) {
              printf("Filter %d: 0x%" PRIx32"\n", i, filters[i].can_id);
              printf("Mask %d: 0x%" PRIx32"\n", i, filters[i].can_mask);
          }
      }
      return 0;
  }
  
  static int _set_bitrate(int argc, char **argv)
  {
      if (argc < 4) {
          print_usage();
          return 1;
      }
  
      int ifnum = strtol(argv[2], NULL, 0);
      if (ifnum >= CAN_DLL_NUMOF) {
          printf("Invalid ifnum %d\n", ifnum);
          return 1;
      }
      uint32_t bitrate = strtoul(argv[3], NULL, 0);
      uint32_t sample_point = 0;
  
      int ret;
      if (argc > 4) {
          sample_point = strtoul(argv[4], NULL, 0);
      }
      printf("Setting bitrate=%" PRIu32 ", sample point=%" PRIu32 "\n",
             bitrate, sample_point);
      ret = raw_can_set_bitrate(ifnum, bitrate, sample_point);
      if (ret < 0) {
          printf("Error when setting bitrate: res=%d\n", ret);
          return 1;
      }
      else if (ret == 1) {
          puts("Bitrate/sample_point cannot be reached");
      }
  
      puts("Bittimings successfully set");
  
      return 0;
  }
  
  static int _get_bitrate(int argc, char **argv)
  {
      if (argc < 3) {
          print_usage();
          return 1;
      }
  
      int ifnum = strtol(argv[2], NULL, 0);
      struct can_bittiming bittiming;
      can_opt_t opt;
      opt.data = &bittiming;
      opt.data_len = sizeof(bittiming);
      opt.opt = CANOPT_BITTIMING;
  
      int ret = raw_can_get_can_opt(ifnum, &opt);
      if (ret < 0) {
          printf("Error when getting bitrate: res=%d\n", ret);
          return 1;
      }
  
      printf("Bitrate read: bitrate=%" PRIu32 ", sample_point=%" PRIu32
             "\nbrp=%" PRIu32 "phase-seg1=%" PRIu32
             ", phase-seg2=%" PRIu32 ", sjw=%" PRIu32 "\n", bittiming.bitrate,
             bittiming.sample_point, bittiming.brp, bittiming.phase_seg1,
             bittiming.phase_seg2, bittiming.sjw);
      return 0;
  }
  
  static int _get_counter(int argc, char **argv)
  {
      if (argc < 3) {
          print_usage();
          return 1;
      }
  
      int res = 0;
      int ifnum = strtol(argv[2], NULL, 0);
      if (ifnum >= CAN_DLL_NUMOF) {
          puts("Invalid interface number");
          return 1;
      }
  
      uint16_t cnt;
      can_opt_t opt;
      opt.data = &cnt;
      opt.data_len = sizeof(cnt);
      opt.opt = CANOPT_TEC;
  
      int ret = raw_can_get_can_opt(ifnum, &opt);
      if (ret < 0) {
          printf("Error when getting TEC: res=%d\n", ret);
          res = 1;
      }
      else {
          printf("TEC=%" PRIu16, cnt);
      }
  
      opt.opt = CANOPT_REC;
  
      ret = raw_can_get_can_opt(ifnum, &opt);
      if (ret < 0) {
          printf("\nError when getting REC: res=%d\n", ret);
          res = 1;
      }
      else {
          printf(", REC=%" PRIu16 "\n", cnt);
      }
  
      return res;
  }
  
  static int _power_up(int argc, char **argv)
  {
      if (argc < 3) {
          print_usage();
          return 1;
      }
  
      int res = 0;
      int ifnum = strtol(argv[2], NULL, 0);
      if (ifnum >= CAN_DLL_NUMOF) {
          puts("Invalid interface number");
          return 1;
      }
  
      int ret = raw_can_power_up(ifnum);
      if (ret < 0) {
          printf("Error when powering up: res=%d\n", ret);
          res = 1;
      }
  
      return res;
  }
  
  static int _power_down(int argc, char **argv)
  {
      if (argc < 3) {
          print_usage();
          return 1;
      }
  
      int res = 0;
      int ifnum = strtol(argv[2], NULL, 0);
      if (ifnum >= CAN_DLL_NUMOF) {
          puts("Invalid interface number");
          return 1;
      }
  
      int ret = raw_can_power_down(ifnum);
      if (ret < 0) {
          printf("Error when powering up: res=%d\n", ret);
          res = 1;
      }
  
      return res;
  }
  
  static int _can_handler(int argc, char **argv)
  {
      if (argc < 2) {
          print_usage();
          return 1;
      }
      else if (strncmp(argv[1], "list", 5) == 0) {
          return _list(argc, argv);
      }
      else if (strncmp(argv[1], "send", 5) == 0) {
          return _send(argc, argv);
      }
      else if (strncmp(argv[1], "recv", 5) == 0) {
          return _receive(argc, argv);
      }
      else if (strncmp(argv[1], "close", 6) == 0) {
          return _close(argc, argv);
      }
  #ifdef MODULE_CAN_ISOTP
      else if (strncmp(argv[1], "bind_isotp", 11) == 0) {
          return _bind_isotp(argc, argv);
      }
      else if (strncmp(argv[1], "send_isotp", 11) == 0) {
          return _send_isotp(argc, argv);
      }
      else if (strncmp(argv[1], "recv_isotp", 11) == 0) {
          return _receive_isotp(argc, argv);
      }
      else if (strncmp(argv[1], "close_isotp", 12) == 0) {
          return _close_isotp(argc, argv);
      }
  #endif
      else if (strncmp(argv[1], "get_filter", 10) == 0) {
          return _get_filter(argc, argv);
      }
      else if (strncmp(argv[1], "set_bitrate", 11) == 0) {
          return _set_bitrate(argc, argv);
      }
      else if (strncmp(argv[1], "get_bitrate", 11) == 0) {
          return _get_bitrate(argc, argv);
      }
      else if (strncmp(argv[1], "get_counter", 11) == 0) {
          return _get_counter(argc, argv);
      }
      else if (strncmp(argv[1], "power_up", 9) == 0) {
          return _power_up(argc, argv);
      }
      else if (strncmp(argv[1], "power_down", 11) == 0) {
          return _power_down(argc, argv);
      }
      else {
          printf("unknown command: %s\n", argv[1]);
          return 1;
      }
  }
  
  static void *_receive_thread(void *args)
  {
      int thread_nb = (int)args;
      struct can_frame frame;
      msg_t msg, msg_queue[RECEIVE_THREAD_MSG_QUEUE_SIZE];
  
      /* setup the device layers message queue */
      msg_init_queue(msg_queue, RECEIVE_THREAD_MSG_QUEUE_SIZE);
  
      printf("%d: launching receive_thread\n", thread_nb);
  
      while (1) {
          msg_receive(&msg);
          switch (msg.type) {
          case CAN_MSG_RECV:
          {
              int ret;
              while ((ret = conn_can_raw_recv(&conn[thread_nb], &frame, msg.content.value))
                     == sizeof(struct can_frame)) {
                  printf("%d: %-8s %" PRIx32 "  [%x] ",
                         thread_nb, raw_can_get_name_by_ifnum(conn[thread_nb].ifnum),
                         frame.can_id, frame.can_dlc);
                  for (int i = 0; i < frame.can_dlc; i++) {
                      printf(" %02X", frame.data[i]);
                  }
                  printf("\n");
              }
              printf("%d: recv terminated: ret=%d\n", thread_nb, ret);
              conn_can_raw_close(&conn[thread_nb]);
              thread_busy[thread_nb] = 0;
              break;
          }
  #ifdef MODULE_CAN_ISOTP
          case CAN_MSG_RECV_ISOTP:
          {
              int ret;
              while ((ret = conn_can_isotp_recv(&conn_isotp[thread_nb], isotp_buf[thread_nb],
                                                ISOTP_BUF_SIZE, msg.content.value))
                     <= ISOTP_BUF_SIZE && ret >= 0) {
                  printf("%d: %-8s ISOTP [%d] ",
                         thread_nb, raw_can_get_name_by_ifnum(conn_isotp[thread_nb].ifnum), ret);
                  for (int i = 0; i < ret; i++) {
                      printf(" %02X", isotp_buf[thread_nb][i]);
                  }
                  printf("\n");
              }
              printf("%d: recv terminated: ret=%d\n", thread_nb, ret);
              break;
          }
          case CAN_MSG_SEND_ISOTP:
          {
              msg_t reply;
              can_opt_t *opt = msg.content.ptr;
              int ret = conn_can_isotp_send(&conn_isotp[thread_nb], opt->data, opt->data_len, 0);
              reply.type = msg.type;
              reply.content.value = ret;
              msg_reply(&msg, &reply);
              break;
          }
  #endif /* MODULE_CAN_ISOTP */
          default:
              printf("%d: _receive_thread: received unknown message\n", thread_nb);
              break;
          }
      }
  
      return NULL;
  }
  
  static const shell_command_t _commands[] = {
      {"test_can", "Test CAN functions", _can_handler},
      { NULL, NULL, NULL},
  };
  
  int main(void)
  {
      for (int i = 0; i < RCV_THREAD_NUMOF; i++) {
          receive_pid[i] = thread_create(thread_stack[i], THREAD_STACKSIZE,
                                         THREAD_PRIORITY_MAIN - 1,
                                         THREAD_CREATE_STACKTEST, _receive_thread,
                                         (void*)i, "receive_thread");
      }
  
      char line_buf[SHELL_DEFAULT_BUFSIZE];
      shell_run(_commands, line_buf, SHELL_DEFAULT_BUFSIZE);
  
      return 0;
  }