#include "kartSender.hpp"

#include <esp_now.h>
#include <esp_wifi.h>

#include "Arduino.h"
#include "Config.hpp"
#include "WiFi.h"
#include "kartMessage.hpp"
bool tp_state = false;
KartSender kart_sender;
// callback when data is sent
void OnDataSent(const esp_now_send_info_t *tx_info, esp_now_send_status_t status) {
    if (status == ESP_NOW_SEND_SUCCESS) {
        // esp_now_deinit();
    }
    // Serial.print("\r\nLast Packet Send Status:\t");
    // Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}

esp_now_peer_info_t peerInfo;
void KartSender::Send() {
    tp_state = !tp_state;
    digitalWrite(21, tp_state);

    kart_msg myData;

    myData.kart_id = config.kart_id;
    myData.transmision = transmission_counter++;

    // Send message via ESP-NOW
    esp_err_t result = esp_now_send(config.trackside_mac, (uint8_t *)&myData, sizeof(myData));

    if (result == ESP_OK) {
        Serial.println("Sent with success");
    } else {
        Serial.println("Error sending the data");
    }
}

void KartSender::Init() {
    WiFi.mode(WIFI_STA);

    // Init ESP-NOW
    if (esp_now_init() != ESP_OK) {
        Serial.println("Error initializing ESP-NOW");
        return;
    }

    // Once ESPNow is successfully Init, we will register for Send CB to
    // get the status of Transmitted packet
    esp_now_register_send_cb(OnDataSent);

    // Register peer
    memcpy(peerInfo.peer_addr, config.trackside_mac, 6);
    peerInfo.channel = 0;
    peerInfo.encrypt = false;

    // Add peer
    if (esp_now_add_peer(&peerInfo) != ESP_OK) {
        Serial.println("Failed to add peer");
        return;
    }
}