#include "Mesh_OTA.h"

static const char *LOG_TAG = "mesh_network";
static uint8_t tx_buf[CONFIG_MESH_MESSAGE_SIZE] = { 0, };
static uint8_t rx_buf[CONFIG_MESH_MESSAGE_SIZE] = { 0, };
uint8_t u8ownMAC[6];
esp_netif_t* netif_sta;
bool bIsMeshConnected;
int32_t i32MeshLayer;
mesh_addr_t meshParentAddr;
void (*pAppRxHandle)(uint8_t*, uint8_t* );
void (*pOTAChildConnectHandle)(uint8_t* );
void (*pOTAMessageHandle)(MESH_PACKET_t* );
void (*pChangeStateOfServerWorkerHandle)(bool );

esp_err_t errMeshNetworkInitialize()
{
    //init module variables
    esp_err_t err;
    bIsMeshConnected = false;
    i32MeshLayer = -1;
    netif_sta = 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(&netif_sta, 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, &vMeshEventHandler, 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;
}

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, &vIPEventHandler, NULL));
    ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_FLASH));
    ERROR_CHECK(esp_wifi_start());
    return err;
}

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;
}

//returns true if MAC address is equal
bool bCheckMACEquality(uint8_t* pu8aMAC,  uint8_t* pu8bMAC)
{
    bool bRet = true;
    uint8_t index = 0;

    while ((index < 6) && (bRet == true))
        {
            if(pu8aMAC[index] != pu8bMAC[index])
                {
                    bRet = false;
                }
            index++;
        }
    return bRet;
}

esp_err_t errGetChildren(mesh_addr_t* pChildren, uint16_t* pu16ChildrenSize)
{
    esp_err_t err = ESP_OK;
    int route_table_size = 0;
    *pu16ChildrenSize = 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(! (bCheckMACEquality(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]);
                            pChildren[*pu16ChildrenSize] = route_table[index];
                            *pu16ChildrenSize = (*pu16ChildrenSize)+1;
                        }
                }
        }
    return err;
}

esp_err_t errGetParentNode(mesh_addr_t* pMeshParentAddr)
{
    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(pMeshParentAddr, &meshParentAddr, sizeof(mesh_addr_t));
        }
    return err;
}

void vGetOwnAddr(mesh_addr_t* pMeshOwnAddr)
{
    memcpy(pMeshOwnAddr->addr, u8ownMAC, 6);
}

bool bIsRootNode()
{
    return esp_mesh_is_root();
}

esp_err_t errMeshNetworkSetAppReceiveHandle(void (*pAppRxHandleTmp)(uint8_t * pu8Data, uint8_t* pu8Sender))
{
    pAppRxHandle = pAppRxHandleTmp; //set handle from app as receive handle if an app packet is received
    return ESP_OK;
}

esp_err_t errMeshNetworkSetChildConnectedHandle(void (*pChildConnectHandleTmp)(uint8_t * pu8Data))
{
    pOTAChildConnectHandle = pChildConnectHandleTmp;
    return ESP_OK;
}

esp_err_t errMeshNetworkSetOTAMessageHandleHandle(void (*pOTAMessageHandleTmp)(MESH_PACKET_t* puMeshPacket))
{
    pOTAMessageHandle = pOTAMessageHandleTmp;
    return ESP_OK;
}

esp_err_t errMeshNetworkSetChangeStateOfServerWorkerHandle(void (*pChangeStateOfServerWorkerHandleTmp)(bool bState))
{
    pChangeStateOfServerWorkerHandle = pChangeStateOfServerWorkerHandleTmp;
    return ESP_OK;
}

esp_err_t errSendMeshPacket(mesh_addr_t* pAddrDest, MESH_PACKET_t* pPacket)
{
    esp_err_t err;
    mesh_data_t data;
    data.data = tx_buf;
    data.size = sizeof(tx_buf);
    data.proto = MESH_PROTO_BIN;
    data.tos = MESH_TOS_P2P;
    memcpy(tx_buf, (uint8_t *)pPacket, sizeof(MESH_PACKET_t));

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

    return err;
}

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

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

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

void vTaskReceiveMeshData(void *arg)
{
    esp_err_t err;
    mesh_addr_t from;
    mesh_data_t data;
    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:
                    //call the rx handle from OTA
                    if(pOTAMessageHandle)
                        {
                            pOTAMessageHandle(&packet);
                        }
                    break;
                default:
                    ESP_LOGE(LOG_TAG, "recv: something");
                    break;
                }//end switch
        } //end while
}