#include <atmel_start.h>

#include <utility_logging.hpp>
#include <utility_assert.hpp>

#include "radio_hw_instance.hpp"
#include "app_logging.hpp"
#include "skullc_samd21_hal.hpp"

namespace Hal = Peripherals::Hal::Samd;

namespace
{

[[noreturn]] void m_faultHandler(const char* expression, const char* file, const int line)
{
  SKULLC_LOG_FATAL("Expression failed: (%s), source: %s:%d", expression, file, line);

  __asm__("BKPT");

  while (true);
}

}

int main()
{
	/* Initializes MCU, drivers and middleware */
	atmel_start_init();

  Utility::Assert::setHandler(m_faultHandler);

  gpio_set_pin_level(OUT_LED_TX, false);

  App::Logging::setup();

  radio::HwInstance* radio_hw = radio::HwInstance::instance();
  SKULLC_LOG_DEBUG("Begin.");

	/* Replace with your application code */
	while (true)
  {
    gpio_toggle_pin_level(OUT_LED_RX);

    const int16_t radio_num = radio_hw->register_read(radio::Registers::PART_NUM);
    SKULLC_LOG_INFO("Reg 0x1C: %d", radio_num);

    gpio_toggle_pin_level(OUT_LED_TX);

    int16_t radio_status = radio_hw->register_read(radio::Registers::TRX_STATUS);
    radio_status &= 0x1F;

    SKULLC_LOG_INFO("Status: %d", radio_status);

    static int transitioned = 0;
    if (radio_status == 0x08 && transitioned == 0)
    {
      SKULLC_LOG_DEBUG("Transitioning...");
      radio_hw->set_current_state(radio::HwInstance::States::PLL_ON);
      transitioned = 1;
    }
    else if (radio_status == 0x09 && transitioned == 1)
    {
      SKULLC_LOG_DEBUG("Transitioning 2...");
      radio_hw->set_current_state(radio::HwInstance::States::RX_ON);
      transitioned = 2;
    }
    else if (radio_status == 0x09 && transitioned == 1)
    {
      SKULLC_LOG_DEBUG("Transitioning 2...");
      radio_hw->set_current_state(radio::HwInstance::States::RX_ON);
      transitioned = 2;
    }

    delay_ms(1000);
	}
}