#include <SPI.h>
#include <lib_aci.h>
#include <aci_setup.h>

#include "services.h"
#include "app.h"

static services_pipe_type_mapping_t services_pipe_type_mapping[NUMBER_OF_PIPES] = SERVICES_PIPE_TYPE_MAPPING_CONTENT;
static hal_aci_data_t setup_msgs[NB_SETUP_MESSAGES] PROGMEM = SETUP_MESSAGES_CONTENT;

static struct aci_state_t aci_state;
static hal_aci_evt_t aci_data;
static bool timing_change_done  = false;

void __ble_assert(const char *file, uint16_t line)
{
  Serial.print("ERROR ");
  Serial.print(file);
  Serial.print(": ");
  Serial.print(line);
  Serial.print("\n");
  while(1);
}

void setup() {
#if defined(BLEND_MICRO_8MHZ)
  // As the F_CPU = 8000000UL, the USB core make the PLLCSR = 0x02
  // But the external xtal is 16000000Hz, so correct it here.
  PLLCSR |= 0x10;             // Need 16 MHz xtal
  while (!(PLLCSR & (1<<PLOCK)));     // wait for lock pll
#elif defined(BLEND_MICRO_16MHZ)
  // The CPU clock in bootloader is 8MHz, change to 16MHz for sketches to run (i.e. overclock running at 3.3v).
  CLKPR = 0x80;
  CLKPR = 0;
#endif

  Serial.begin((unsigned int)115200);

  // Wait until the serial port is available (useful only for the Leonardo)
  // As the Leonardo board is not reseted every time you open the Serial Monitor
#if defined (__AVR_ATmega32U4__)
  while(!Serial) {
  }

  delay(1000);
#elif defined(__PIC32MX__)
  delay(1000);
#endif

  setup_rf();
}

void setup_rf() {
  Serial.println(F("setup_rf()"));
  // Point ACI data structures to the the setup data that the nRFgo studio generated for the nRF8001
  if (NULL != services_pipe_type_mapping) {
    aci_state.aci_setup_info.services_pipe_type_mapping = &services_pipe_type_mapping[0];
  }
  else {
    aci_state.aci_setup_info.services_pipe_type_mapping = NULL;
  }
  aci_state.aci_setup_info.number_of_pipes    = NUMBER_OF_PIPES;
  aci_state.aci_setup_info.setup_msgs         = (hal_aci_data_t*)setup_msgs;
  aci_state.aci_setup_info.num_setup_msgs     = NB_SETUP_MESSAGES;

  // Tell the ACI library, the MCU to nRF8001 pin connections.
  // The Active pin is optional and can be marked UNUSED
  aci_state.aci_pins.board_name = REDBEARLAB_SHIELD_V2;
  aci_state.aci_pins.reqn_pin   = 6;
  aci_state.aci_pins.rdyn_pin   = 7;
  aci_state.aci_pins.mosi_pin   = MOSI;
  aci_state.aci_pins.miso_pin   = MISO;
  aci_state.aci_pins.sck_pin    = SCK;

  // SPI_CLOCK_DIV8  = 2MHz SPI speed
  // SPI_CLOCK_DIV16 = 1MHz SPI speed
  aci_state.aci_pins.spi_clock_divider      = SPI_CLOCK_DIV8;
  aci_state.aci_pins.reset_pin              = UNUSED; //4 for Nordic board, UNUSED for REDBEARLAB_SHIELD_V1_1
  aci_state.aci_pins.active_pin             = UNUSED;
  aci_state.aci_pins.optional_chip_sel_pin  = UNUSED;

  aci_state.aci_pins.interface_is_interrupt = false; //Interrupts still not available in Chipkit
  aci_state.aci_pins.interrupt_number       = 4;

  // We reset the nRF8001 here by toggling the RESET line connected to the nRF8001
  // If the RESET line is not available we call the ACI Radio Reset to soft reset the nRF8001
  // then we initialize the data structures required to setup the nRF8001
  // The second parameter is for turning debug printing on for the ACI Commands and Events so they be printed on the Serial
  lib_aci_init(&aci_state, false);
  Serial.println(F("lib_aci_init done"));
}

static bool rf_started = false;
static bool setup_required = false;

