/**
* @file Mesh_Network.c
* @brief Mesh network layer used by OTA and APP
* @author Hendrik Schutter, init based in ESP32-IDE code
* @date 20.01.2021
*
* Additional Infos: Start network and send and receive data.
*/

#include "Mesh_Network.h"

static const char *LOG_TAG = "mesh_network";

//w: errMeshNetworkInitialize
//r: errMeshNetworkInitialize;vMeshNetworkGetOwnAddr;errMeshNetworkGetChildren
uint8_t u8ownMAC[6];

//w: errMeshNetworkInitialize; vMeshNetworkMeshEventHandler
//r: vMeshNetworkMeshEventHandler
esp_netif_t* pNetifSta;

//w: errMeshNetworkInitialize; vMeshNetworkMeshEventHandler
//r: errMeshNetworkInitialize;
bool bIsMeshConnected;

//w: errMeshNetworkInitialize; vMeshNetworkMeshEventHandler
//r: vMeshNetworkMeshEventHandler
int32_t i32MeshLayer;

//w: errMeshNetworkInitialize; vMeshNetworkMeshEventHandler
//r: vMeshNetworkMeshEventHandler
mesh_addr_t meshParentAddr;

//function pointer for callbacks
void (*pAppRxHandle)(const uint8_t* const, const uint8_t* const);
void (*pOTAChildConnectHandle)(const uint8_t* const);
void (*pOTAMessageHandle)(const MESH_PACKET_t* const);
void (*pChangeStateOfServerWorkerHandle)(const bool );

/**
* @fn esp_err_t errMeshNetworkInitialize()
* @brief Starts the mesh network
* @param void
* @return ESP32 error code
* @author Hendrik Schutter, init based in ESP32-IDE code
* @date 20.01.2021
*
* Initialize the network
*/
esp_err_t errMeshNetworkInitialize(void)
{
    //init module variables
    esp_err_t err;
    bIsMeshConnected = false;
    i32MeshLayer = -1;
    pNetifSta = NULL;

    err = nvs_flash_init(); //init non-volatile storage

#ifdef ERASE_NVS
    if(err == ESP_ERR_NVS_NO_FREE_PAGES) //check if storage is full
        {
            ERROR_CHECK(nvs_flash_erase());
        }
#endif

    // tcpip initialization
    ERROR_CHECK(esp_netif_init());

    //event initialization
    ERROR_CHECK(esp_event_loop_create_default());

    //create network interfaces for mesh (only station instance saved for further manipulation, soft AP instance ignored
    ERROR_CHECK(esp_netif_create_default_wifi_mesh_netifs(&pNetifSta, NULL));

    //wifi initialization
    ERROR_CHECK(errMeshNetworkInitializeWiFi());

    //mesh initialization
    ERROR_CHECK(esp_mesh_init());

    //mesh initialization
    ERROR_CHECK(esp_event_handler_register(MESH_EVENT, ESP_EVENT_ANY_ID, &vMeshNetworkMeshEventHandler, NULL));

    //set mesh topology
    ERROR_CHECK(esp_mesh_set_topology(CONFIG_MESH_TOPOLOGY));

    //set mesh max layer according to the topology
    ERROR_CHECK(esp_mesh_set_max_layer(CONFIG_MESH_MAX_LAYER));
    ERROR_CHECK(esp_mesh_set_vote_percentage(1));
    ERROR_CHECK(esp_mesh_set_xon_qsize(128));

    //Disable mesh PS function
    ERROR_CHECK(esp_mesh_disable_ps());
    ERROR_CHECK(esp_mesh_set_ap_assoc_expire(10));

    mesh_cfg_t cfg = MESH_INIT_CONFIG_DEFAULT();

    /* mesh ID */
    memcpy((uint8_t *) &cfg.mesh_id, CONFIG_MESH_ID, 6);

    ERROR_CHECK(errMeshNetworkInitializeRouter(&cfg));

    /* mesh softAP */
    ERROR_CHECK(esp_mesh_set_ap_authmode(CONFIG_MESH_AP_AUTHMODE));
    cfg.mesh_ap.max_connection = CONFIG_MESH_AP_CONNECTIONS;
    memcpy((uint8_t *) &cfg.mesh_ap.password, CONFIG_MESH_AP_PASSWD,
           strlen(CONFIG_MESH_AP_PASSWD));
    ERROR_CHECK(esp_mesh_set_config(&cfg));

    /* mesh start */
    ERROR_CHECK(esp_mesh_start());

    ERROR_CHECK(esp_base_mac_addr_get(u8ownMAC))

    //debug info
    ESP_LOGD(LOG_TAG, "mesh starts successfully, heap:%d, %s<%d>%s, ps:%d\n",  esp_get_minimum_free_heap_size(),
             esp_mesh_is_root_fixed() ? "root fixed" : "root not fixed",
             esp_mesh_get_topology(), esp_mesh_get_topology() ? "(chain)":"(tree)", esp_mesh_is_ps_enabled());


    ESP_LOGI(LOG_TAG, "Node MAC: \"%x:%x:%x:%x:%x:%x\" ", u8ownMAC[0], u8ownMAC[1], u8ownMAC[2], u8ownMAC[3], u8ownMAC[4], u8ownMAC[5]);

    return ESP_OK;
}

