/*
   * Copyright (c) 2016, Nordic Semiconductor
   * All rights reserved.
   *
   * Redistribution and use in source and binary forms, with or without
   * modification, are permitted provided that the following conditions
   * are met:
   * 1. Redistributions of source code must retain the above copyright
   *    notice, this list of conditions and the following disclaimer.
   * 2. Redistributions in binary form must reproduce the above copyright
   *    notice, this list of conditions and the following disclaimer in the
   *    documentation and/or other materials provided with the distribution.
   * 3. Neither the name of the Institute nor the names of its contributors
   *    may be used to endorse or promote products derived from this software
   *    without specific prior written permission.
   *
   * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
   * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   * ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
   * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
   * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
   * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
   * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
   * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
   * SUCH DAMAGE.
   *
   */
  
  #include <stdint.h>
  #include <string.h>
  
  #define DONT_OVERRIDE_NVIC
  
  #include "ble-core.h"
  #include "ble_ipsp.h"
  #include "ble_gap.h"
  
  #include "ble-mac.h"
  
  #define ENABLE_DEBUG (0)
  #include "debug.h"
  
  #if defined(MODULE_OD) && ENABLE_DEBUG
  #include "od.h"
  #endif
  
  #ifndef BLE_MAC_MAX_INTERFACE_NUM
  #define BLE_MAC_MAX_INTERFACE_NUM (1U)  /**< Maximum number of interfaces, i.e.,
                                               connection to master devices */
  #endif
  
  typedef struct {
    uint8_t peer_addr[8];
    ble_ipsp_handle_t handle;
  } ble_mac_interface_t;
  
  static ble_mac_interface_t interfaces[BLE_MAC_MAX_INTERFACE_NUM];
  
  volatile int ble_mac_busy_tx;
  volatile int ble_mac_busy_rx;
  
  static ble_mac_inbuf_t inbuf;
  static ble_mac_callback_t _callback;
  
  /**
   * @brief Lookup interface by IPSP connection.
   *
   * @param[in] handle    a pointer to IPSP handle.
   * @return a pointer to interface structure
   * @return NULL if no interface has been found for a given handle
   */
  static ble_mac_interface_t *ble_mac_interface_lookup(ble_ipsp_handle_t *handle)
  {
      if (handle == NULL) {
          return NULL;
      }
      for (int i = 0; i < BLE_MAC_MAX_INTERFACE_NUM; i++) {
          if (interfaces[i].handle.conn_handle == handle->conn_handle &&
              interfaces[i].handle.cid == handle->cid) {
              return &interfaces[i];
          }
      }
      return NULL;
  }
  
  /**
   * @brief Add IPSP connection to the interface table.
   *
   * This function binds IPSP connection with peer address.
   *
   * @param[in] peer      a pointer to eui64 address
   * @param[in] handle    a pointer to IPSP handle
   *
   * @return  a pointer to an interface structure on success
   * @return  NULL if interface table is full
   */
  static ble_mac_interface_t *ble_mac_interface_add(uint8_t peer[8],
                                                    ble_ipsp_handle_t *handle)
  {
      DEBUG("ble_mac_interface_add()\n");
      for (int i = 0; i < BLE_MAC_MAX_INTERFACE_NUM; i++) {
          if (interfaces[i].handle.conn_handle == 0 && interfaces[i].handle.cid == 0) {
              memcpy(&interfaces[i].handle, handle, sizeof(ble_ipsp_handle_t));
              memcpy(&interfaces[i].peer_addr, peer, 8);
  
              /* notify handler thread */
              /* msg_t m = { .type = BLE_IFACE_ADDED, .content.ptr = &interfaces[i] }; */
              /* msg_send(&m, gnrc_nordic_ble_6lowpan_pid); */
  
              return &interfaces[i];
          }
      }
      return NULL;
  }
  
  /**
   * @brief       Remove interface from the interface table.
   * @param[in]   interface a pointer to interface
   */
  static void ble_mac_interface_delete(ble_mac_interface_t *interface)
  {
      DEBUG("ble_mac_interface_delete()\n");
      memset(interface, 0, sizeof(ble_mac_interface_t));
  }
  
  /**
   * @brief Lookup IPSP handle by peer address.
   *
   * @param[in] addr a pointer to eui64 address.
   * @retval a pointer to IPSP handle on success
   * @retval NULL if an IPSP handle for given address haven't been found
   */
  static ble_ipsp_handle_t *_find_handle(const uint8_t *addr)
  {
      for (int i = 0; i < BLE_MAC_MAX_INTERFACE_NUM; i++) {
          if (memcmp(interfaces[i].peer_addr, addr, BLE_SIXLOWPAN_L2_ADDR_LEN) == 0) {
              return &interfaces[i].handle;
          }
      }
      return NULL;
  }
  
  /**
   * @brief Send packet on a given IPSP handle.
   *
   * @param[in] handle a pointer to IPSP handle.
   * @return 1 on success, 0 otherwise
   */
  static int _send_to_peer(ble_ipsp_handle_t *handle, void *data, size_t len)
  {
    DEBUG("ble-mac: sending packet[GAP handle:%d CID:0x%04X]\n",
            handle->conn_handle, handle->cid);
    return ble_ipsp_send(handle, data, len);
  }
  
  static int _is_broadcast(uint8_t dest[8])
  {
      uint32_t *_dest = (uint32_t*)dest;
      for (int i = 0; i < 2; i++) {
          if (_dest[i]) {
              return 0;
          }
      }
      return 1;
  }
  
  int ble_mac_send(uint8_t dest[8], void *data, size_t len)
  {
      DEBUG("ble_mac_send(): sending pkt with len %u\n", (unsigned)len);
  
  #if defined(MODULE_OD) && ENABLE_DEBUG
      od_hex_dump(dest, 8, OD_WIDTH_DEFAULT);
      od_hex_dump(data, len, OD_WIDTH_DEFAULT);
  #endif
  
      ble_ipsp_handle_t *handle;
      int ret = -1;
  
      if ((!dest) || _is_broadcast(dest)) {
          DEBUG("broadcast\n");
          for (int i = 0; i < BLE_MAC_MAX_INTERFACE_NUM; i++) {
              if (interfaces[i].handle.cid != 0 && interfaces[i].handle.conn_handle != 0) {
                  ret = _send_to_peer(&interfaces[i].handle, data, len);
                  DEBUG("ret=%i\n", ret);
              }
          }
      }
      else if ((handle = _find_handle(dest)) != NULL) {
          DEBUG("unicast\n");
          ret = _send_to_peer(handle, data, len);
      }
      else {
          DEBUG("ble-mac: no connection found for peer\n");
      }
  
      if (ret == NRF_SUCCESS) {
          ble_mac_busy_tx = 1;
          return 0;
      }
      else {
          DEBUG("ble-mac: send error: %i\n", ret);
          return -1;
      }
  }
  
  static uint32_t ble_mac_ipsp_evt_handler_irq(ble_ipsp_handle_t *p_handle, ble_ipsp_evt_t *p_evt)
  {
      uint32_t retval = NRF_SUCCESS;
  
      ble_mac_interface_t *p_instance = ble_mac_interface_lookup(p_handle);
  
      if (p_handle) {
          DEBUG("ble-mac: IPSP event [handle:%d CID 0x%04X]\n", p_handle->conn_handle, p_handle->cid);
      }
  
      switch (p_evt->evt_id) {
          case BLE_IPSP_EVT_CHANNEL_CONNECTED: {
              uint8_t peer_addr[8];
  
              DEBUG("ble-mac: channel connected\n");
              ble_eui64_from_eui48(peer_addr, p_evt->evt_param->params.ch_conn_request.peer_addr.addr,
                                   p_evt->evt_param->params.ch_conn_request.peer_addr.addr_type ==
                                   BLE_GAP_ADDR_TYPE_PUBLIC);
  
              p_instance = ble_mac_interface_add(peer_addr, p_handle);
  
              if (p_instance != NULL) {
                  DEBUG("ble-mac: added new IPSP interface\n");
              }
              else {
                  DEBUG("ble-mac: cannot add new interface. Table is full\n");
                  ble_ipsp_disconnect(p_handle);
              }
              break;
          }
  
          case BLE_IPSP_EVT_CHANNEL_DISCONNECTED: {
              DEBUG("ble-mac: channel disconnected\n");
              if (p_instance != NULL) {
                  DEBUG("ble-mac: removed IPSP interface\n");
                  ble_mac_interface_delete(p_instance);
              }
              break;
          }
  
          case BLE_IPSP_EVT_CHANNEL_DATA_RX: {
              DEBUG("ble-mac: data received\n");
              if (p_instance != NULL) {
                  if (ble_mac_busy_rx) {
                      DEBUG("ble-mac: packet dropped as input buffer is busy\n");
                      break;
                  }
  
                  if (p_evt->evt_param->params.ch_rx.len > BLE_SIXLOWPAN_MTU) {
                      DEBUG("ble-mac: packet buffer is too small!\n");
                      break;
                  }
  
                  ble_mac_busy_rx = 1;
  
                  inbuf.len = p_evt->evt_param->params.ch_rx.len;
                  memcpy(inbuf.payload, p_evt->evt_param->params.ch_rx.p_data, inbuf.len);
                  memcpy(inbuf.src, p_instance->peer_addr, 8);
                  sd_ble_gap_rssi_get(p_handle->conn_handle, &inbuf.rssi);
  
                  _callback(BLE_EVENT_RX_DONE, &inbuf);
              }
              else {
                  DEBUG("ble-mac: got data to unknown interface!\n");
              }
              break;
          }
  
          case BLE_IPSP_EVT_CHANNEL_DATA_TX_COMPLETE: {
              DEBUG("ble-mac: data transmitted\n");
              ble_mac_busy_tx = 0;
              //_callback(BLE_EVENT_TX_DONE, NULL);
              break;
          }
      }
  
      if (sched_context_switch_request) {
          NVIC_SetPendingIRQ( SWI0_EGU0_IRQn );
      }
  
      return retval;
  }
  
  void ble_mac_init(ble_mac_callback_t callback)
  {
      assert(callback);
  
      uint32_t res;
      ble_ipsp_init_t ipsp_init_params = {
          .evt_handler = ble_mac_ipsp_evt_handler_irq
      };
  
      _callback = callback;
  
      res = ble_ipsp_init(&ipsp_init_params);
      DEBUG("ble_ipsp_init() res = %" PRIu32 "\n", res);
      (void)res;
  }