#include <LowPower.h>
#include <EEPROM.h>
#include <SPI.h>
#include <lib_aci.h>
#include <aci_setup.h>
#include <aci_evts.h>

// See config.h on how to configure the sketch
#include "config.h"
#include "services.h"
#include "app.h"

static void setup_rf();
static void show_pipes();

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

static const uint8_t sm_pipe_set = PIPE_SOIL_MOISTURE_SOIL_MOISTURE_CONTROL_SET;
static const uint8_t sm_pipe_tx = PIPE_SOIL_MOISTURE_SOIL_MOISTURE_CONTROL_TX;
static const uint8_t sm_pipe_rx = PIPE_SOIL_MOISTURE_SOIL_MOISTURE_CONTROL_RX_ACK_AUTO;

static struct aci_state_t aci_state;
static hal_aci_evt_t aci_data;
static boolean 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);
}

#if defined(__AVR_ATmega32U4__)
static void go_to_sleep() {
  LowPower.idle(SLEEP_1S, ADC_OFF, TIMER4_OFF, TIMER3_OFF, TIMER1_OFF, TIMER0_OFF, SPI_OFF, USART1_OFF, TWI_OFF, USB_OFF);
}
#else
#warning No sleep support for current CPU architecture.
static void go_to_sleep() {
}
#endif

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);

#if WAIT_FOR_SERIAL_BEFORE_STARING == 1
  // 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

#endif // WAIT_FOR_SERIAL_BEFORE_STARING

  setup_rf();
  sm_setup();
}

static 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.board_name = REDBEARLAB_SHIELD_V1_1;
  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;
  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;
//  int ret;

  // 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*/);
          // lib_aci_broadcast(10/* in seconds */, 0x0100 /* advertising interval 100ms */);
//          ret = lib_aci_open_adv_pipe(PIPE_BATTERY_BATTERY_LEVEL_BROADCAST);
//          ret = lib_aci_open_adv_pipe(PIPE_SOIL_MOISTURE_SOIL_MOISTURE_LEVEL_BROADCAST);
          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));
      }
      if (aci_evt->params.cmd_rsp.cmd_opcode == ACI_CMD_GET_TEMPERATURE) {
        Serial.print("aci_evt->params.cmd_rsp.params.get_temperature=");
        Serial.print(aci_evt->params.cmd_rsp.params.get_temperature.temperature_value, DEC);
        Serial.println();

        int32_t t = aci_evt->params.cmd_rsp.params.get_temperature.temperature_value;

        // t is in 1/4 degrees celcius parts.

        // Multiply t by 25 without having to include float support
        t = t * 16 + t * 8 + t;

        uint32_t exponent = 2 << 24;

        // example. reading=111. real temperature = 111 / 4 = 27.75 dec C
        // Calculation: t = 2775, exp = 2. formula: 10^exp * 0.t => 10^2 * 0.2775 = 27.75

        // The value of flags
        // bit 0: 0=Celcius, 1=Farenheight
        // bit 1: 0=time stamp field not present, 1=present
        // bit 2: 0=temperature type field not present, 1=present
        // bit 3-7: reserved
        // Reference: https://developer.bluetooth.org/gatt/characteristics/Pages/CharacteristicViewer.aspx?u=org.bluetooth.characteristic.temperature_measurement.xml
        struct {
          uint8_t flags;
          uint32_t value;
          // time stamp
          // temperature type
        } temperature_measurement = {
                0,
                exponent | (t & 0x00FFFFFF)
        };

        lib_aci_set_local_data(&aci_state, PIPE_SOIL_MOISTURE_INTERMEDIATE_TEMPERATURE_SET,
                               (uint8_t *) &temperature_measurement,
                               sizeof(temperature_measurement));
      }
      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();

      lib_aci_get_temperature();

      sm_on_connect();
      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"));

      sm_on_disconnect();
      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 == sm_pipe_rx) {
        on_soil_moisture_ctrl(aci_evt->params.data_received.rx_data.aci_data, aci_evt->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("do_aci_setup 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;
      static bool reset_attempted = false;
      count++;

      if (!reset_attempted) {
        if (count == 3) {
          reset_attempted = true;
          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++;
    }
  }

  sm_loop();

#ifdef USE_LOW_POWER_MODE == 1
#ifdef SM_DEBUG == 1
  Serial.println(F("Sleeping..."));
  Serial.flush();
#endif // SM_DEBUG

  go_to_sleep();
#endif // USE_LOW_POWER_MODE
}

static 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);
  }
}
/*
boolean tx_moisture(sm_res *res) {
  static const uint8_t pipe = PIPE_SOIL_MOISTURE_SOIL_MOISTURE_LEVEL_TX;
  uint8_t *data = (uint8_t *)res;
  uint8_t len = 2 + res->len;
  boolean status = false;

  boolean available = lib_aci_is_pipe_available(&aci_state, pipe);

  Serial.print(F("tx_soil_moisture, 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(available, DEC);

  if (available && aci_state.data_credit_available > 0) {
    status = lib_aci_send_data(pipe, data, len);
    if (status) {
      aci_state.data_credit_available--;
    }
  }
  return status;
}
*/
void notify_battery_level(uint8_t value) {
  static const uint8_t pipe = PIPE_BATTERY_BATTERY_LEVEL_SET;

  Serial.print(F("notify_battery_level, value="));
  Serial.println(value, DEC);

  value = value % 101;

  lib_aci_send_data(pipe, &value, 1);
}

void notify_soil_moisture(const struct sm_res& res, uint8_t body_len) {

  uint8_t *data = (uint8_t *)&res;
  uint8_t len = SM_RES_HEADER_SIZE + body_len;

//  Serial.print(F("notify_soil_moisture, code="));
//  Serial.print(res.code, DEC);
//  Serial.print(F(", body_len="));
//  Serial.println(body_len, DEC);

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

  bool available = lib_aci_is_pipe_available(&aci_state, sm_pipe_tx);
//  Serial.print(F("pipe available="));
//  Serial.println(available, DEC);

  // This should probably be an explicit part of the API, but for now it makes it easier to implement a synchronous interface.
  lib_aci_set_local_data(&aci_state, sm_pipe_set, (uint8_t *)&res, len);

  // There is no need to lod messages that won't be sent.
  if (!available) {
    return;
  }

#if SM_DEBUG == 1
Serial.println("write_res");
  write_res(res);
#endif

  if (aci_state.data_credit_available = 0) {
#if SM_DEBUG == 1
    Serial.println("Not enough credits to send notification.");
#endif
    return;
  }

  boolean sent = lib_aci_send_data(pipe_tx, data, len);
  if (sent) {
    aci_state.data_credit_available--;
  } else {
#if SM_DEBUG == 1
    Serial.println("Sending failed");
#endif
  }
}