/**
* @fn esp_err_t errMeshNetworkInitializeWiFi()
* @brief Starts the WiFI
* @param void
* @return ESP32 error code
* @author Hendrik Schutter, init based in ESP32-IDE code
* @date 20.01.2021
*
* start the wifi
*/
esp_err_t errMeshNetworkInitializeWiFi()
{
//wifi initialization
    esp_err_t err = ESP_OK;
    wifi_init_config_t config = WIFI_INIT_CONFIG_DEFAULT();
    ERROR_CHECK(esp_wifi_init(&config));
    ERROR_CHECK(esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &vMeshNetworkIpEventHandler, NULL));
    ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_FLASH));
    ERROR_CHECK(esp_wifi_start());
    return err;
}

/**
* @fn esp_err_t errMeshNetworkInitializeRouter(mesh_cfg_t* cfg)
* @brief Starts the router
* @param cfg router config
* @return ESP32 error code
* @author Hendrik Schutter, init based in ESP32-IDE code
* @date 20.01.2021
*
* Initialize the network
*/
esp_err_t errMeshNetworkInitializeRouter(mesh_cfg_t* cfg)
{
//router initialization
    esp_err_t err = ESP_OK;
    (*cfg).channel = CONFIG_MESH_CHANNEL;
    (*cfg).router.ssid_len = strlen(CONFIG_MESH_ROUTER_SSID);
    memcpy((uint8_t *) &(*cfg).router.ssid, CONFIG_MESH_ROUTER_SSID, (*cfg).router.ssid_len);
    memcpy((uint8_t *) &(*cfg).router.password, CONFIG_MESH_ROUTER_PASSWD,
           strlen(CONFIG_MESH_ROUTER_PASSWD));
    return err;
}

/**
* @fn esp_err_t errMeshNetworkSetChildConnectedHandle(void (*pChildConnectHandleTmp)(const uint8_t* const cpcu8Data))
* @brief set callback for event when child connects
* @param (*pChildConnectHandleTmp)(const uint8_t* const cpcu8Data) function pointer
* @return ESP32 error code
* @author Hendrik Schutter
* @date 20.01.2021
*/
esp_err_t errMeshNetworkSetChildConnectedHandle(void (*pChildConnectHandleTmp)(const uint8_t* const cpcu8Data))
{
    pOTAChildConnectHandle = pChildConnectHandleTmp;
    return ESP_OK;
}

/**
* @fn esp_err_t errMeshNetworkSetAppReceiveHandle(void (*pAppRxHandleTmp)(const uint8_t* const cpcu8Data, const uint8_t* const pu8Sender))
* @brief set callback for event when application data is received
* @param (*pAppRxHandleTmp)(const uint8_t* const cpcu8Data, const uint8_t* const pu8Sender) function pointer
* @return ESP32 error code
* @author Hendrik Schutter
* @date 20.01.2021
*/
esp_err_t errMeshNetworkSetAppReceiveHandle(void (*pAppRxHandleTmp)(const uint8_t* const cpcu8Data, const uint8_t* const pu8Sender))
{
    pAppRxHandle = pAppRxHandleTmp; //set handle from app as receive handle if an app packet is received
    return ESP_OK;
}

