/// This software is distributed under the terms of the MIT License.
/// Copyright (c) 2020 OpenCyphal
/// Authors: Pavel Kirienko <pavel.kirienko@zubax.com>, Tom De Rybel
/// <tom.derybel@robocow.be>

// This is needed to enable the necessary declarations in sys/
#include "FreeRTOS.h"
#include <string.h>

#include "canard.h"
#include "esp32twaican.h"

#include "task.h" 
#include "driver/twai.h"

#include "Arduino.h"

#include "USB.h"
extern USBCDC usbserial;
#define Serial usbserial

int esp32twaicanOpen(const int pin_tx, const int pin_rx,
                     const struct CanardFilter *filter_config) {

  twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(
      (gpio_num_t)pin_tx, (gpio_num_t)pin_rx, TWAI_MODE_NORMAL);
  g_config.rx_queue_len = 100;
  g_config.tx_queue_len = 100;
  twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
  twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
  if (filter_config) {
    f_config.acceptance_code = filter_config->extended_can_id;
    f_config.acceptance_mask = filter_config->extended_mask;
    f_config.single_filter = true;
  }
  int ret = twai_driver_install(&g_config, &t_config, &f_config);
  if (ret != ESP_OK) {
    // Serial.println("CAN1 Driver initialized");
    assert(false);
    return ret;
  }
  ret = twai_start();
  if (ret != ESP_OK) {
    assert(false);
    return ret;
  }

  uint32_t alerts_to_enable = TWAI_ALERT_TX_IDLE | TWAI_ALERT_TX_SUCCESS |
                              TWAI_ALERT_TX_FAILED | TWAI_ALERT_RX_QUEUE_FULL |
                              TWAI_ALERT_RX_DATA | TWAI_ALERT_ERR_PASS |
                              TWAI_ALERT_BUS_ERROR;
  ret = twai_reconfigure_alerts(alerts_to_enable, NULL);
  if (ret != ESP_OK) {
    assert(false);
    // Serial.println("Failed to reconfigure alerts");
    return ret;
  }
  //digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000);

  return 0;
}

int16_t esp32twaicanPush(const struct CanardFrame *const frame,
                         const CanardMicrosecond timeout_usec) {
  if ((frame == NULL) || (frame->payload.size == 0) ||
      (frame->payload.size > UINT8_MAX)) {
    return -1;
  }

  // Queue message for transmission
  twai_message_t message;
  message.extd = 1; // extended can id
  message.identifier = frame->extended_can_id;
  message.rtr = 0;
  message.self = 0;
  message.data_length_code = frame->payload.size;
  //printf(stderr, "size %d  data %d", frame->payload.size, (int)frame->payload.data);
  memcpy(&message.data[0], ((const uint8_t *)frame->payload.data), (int)frame->payload.size);
  int ret = twai_transmit(&message, (timeout_usec * (1e6 / configTICK_RATE_HZ)));
  if (ret == ESP_OK) {
    return 1;
    // Serial.println("CAN1: Message queued for transmission");
  } else if (ret == ESP_ERR_TIMEOUT) {
    /* Queue full and timed out waiting for empty slot */
    return 0;
  } else if( ret == ESP_ERR_INVALID_STATE) {
    assert(false);
  }
  return -1;
}

int16_t esp32twaicanPop(struct CanardFrame *const out_frame,
                        CanardMicrosecond *const out_timestamp_usec,
                        const size_t payload_buffer_size,
                        void *const payload_buffer,
                        const CanardMicrosecond timeout_usec,
                        bool *const loopback) {
  if ((out_frame == NULL) || (payload_buffer == NULL)) {
    return -1;
  }

  uint32_t alerts_triggered;
  twai_read_alerts(&alerts_triggered,
                   (timeout_usec * (1e6 / configTICK_RATE_HZ)));
  twai_status_info_t twaistatus;
  twai_get_status_info(&twaistatus);
  if (alerts_triggered & TWAI_ALERT_RX_DATA || true) {
    twai_message_t message;
    if (twai_receive(&message, 0) == ESP_OK) {
    //Serial.println("TWAI_ALERT_RX_DATA ESP_OK");
      // Non-blocking receive messages from the socket and validate.
      const ssize_t read_size = message.data_length_code;
      if (read_size < 0) {
        return -1; // Error occurred -- return the negated error
                   // code.
      }
      if (read_size > payload_buffer_size) {
        return -1;
      }

      uint8_t cmp[] = {0,1,2,3,4,5,6,7};
      if(memcmp(message.data, cmp,sizeof(cmp))==0) {
        Serial.printf("MSG!\n");
      }

      const bool valid = (message.extd) &&         // Extended frame
                         (!message.rtr) &&          // Not RTR frame
                         (true /* error frame */); // Not error frame
      if (!valid) {
        return 0; // Not an extended data frame -- drop silently and return
                  // early.
      }




      // Handle the loopback frame logic.
      const bool loopback_frame = false;
      // ((uint32_t)msg.msg_flags & (uint32_t)MSG_CONFIRM) != 0;
      if (loopback == NULL && loopback_frame) {
        return 0; // The loopback pointer is NULL and this is a loopback frame
                  // -- drop silently and return early.
      }
      if (loopback != NULL) {
        *loopback = loopback_frame;
      }
      // Obtain the CAN frame time stamp from the kernel.
      if (NULL != out_timestamp_usec) {
        *out_timestamp_usec =
            (CanardMicrosecond)(xTaskGetTickCount() *
                                (1e6 / configTICK_RATE_HZ));
      }
      out_frame->extended_can_id = message.identifier;
      out_frame->payload.size = message.data_length_code;
      out_frame->payload.data = payload_buffer;
      (void)memcpy(payload_buffer, &message.data[0], message.data_length_code);
      return 1;
    }
  }
  return 0;
}
