#include "bbnow.hpp"
#include "freertos/FreeRTOS.h"
#include <stdio.h>
#include <inttypes.h>
#include "esp_wifi_types_generic.h"
#include "freertos/projdefs.h"
#include "sdkconfig.h"
#include "freertos/task.h"
#include "esp_chip_info.h"
#include "esp_flash.h"
#include "esp_system.h"
#include "blk_box.h"
#include "blk_box_drivers/i2c.h"
#include "blk_box_drivers/inputs.hpp"
#include "blk_box_drivers/leds.hpp"
#include "blk_box_drivers/char_lcd.hpp"
#include "blk_box_drivers/ssegs.hpp"
#include "blk_box_drivers/helpers.hpp"


#include "esp_log.h"
#include "esp_event.h"
#include "esp_netif.h"
#include "esp_wifi.h"
#include "esp_log.h"
#include "esp_mac.h"
#include "esp_now.h"
#include "esp_crc.h"
#include "nvs_flash.h"


#define ESPNOW_CHANNEL 6
#define ESPNOW_DATA_MAX_LEN 24

const static char TAG[] = "main";


const static uint8_t ESPNOW_PMK[16] = {0x4F, 0xA2, 0x8E, 0x11, 0xD7, 0x6C, 0xB5, 0x3E, 0x94, 0xF0, 0x2A, 0x83, 0xC6, 0x9D, 0x1B, 0x5E};

const static uint8_t BROADCAST_MAC[ESP_NOW_ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };

static void example_espnow_send_cb(const esp_now_send_info_t *tx_info, esp_now_send_status_t status);
static void example_espnow_recv_cb(const esp_now_recv_info_t *recv_info, const uint8_t *data, int len);


void init_nvs(void) {
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK( nvs_flash_erase() );
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK( ret );
}

void init_wifi(void)
{
    ESP_ERROR_CHECK(esp_netif_init());
    ESP_ERROR_CHECK(esp_event_loop_create_default());
    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK( esp_wifi_init(&cfg) );
    ESP_ERROR_CHECK( esp_wifi_set_storage(WIFI_STORAGE_RAM) );
    ESP_ERROR_CHECK( esp_wifi_set_mode(WIFI_MODE_AP) );
    ESP_ERROR_CHECK( esp_wifi_start());
    ESP_ERROR_CHECK( esp_wifi_set_channel(ESPNOW_CHANNEL, WIFI_SECOND_CHAN_NONE));

    // enable long range
    // ESP_ERROR_CHECK( esp_wifi_set_protocol(ESPNOW_WIFI_IF, WIFI_PROTOCOL_11B|WIFI_PROTOCOL_11G|WIFI_PROTOCOL_11N|WIFI_PROTOCOL_LR) );
}

void init_espnow(void) {
    ESP_ERROR_CHECK( esp_now_init() );
    ESP_ERROR_CHECK( esp_now_register_send_cb(example_espnow_send_cb) );
    ESP_ERROR_CHECK( esp_now_register_recv_cb(example_espnow_recv_cb) );

    /* Set primary master key. */
    ESP_ERROR_CHECK( esp_now_set_pmk(ESPNOW_PMK));

    // Add broadcast peer information to peer list, allowing us to send to it
    esp_now_peer_info_t peer = {0};
    peer.channel = ESPNOW_CHANNEL;
    peer.ifidx = WIFI_IF_AP;
    peer.encrypt = false;
    memcpy(peer.peer_addr, BROADCAST_MAC, ESP_NOW_ETH_ALEN);
    ESP_ERROR_CHECK( esp_now_add_peer(&peer) );
}

/* ESPNOW sending or receiving callback function is called in WiFi task.
 * Users should not do lengthy operations from this task. Instead, post
 * necessary data to a queue and handle it from a lower priority task. */
static void example_espnow_send_cb(const esp_now_send_info_t *tx_info, esp_now_send_status_t status)
{
    if (tx_info == NULL) {
        ESP_LOGE(TAG, "Send cb arg error");
        return;
    }
    ESP_LOGI(TAG, "sending espnow packet of size: %d to " MACSTR, tx_info->data_len, MAC2STR(tx_info->des_addr));
}