/**
* @fn esp_err_t errMeshNetworkSetOTAMessageHandleHandle(void (*pOTAMessageHandleTmp)(const MESH_PACKET_t* const cpcuMeshPacket))
* @brief set callback for event when OTA message is received
* @param (*pOTAMessageHandleTmp)(const MESH_PACKET_t* const cpcuMeshPacket) function pointer
* @return ESP32 error code
* @author Hendrik Schutter
* @date 20.01.2021
*/
esp_err_t errMeshNetworkSetOTAMessageHandleHandle(void (*pOTAMessageHandleTmp)(const MESH_PACKET_t* const cpcuMeshPacket))
{
    pOTAMessageHandle = pOTAMessageHandleTmp;
    return ESP_OK;
}

/**
* @fn esp_err_t errMeshNetworkSetChangeStateOfServerWorkerHandle(void (*pChangeStateOfServerWorkerHandleTmp)(const bool cbState))
* @brief set callback for event when connectify to server is changed
* @param (*pChangeStateOfServerWorkerHandleTmp)(const bool cbState) function pointer
* @return ESP32 error code
* @author Hendrik Schutter
* @date 20.01.2021
*/
esp_err_t errMeshNetworkSetChangeStateOfServerWorkerHandle(void (*pChangeStateOfServerWorkerHandleTmp)(const bool cbState))
{
    pChangeStateOfServerWorkerHandle = pChangeStateOfServerWorkerHandleTmp;
    return ESP_OK;
}

esp_err_t errMeshNetworkSendMeshPacket(const mesh_addr_t* const cpcAddrDest, const MESH_PACKET_t* const cpcPacket)
{
    esp_err_t err;
    mesh_data_t data;
    uint8_t tx_buf[CONFIG_MESH_MESSAGE_SIZE] = { 0, };
    data.data = tx_buf;
    data.size = sizeof(tx_buf);
    data.proto = MESH_PROTO_BIN;
    data.tos = MESH_TOS_P2P;
    memcpy(tx_buf, (uint8_t *)cpcPacket, sizeof(MESH_PACKET_t));

    err = esp_mesh_send(cpcAddrDest, &data, MESH_DATA_P2P, NULL, 0);

    return err;
}

bool bMeshNetworkIsRootNode()
{
    return esp_mesh_is_root();
}

bool bMeshNetworkIsNodeNeighbour(const mesh_addr_t* const cpcNode)
{
    esp_err_t err = ESP_OK;
    bool bReturn = false;
    mesh_addr_t addrParent;  //addr of parent node
    mesh_addr_t childrenAddr[CONFIG_MESH_ROUTE_TABLE_SIZE]; //array of children attached to this node
    uint16_t u16ChildrenSize = 0U; //number of children attached to this node

    err = errMeshNetworkGetParentNode(&addrParent);

    if(err == ESP_OK)
        {
            if(bMeshNetworkCheckMACEquality(cpcNode->addr,  addrParent.addr) == true)
                {
                    bReturn = true; //node was found
                }
        }

    if(bReturn == false)
        {
            err = ESP_OK; //reset error code

            ERROR_CHECK(errMeshNetworkGetChildren(childrenAddr, &u16ChildrenSize)); //get all children

            for (uint16_t u16Index = 0; ((u16Index < u16ChildrenSize) && (err == ESP_OK) && (bReturn == false)); u16Index++)
                {
                    if(bMeshNetworkCheckMACEquality(cpcNode->addr,  childrenAddr[u16Index].addr) == true)
                        {
                            bReturn = true; //node was found
                        }
                }
        }
    return bReturn;
}

//returns true if MAC address is equal
bool bMeshNetworkCheckMACEquality(const uint8_t* const cpcu8aMAC, const uint8_t* const cpcu8bMAC)
{
    bool bRet = true;
    uint8_t index = 0;

    while ((index < 6) && (bRet == true))
        {
            if(cpcu8aMAC[index] != cpcu8bMAC[index])
                {
                    bRet = false;
                }

            if(index == 5)
                {
                    //last byte of mac
                    if(abs((cpcu8aMAC[index] - cpcu8bMAC[index])) <= 1)
                        {
                            bRet = true; //last byte differs 1 ore less
                        }
                }
            index++;
        }
    return bRet;
}

