#include "ESPNow.hpp"

static const char *TAG = "ESPNOW";

uint8_t BROADCAST_MAC[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
esp_now_peer_info_t hostInfo;
Preferences preferences;
bool msg_recv = false;

bool was_msg_received() {
    if (msg_recv) {
        msg_recv = false;
        return true;
    }
    return false;
}

void get_host_mac(uint8_t *destination) {
    RtcMemory::getInstance().get_host_mac(destination);
}

esp_err_t add_host_to_peers(response received) {
    esp_now_peer_info_t host;
    memset(&host, 0, sizeof(host));
    memcpy(host.peer_addr, received.mac, sizeof(received.mac));
    host.encrypt = false;
    host.channel = 0;
    return esp_now_add_peer(&host);
}

void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status) {
    ESP_LOGE(TAG, "Message sent to %02X:%02X:%02X:%02X:%02X:%02X", mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3],
             mac_addr[4], mac_addr[5]);
}

void on_data_recv(const uint8_t *mac, const uint8_t *incomingData, int len) {
    // is msg host -> yes -> set bool
    // assume host change not happening, rare event
    // => on host change, broadcast
    //	ESP_LOGD(TAG, "Message received");
    // time this
    auto start = millis();
    response received_msg;
    memcpy(&received_msg, incomingData,
           sizeof(received_msg)); // TODO: check for valid mac
    // all the esp32 macs so far use the same first 3(?) bytes so maybe use that
    // First three bytes of MACs are always vendor specific. But vendors have a range of them.
    // all Espressif registered MAC starting bytes: https://maclookup.app/vendors/espressif-inc
    // you can also set your own MAC https://randomnerdtutorials.com/get-change-esp32-esp8266-mac-address-arduino/
    switch (received_msg.type) {
        case hostChange: {
            ESP_LOGI(TAG, "hostChange received");
            Time::getInstance().setTime(received_msg.time);
            // delete old host
            preferences.begin("config", false);
            if (preferences.isKey("host")) {
                ESP_LOGI(TAG, "removing old host");
                uint8_t old[6];
                preferences.end();
                get_host_mac(old); // maybe problem here, re-opening preferences
                esp_now_del_peer(old);
            }
            // add new host
            preferences.begin("config", false);
            if (preferences.putBytes("host", received_msg.mac, sizeof(received_msg.mac)) > 0) {
                ESP_LOGI(TAG, "Host MAC address saved to flash %02X:%02X:%02X:%02X:%02X:%02X", received_msg.mac[0],
                         received_msg.mac[1], received_msg.mac[2], received_msg.mac[3], received_msg.mac[4],
                         received_msg.mac[5]);
            } else {
                ESP_LOGI(TAG, "Couldn't save Host Mac to flash");
            }
            preferences.end();
            add_host_to_peers(received_msg);
        }
        case dataAck: {
            //			ESP_LOGI(TAG, "dataAck received.");
            Time::getInstance().setTime(
                    received_msg.time); // see https://www.esp32.com/viewtopic.php?t=9965, maybe this needs an offset

            //			ESP_LOGI(TAG, "Timestamp received: %ld", Time::getInstance().getEpochSeconds());
            preferences.begin("config", false);
            if (!preferences.isKey("host")) {
                if (preferences.putBytes("host", received_msg.mac, sizeof(received_msg.mac)) > 0) {
                    //			ESP_LOGI(TAG, "host MAC address saved to flash %02X:%02X:%02X:%02X:%02X:%02X",
                    // received_msg.host[0],
                    //			received_msg.host[1],received_msg.host[2],received_msg.host[3],received_msg.host[4],received_msg.host[5]);
                    //				}

                    // add host to peers
                    add_host_to_peers(received_msg);
                }
            }
            preferences.end();
            // delay(50);
            msg_recv = true;
        }
        default: {
            break;
        }
    }
    auto end = millis();
}

esp_err_t espnow_setup() {
    esp_err_t result;
    WiFi.mode(WIFI_STA);
    result = esp_now_init();
    if (result != ESP_OK) {
        // initialization failed
        ESP_LOGE(TAG, "ESPNow setup failed");
        return result;
    }

    get_host_mac(hostInfo.peer_addr); // check if there is a host saved in flash mem, broadcast otherwise

    hostInfo.channel = 0;

    // TODO: PMK is used to encrypt LMK with the AES-128 algorithm. Call esp_now_set_pmk() to set PMK. If PMK is not
    // set, a default PMK will be used.
    // https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/network/esp_now.html
    hostInfo.encrypt = false;
    esp_now_add_peer(&hostInfo);

    esp_now_register_recv_cb(on_data_recv);
    esp_now_register_send_cb(on_data_sent);

    ESP_LOGI(TAG, "ESPNow started. MAC: %s", WiFi.macAddress().c_str());
    return ESP_OK;
}