static void aci_loop() {
  uint8_t pipe_number;

  // We enter the if statement only when there is a ACI event available to be processed
  if (lib_aci_event_get(&aci_state, &aci_data)) {
    aci_evt_t * aci_evt;
    aci_evt = &aci_data.evt;

    switch(aci_evt->evt_opcode) {
    case ACI_EVT_DEVICE_STARTED:
      Serial.println(F("ACI_EVT_DEVICE_STARTED"));
      aci_state.data_credit_total = aci_evt->params.device_started.credit_available;

      Serial.print(F("aci_state.data_credit_total="));
      Serial.println(aci_state.data_credit_total, DEC);

      switch(aci_evt->params.device_started.device_mode) {
      case ACI_DEVICE_SETUP:
        Serial.println(F("ACI_DEVICE_SETUP"));
        rf_started = true;
        setup_required = true;
        break;

      case ACI_DEVICE_STANDBY:
        Serial.println(F("ACI_DEVICE_STANDBY"));
        if (aci_evt->params.device_started.hw_error) {
          delay(20); // Magic number used to make sure the HW error event is handled correctly.
        }
        else {
          lib_aci_connect(180/* in seconds */, 0x0050 /* advertising interval 50ms*/);
          Serial.println(F("Advertising started"));
        }
        break;
      case ACI_DEVICE_INVALID:
      case ACI_DEVICE_TEST:
      case ACI_DEVICE_SLEEP:
        // Ignored
        break;
      }
      break;

    case ACI_EVT_CMD_RSP:
      // Serial.println(F("ACI_EVT_CMD_RSP"));
      // Serial.print(F("aci_evt->params.cmd_rsp.cmd_opcode="));
      // Serial.println(aci_evt->params.cmd_rsp.cmd_opcode, HEX);
      // Serial.print(F("aci_evt->params.cmd_rsp.cmd_status="));
      // Serial.println(aci_evt->params.cmd_rsp.cmd_status, HEX);

      //If an ACI command response event comes with an error -> stop
      if (aci_evt->params.cmd_rsp.cmd_status != ACI_STATUS_SUCCESS) {
        //ACI ReadDynamicData and ACI WriteDynamicData will have status codes of
        //TRANSACTION_CONTINUE and TRANSACTION_COMPLETE
        //all other ACI commands will have status code of ACI_STATUS_SCUCCESS for a successful command//
        // Serial.print(F("ACI Command "));
        // Serial.println(aci_evt->params.cmd_rsp.cmd_opcode, HEX);
        // Serial.print(F("Evt Cmd respone: Status "));
        // Serial.println(aci_evt->params.cmd_rsp.cmd_status, HEX);
      }
      if (aci_evt->params.cmd_rsp.cmd_opcode == ACI_CMD_GET_DEVICE_VERSION) {
        //Store the version and configuration information of the nRF8001 in the Hardware Revision String Characteristic
        // lib_aci_set_local_data(&aci_state, PIPE_DEVICE_INFORMATION_HARDWARE_REVISION_STRING_SET,
        //   (uint8_t *)&(aci_evt->params.cmd_rsp.params.get_device_version),
        //   sizeof(aci_evt_cmd_rsp_params_get_device_version_t));
      }
      break;

    case ACI_EVT_CONNECTED:
      Serial.println(F("ACI_EVT_CONNECTED"));
      timing_change_done              = false;
      aci_state.data_credit_available = aci_state.data_credit_total;
      Serial.print(F("aci_state.data_credit_available="));
      Serial.println(aci_state.data_credit_available, DEC);

      // Get the device version of the nRF8001 and store it in the Hardware Revision String.
      // This will trigger a ACI_CMD_GET_DEVICE_VERSION.
      lib_aci_device_version();
      break;

    case ACI_EVT_PIPE_STATUS:
      Serial.println(F("ACI_EVT_PIPE_STATUS"));
      show_pipes();
      break;

    case ACI_EVT_TIMING:
      Serial.println(F("ACI_EVT_TIMING"));
      break;

    case ACI_EVT_DISCONNECTED:
      Serial.println(F("ACI_EVT_DISCONNECTED"));
      lib_aci_connect(180/* in seconds */, 0x0100 /* advertising interval 100ms*/);
      Serial.println(F("Advertising started"));
      break;

    case ACI_EVT_DATA_RECEIVED:
      pipe_number = aci_evt->params.data_received.rx_data.pipe_number;

      Serial.print(F("ACI_EVT_DATA_RECEIVED: pipe_number="));
      Serial.println(pipe_number, DEC);

      if (pipe_number == PIPE_FIKEN_STATUS_PANEL_GAUGE_DATA_RX_ACK) {
        if (aci_evt->len != PIPE_FIKEN_STATUS_PANEL_GAUGE_DATA_RX_ACK_MAX_SIZE) {
          break;
        }
        on_gauge_data(aci_evt->params.data_received.rx_data.aci_data, aci_evt->len - 2);
        lib_aci_send_ack(&aci_state, PIPE_FIKEN_STATUS_PANEL_GAUGE_DATA_RX_ACK);
      }

      if (pipe_number == PIPE_FIKEN_STATUS_PANEL_GAUGE_CONTROL_RX) {
        uint8_t len = aci_evt->len - 2;
        // if (aci_evt->len != PIPE_FIKEN_STATUS_PANEL_GAUGE_CONTROL_RX_MAX_SIZE) {
        //  break;
        // }

        on_gauge_ctrl(aci_evt->params.data_received.rx_data.aci_data, len);
      }
      break;

    case ACI_EVT_DATA_CREDIT:
      Serial.println(F("ACI_EVT_DATA_CREDIT"));
      aci_state.data_credit_available = aci_state.data_credit_available + aci_evt->params.data_credit.credit;
      Serial.print(F("aci_state.data_credit_available="));
      Serial.println(aci_state.data_credit_available, DEC);
      break;

    case ACI_EVT_PIPE_ERROR:
      Serial.println(F("ACI_EVT_PIPE_ERROR"));
      //See the appendix in the nRF8001 Product Specication for details on the error codes
      Serial.print(F("ACI Evt Pipe Error: Pipe #:"));
      Serial.print(aci_evt->params.pipe_error.pipe_number, DEC);
      Serial.print(F("  Pipe Error Code: 0x"));
      Serial.println(aci_evt->params.pipe_error.error_code, HEX);

      // Increment the credit available as the data packet was not sent.
      // The pipe error also represents the Attribute protocol Error Response sent from the peer and that should not be counted
      // for the credit.
      if (ACI_STATUS_ERROR_PEER_ATT_ERROR != aci_evt->params.pipe_error.error_code) {
        aci_state.data_credit_available++;
      }
      break;

    case ACI_EVT_HW_ERROR:
      Serial.println(F("ACI_EVT_HW_ERROR"));
      Serial.print(F("HW error: "));
      Serial.println(aci_evt->params.hw_error.line_num, DEC);

      for(uint8_t counter = 0; counter <= (aci_evt->len - 3); counter++) {
        Serial.write(aci_evt->params.hw_error.file_name[counter]); //uint8_t file_name[20];
      }
      Serial.println();
      lib_aci_connect(180/* in seconds */, 0x0050 /* advertising interval 50ms*/);
      Serial.println(F("Advertising started"));
      break;

    case ACI_EVT_INVALID:
      Serial.println(F("ACI_EVT_INVALID"));
      break;
    case ACI_EVT_ECHO:
      Serial.println(F("ACI_EVT_ECHO"));
      break;
    case ACI_EVT_BOND_STATUS:
      Serial.println(F("ACI_EVT_BOND_STATUS"));
      break;
    case ACI_EVT_DATA_ACK:
      Serial.println(F("ACI_EVT_DATA_ACK"));
      break;
    case ACI_EVT_DISPLAY_PASSKEY:
      Serial.println(F("ACI_EVT_DISPLAY_PASSKEY"));
      break;
    case ACI_EVT_KEY_REQUEST:
      Serial.println(F("ACI_EVT_KEY_REQUEST"));
      break;
    }
  }
  else {
    // Serial.println(F("No ACI Events available"));
    // No event in the ACI Event queue and if there is no event in the ACI command queue the arduino can go to sleep
    // Arduino can go to sleep now
    // Wakeup from sleep from the RDYN line
  }

  /* setup_required is set to true when the device starts up and enters setup mode.
   * It indicates that do_aci_setup() should be called. The flag should be cleared if
   * do_aci_setup() returns ACI_STATUS_TRANSACTION_COMPLETE.
   */
  if (setup_required) {
    int ret = do_aci_setup(&aci_state);
    // Serial.print(F("ret="));
    // Serial.println(ret, DEC);
    if (SETUP_SUCCESS == ret) {
      setup_required = false;
    }
  }
}