esp_err_t errMeshNetworkStartReceiveTask()
{
    esp_err_t err = ESP_OK;
    BaseType_t xReturned;

    xReturned = xTaskCreate(vMeshNetworkTaskReceiveMeshData, "ReceiveMeshData", 7000, NULL, 5, NULL);

    if(xReturned != pdPASS)
        {
            err = ESP_FAIL;
        }
    return err;
}

void vMeshNetworkGetOwnAddr(mesh_addr_t* const cpMeshOwnAddr)
{
    memcpy(cpMeshOwnAddr->addr, u8ownMAC, 6);
}

esp_err_t errMeshNetworkGetChildren(mesh_addr_t* const cpChildren, uint16_t* const cpu16ChildrenSize)
{
    esp_err_t err = ESP_OK;
    int route_table_size = 0;
    *cpu16ChildrenSize = 0;
    mesh_addr_t route_table[CONFIG_MESH_ROUTE_TABLE_SIZE];
    ERROR_CHECK(esp_mesh_get_routing_table((mesh_addr_t *) &route_table, (CONFIG_MESH_ROUTE_TABLE_SIZE * 6), &route_table_size));

    if (err == ESP_OK)
        {
            for(uint16_t index = 0; index < esp_mesh_get_routing_table_size(); index++)
                {
                    if(! (bMeshNetworkCheckMACEquality(u8ownMAC, route_table[index].addr))  )
                        {
                            //child node
                            //ESP_LOGI(LOG_TAG, "adding Node: \"0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x\" ", route_table[index].addr[0], route_table[index].addr[1], route_table[index].addr[2], route_table[index].addr[3], route_table[index].addr[4], route_table[index].addr[5]);
                            cpChildren[*cpu16ChildrenSize] = route_table[index];
                            (*cpu16ChildrenSize) =  (*cpu16ChildrenSize)+1;
                        }
                }
        }
    return err;
}

void vMeshNetworkTaskReceiveMeshData(void *arg)
{
    esp_err_t err;
    mesh_addr_t from;
    mesh_data_t data;
    uint8_t rx_buf[CONFIG_MESH_MESSAGE_SIZE] = { 0, };
    int flag = 0;
    data.data = rx_buf;
    data.size = CONFIG_MESH_MESSAGE_SIZE;

    while (true)
        {
            data.size = CONFIG_MESH_MESSAGE_SIZE;
            err = esp_mesh_recv(&from, &data, portMAX_DELAY, &flag, NULL, 0);
            if (err != ESP_OK || !data.size)
                {
                    ESP_LOGE(LOG_TAG, "err:0x%x, size:%d", err, data.size);
                    continue;
                }
            MESH_PACKET_t packet;
            memcpy(&packet, (uint8_t *)rx_buf, sizeof(MESH_PACKET_t)); //parse MESH_PACKET_t
            memcpy(&packet.meshSenderAddr, &from, sizeof(mesh_addr_t)); //copy sender into packet

            switch (packet.type)
                {
                case APP_Data:
                    ESP_LOGD(LOG_TAG, "recv: APP_Data");
                    //call the rx handle from app
                    pAppRxHandle(packet.au8Payload, from.addr);  //hand over payload and sender of this mesh packet
                    break;
                case OTA_Version_Request:
                case OTA_Version_Response:
                case OTA_Data:
                case OTA_ACK:
                case OTA_Complete:
                case OTA_Abort:
                    //call the rx handle from OTA
                    if(pOTAMessageHandle)
                        {
                            pOTAMessageHandle(&packet);
                        }
                    break;
                default:
                    ESP_LOGE(LOG_TAG, "recv: something");
                    break;
                }//end switch
        } //end while
}

esp_err_t errMeshNetworkGetParentNode(mesh_addr_t* const cpMeshParentAddr)
{
    esp_err_t err = ESP_OK;

    if((bIsMeshConnected == false) || (esp_mesh_is_root()))
        {
            //this node is not connected or is the root --> this node has no parent
            err = ESP_FAIL;
        }
    else
        {
            //node has parent
            memcpy(cpMeshParentAddr, &meshParentAddr, sizeof(mesh_addr_t));
        }
    return err;
}