//
// Created by erki on 4.06.22.
//

#include "radio_hw_instance.hpp"

#include "radio_hardware_instance.h"
#include "radio_gpio.h"
#include "radio_spi.h"
#include "utility_logging.hpp"

#include <hal_delay.h>
#include <hal_ext_irq.h>
#include <utility_assert.hpp>
#include <utility_staticpointer.hpp>
#include <optional>

namespace
{

Utility::StaticPointer<radio::HwInstance> _INSTANCE;

void _irq_handler()
{
  SKULLC_ASSERT_DEBUG(_INSTANCE.isInitialized());
  _INSTANCE->irq_handler();
}

}

namespace radio
{

HwInstance* HwInstance::instance()
{
  if (!_INSTANCE.isInitialized())
  {
    _INSTANCE.setup();
  }

  return _INSTANCE.get();
}

HwInstance::HwInstance()
{
  radio_gpio_init();

  m_spi = radio_spi_init();
  spi_m_sync_get_io_descriptor(m_spi, &m_spi_io);
  spi_m_sync_enable(m_spi);

  gpio_set_pin_level(OUT_RADIO_RST, false);
  gpio_set_pin_level(OUT_RADIO_SLP_TR, false);
  delay_ms(10);

  ext_irq_enable(IN_RADIO_IRQ);
  ext_irq_register(IN_RADIO_IRQ, _irq_handler);
  ext_irq_register(0, _irq_handler);

  gpio_set_pin_level(OUT_RADIO_RST, true);
  delay_ms(10);

  // Enable safe mode for TX.
  register_write(Registers::TRX_CTRL_2, 0x80);

  // Disable external clock output.
  std::uint8_t trx_ctrl = register_read(Registers::TRX_CTRL_0);
  trx_ctrl &= ~(0x07);
  trx_ctrl &= ~(0x08);
  register_write(Registers::TRX_CTRL_0, trx_ctrl);

  // Enable interrupts.
  register_write(Registers::IRQ_MASK, 0x08);

  // clear interrupts.
  register_read(Registers::IRQ_STATUS);

  set_current_state(States::TRX_OFF);
}

void HwInstance::irq_handler()
{
  SKULLC_LOG_DEBUG("IRQ set.");
}

uint8_t HwInstance::register_read(const Registers& address)
{
  const uint8_t address_to_write = (uint8_t(address) & 0x3F) | (1 << 7);
  gpio_set_pin_level(OUT_RADIO_CS, false);

  delay_us(1);

  io_write(m_spi_io, &address_to_write, 1);
  delay_us(1);
  uint8_t data = 0xFF;
  io_read(m_spi_io, &data, 1);

  gpio_set_pin_level(OUT_RADIO_CS, true);

  return data;
}

void HwInstance::register_write(const Registers& address, const uint8_t value)
{
  uint8_t data[2] = {
    uint8_t((uint8_t(address) & 0x3F) | (1 << 7) | (1 << 6)),
    uint8_t(value & 0xEF)
  };

  gpio_set_pin_level(OUT_RADIO_CS, false);
  io_write(m_spi_io, data, 2);

  gpio_set_pin_level(OUT_RADIO_CS, true);
}

bool HwInstance::set_current_state(const States& new_state)
{
  if (new_state == m_current_state)
    return true;

  m_wait_transition_complete();
  bool can_transition = false;
  std::optional<std::uint8_t> trx_state_value = std::nullopt;

  switch (m_current_state)
  {
  case States::P_ON:
    if (new_state == States::TRX_OFF)
    {
      can_transition = true;
      trx_state_value = 0x08;
    }
    break;
  case States::TRX_OFF:
    if (new_state == States::RX_ON)
    {
      can_transition = true;
      trx_state_value = 0x06;
    }
    else if (new_state == States::PLL_ON)
    {
      can_transition = true;
      trx_state_value = 0x09;
    }
    break;
  case States::RX_ON:
    if (new_state == States::PLL_ON)
    {
      can_transition = true;
      trx_state_value = 0x09;
    }
    else if (new_state == States::TRX_OFF)
    {
      can_transition = true;
      trx_state_value = 0x08;
    }
    break;
  case States::PLL_ON:
    if (new_state == States::RX_ON)
    {
      can_transition = true;
      trx_state_value = 0x06;
    }
    else if (new_state == States::TRX_OFF)
    {
      can_transition = true;
      trx_state_value = 0x08;
    }
    break;
  default:
    SKULLC_ASSERT_DEBUG(false);
    return false;
  }

  if (can_transition)
  {
    register_write(Registers::TRX_STATE, *trx_state_value);
    m_wait_transition_complete();
    m_current_state = new_state;
    return true;
  }
  else
  {
    return false;
  }
}

void HwInstance::m_wait_transition_complete()
{
  while (true)
  {
    int16_t radio_status = register_read(radio::Registers::TRX_STATUS);
    radio_status &= 0x1F;

    if (radio_status != 0x1F)
      return;
  }
}

}