static uint8_t value = 0;
void loop() {
  static unsigned long last = 0, now;

  aci_loop();

  if (Serial.available()) {
    Serial.write(Serial.read());
  }

  now = millis();
  if (now - last > 3000) {
    last = now;

    if (!rf_started) {
      static int count = 0;
      count++;
      /*
      if(count == 3) {
        Serial.println(F("RF did not start, resetting RF"));

        // asm volatile ("jmp 0");
        // lib_aci_pin_reset();
        setup_rf();
        count = 0;
        return;
      } else {
        Serial.println(F("waiting for RF to start"));
      }
      */
    }
    else if (!setup_required) {
      value++;
      lib_aci_set_local_data(&aci_state, PIPE_FIKEN_STATUS_PANEL_GAUGE_DATA_SET, &value, 1);
      Serial.print(F("value="));
      Serial.println(value, HEX);

      // show_pipes();
    }
  }
}

void show_pipes() {
  for (uint8_t i = 1; i <= NUMBER_OF_PIPES; i++) {
    uint8_t x = lib_aci_is_pipe_available(&aci_state, i);
    Serial.print(F("pipe #"));
    Serial.print(i, DEC);
    Serial.print(F(", available=?"));
    Serial.println(x, DEC);
  }
}

bool send_ctrl(fsp_res *res) {
  uint8_t *data = (uint8_t *)res;
  uint8_t len = 2 + res->len;
  bool status = false;

  Serial.print(F("send_ctrl, len="));
  Serial.println(len, DEC);
  Serial.print(F("aci_state.data_credit_available="));
  Serial.println(aci_state.data_credit_available, DEC);
  Serial.print(F("available="));
  Serial.println(lib_aci_is_pipe_available(&aci_state, PIPE_FIKEN_STATUS_PANEL_GAUGE_CONTROL_TX), DEC);

  if (lib_aci_is_pipe_available(&aci_state, PIPE_FIKEN_STATUS_PANEL_GAUGE_CONTROL_TX) &&
      aci_state.data_credit_available >= 1) {
    status = lib_aci_send_data(PIPE_FIKEN_STATUS_PANEL_GAUGE_CONTROL_TX, data, len);
    if (status) {
      aci_state.data_credit_available--;
    }
  }
  return status;
}