static void example_espnow_recv_cb(const esp_now_recv_info_t *recv_info, const uint8_t *data, int len)
{
    uint8_t * src_addr = recv_info->src_addr;
    uint8_t * des_addr = recv_info->des_addr;

    if (src_addr == NULL || data == NULL || len <= 0) {
        ESP_LOGE(TAG, "Receive cb arg error");
        return;
    }

    ESP_LOGI(TAG, "got espnow packet of size: %d from " MACSTR " to " MACSTR, MAC2STR(src_addr), MAC2STR(des_addr));

    // if (IS_BROADCAST_ADDR(des_addr)) {
    //     /* If added a peer with encryption before, the receive packets may be
    //      * encrypted as peer-to-peer message or unencrypted over the broadcast channel.
    //      * Users can check the destination address to distinguish it.
    //      */
    //     ESP_LOGD(TAG, "Receive broadcast ESPNOW data");
    // } else {
    //     ESP_LOGD(TAG, "Receive unicast ESPNOW data");
    // }
}

extern "C" void app_main(void) {
    BlkBoxInitConfig blk_box_cfg = {};
    init_blk_box(blk_box_cfg);

    LCDController::set_backlight(true);

    lcd_do_splash();

    LCDController::set_resting_cursor_mode(CursorMode::Show);
    LCDController::set_resting_cursor_pos(0, 0);

    SSegController::enable_game_timer();
    SSegController::enable_module_timer();

    ESP_LOGI(TAG, "initializing nvs");
    init_nvs();
    ESP_LOGI(TAG, "initializing wifi");
    init_wifi();
    ESP_LOGI(TAG, "initializing espnow");
    init_espnow();
    ESP_LOGI(TAG, "initialized!");

    // uint8_t mac[6];
    // ESP_ERROR_CHECK(esp_wifi_get_mac(WIFI_IF_STA, mac));
    // ESP_LOGI(TAG, "My MAC: %02X:%02X:%02X:%02X:%02X:%02X",
    //          mac[0], mac[1], mac[2],
    //          mac[3], mac[4], mac[5]);

    // BBNowPubPacket tens_discovery_packet = BBNowPubPacket::new_discovery(BlkBoxNowPubDiscoveryData {
    //     .expantion_type = BlkBoxExpantion::TENS,
    // });
    // while (1) {
    //     auto buf = tens_discovery_packet.serialize();
    //     ESP_LOGI(TAG, "sending packet of size %d from main", buf.size());
    //     ESP_ERROR_CHECK(esp_now_send(BROADCAST_MAC, buf.data(), buf.size()));
    //     vTaskDelay(pdMS_TO_TICKS(1000));
    // }

    uint32_t i = 0;

    while (1) {
        if (InputsController::has_button_press()) {
            Button b = InputsController::wait_button_press();
            printf("Button pressed: %d\n", raw_value(b));
            if (b == Button::GREEN) {
                LEDController::set_indicator(static_cast<IndicatorLED>(i + LED_SHAPE_COUNT), LEDColor::GREEN);
                LEDController::flush();
            } else if (b == Button::YELLOW) {
                LEDController::set_indicator(static_cast<IndicatorLED>(i + LED_SHAPE_COUNT), LEDColor::YELLOW);
                LEDController::flush();
            } else if (b == Button::RED) {
                i = (i + 1) % LED_INDICATOR_COUNT;
                LCDController::set_resting_cursor_pos(0, i % 20);
            } else if (b == Button::BLUE) {
                i = (i + LED_INDICATOR_COUNT - 1) % LED_INDICATOR_COUNT;
                LCDController::set_resting_cursor_pos(0, i % 20);
            }
        }

        if (InputsController::has_switch_flip()) {
            SwitchFlip flip = InputsController::wait_switch_flip();
            printf("Switch flipped: %d, new state: %d\n", raw_value(flip.get_switch()), flip.is_up());
            if (flip.get_switch() == Switch::S1) {
                SSegController::set_game_time(SSegController::get_game_time() + 10000);
            }
            if (flip.get_switch() == Switch::S2) {
                SSegController::set_module_time(SSegController::get_module_time() + 10000);
            }
            if (flip.get_switch() == Switch::S3) {
                SSegController::start_game_timer();
            }
            if (flip.get_switch() == Switch::S4) {
                if (flip.is_up()) {
                    SSegController::start_module_timer();
                } else {
                    SSegController::stop_module_timer();
                }
            }
        }

        if (InputsController::has_keypad_press()) {
            KeypadKey k = InputsController::wait_keypad_press();
            printf("Keypad key pressed: %c\n", keypad_key_to_char(k));
            LEDController::flush();
        }

        vTaskDelay(10 / portTICK_PERIOD_MS);
    }

}