#include "EspNowSerial.h"

#if SOC_WIFI_SUPPORTED || CONFIG_ESP_HOSTED_ENABLED


EspNowSerial* EspNowSerial::_instance = nullptr;

EspNowSerial::EspNowSerial(wifi_mode_t mode)
: _wifi_mode{mode}, _wifi_if{mode == WIFI_MODE_AP ? WIFI_IF_AP : WIFI_IF_STA} 
{

}

esp_err_t EspNowSerial::add_peer(EspMacAddress peer_address)
{
    if (_broadcast == true)
        return -1;

    esp_now_peer_info_t peer_info;

    memcpy(peer_info.peer_addr, peer_address, ESP_NOW_ETH_ALEN);
    peer_info.channel = 0;      // current channel  
    peer_info.encrypt = false;
    peer_info.ifidx = _wifi_if;
    memset(peer_info.lmk, 0, ESP_NOW_KEY_LEN);
    peer_info.priv = nullptr;

    return esp_now_add_peer(&peer_info);
}

esp_err_t EspNowSerial::add_peer_encrypted(EspMacAddress peer_address, CcmpMasterKey lmk)
{
    if (_broadcast == true || _has_pmk == false)
        return -1;

    esp_now_peer_info_t peer_info;

    memcpy(peer_info.peer_addr, peer_address, ESP_NOW_ETH_ALEN);
    peer_info.channel = 0;      // current channel
    peer_info.encrypt = true;
    peer_info.ifidx = _wifi_if;
    memcpy(peer_info.lmk, lmk, ESP_NOW_KEY_LEN);
    peer_info.priv = nullptr;

    return esp_now_add_peer(&peer_info);
}

esp_err_t EspNowSerial::begin(uint8_t channel, bool broadcast, CcmpMasterKey pmk)
{
    _broadcast = broadcast;                                             // set broadcast flag
    EspNowSerial::_instance = this;                                     // attach global instance as this object
    _rx_queue.init();                                                    // init rx buffer
    _data_sent = 1;                                                     // reset data sent flag

    WiFi.mode(_wifi_mode);                                              // set device as a Wi-Fi Station
    esp_wifi_set_channel(channel, WIFI_SECOND_CHAN_NONE);               // force WiFi channel
                                            
    esp_err_t status = esp_now_init();                                  // init esp-now

    if (status == ESP_OK)
    {      
        esp_now_register_send_cb(_on_data_sent);                        // register callbacks
        esp_now_register_recv_cb(_on_data_recv);
        
        if (_broadcast == false && pmk != nullptr)                      // set pmk if encrypted (can't encrypted broadcast)
        {            
            _has_pmk = true;  
            esp_now_set_pmk(pmk);
        }
    
        if (_broadcast == broadcast)                                    // set broadcast peer if needed
        {
            esp_now_peer_info_t broadcast_peer;

            memset(broadcast_peer.peer_addr, 0xFF, ESP_NOW_ETH_ALEN);
            broadcast_peer.channel = 0;  
            broadcast_peer.encrypt = false;
            broadcast_peer.ifidx = _wifi_if;
            memset(broadcast_peer.lmk, 0, ESP_NOW_KEY_LEN);
            broadcast_peer.priv = nullptr;

            status = esp_now_add_peer(&broadcast_peer);
        }
    }

    return status;
}

void EspNowSerial::end()
{
    esp_now_deinit();       
    WiFi.mode(WIFI_OFF); 
}

int EspNowSerial::available()
{
    return _rx_queue.size();
}

int EspNowSerial::read()
{
    return _rx_queue.read_byte();
}

size_t EspNowSerial::readBytes(uint8_t *buffer, size_t length)
{
    return _rx_queue.read_data(buffer, length);
}

int EspNowSerial::peek()
{
    return _rx_queue.peek();
}

size_t EspNowSerial::write(uint8_t value)
{ 
    if (!_data_sent)
        return 0;
   
   uint8_t buffer[1] = {value};
   esp_err_t result = esp_now_send(nullptr, buffer, 1);

    if (result != ESP_OK)
        return 0;
    
    return 1;
};

size_t EspNowSerial::write(const uint8_t *buffer, size_t size)
{
    if (!_data_sent)
        return 0;
    
    _data_sent = 0;     // reset data sent flag
    esp_err_t result = esp_now_send(nullptr, buffer, size);

    if (result != ESP_OK)
        return 0;
    
    return size;
}

#if !defined(ARDUINO_CORE_3X)

void EspNowSerial::_on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status) 
{
    EspNowSerial::_instance->_data_sent = 1;
}

void EspNowSerial::_on_data_recv(const uint8_t *mac_addr, const uint8_t *data, int len) 
{
    EspNowSerial::_instance->_rx_queue.write_data((uint8_t*)data, len);
}

#else // ARDUINO_CORE_3X

void EspNowSerial::_on_data_sent(const esp_now_send_info_t *tx_info, esp_now_send_status_t status)
{
    EspNowSerial::_instance->_data_sent = 1;
}


void EspNowSerial::_on_data_recv(const esp_now_recv_info_t * esp_now_info, const uint8_t *data, int data_len)
{
    EspNowSerial::_instance->_rx_queue.write_data((uint8_t*)data, data_len);
}


#endif /* SOC_WIFI_SUPPORTED */


#endif // ARDUINO_CORE_3X