#include "mesh_ota.h"

static const char *LOG_TAG = "mesh_network";
static const uint8_t MESH_ID[6] = { 0x77, 0x77, 0x77, 0x77, 0x77, 0x77};
static uint8_t tx_buf[TX_SIZE] = { 0, };
static uint8_t rx_buf[RX_SIZE] = { 0, };
static bool is_mesh_connected = false;
static mesh_addr_t mesh_parent_addr;

static int mesh_layer = -1;
static esp_netif_t *netif_sta = NULL;

uint8_t ownMAC[6];


esp_err_t errMeshNetworkInitialize()
{
    esp_err_t err;

    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, 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(ownMAC))

    //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\" ", ownMAC[0], ownMAC[1], ownMAC[2], ownMAC[3], ownMAC[4], ownMAC[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 children[], 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(ownMAC, route_table[index].addr))  )
            {
                //child node
                //ESP_LOGI(MESH_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]);
                children[*pu16ChildrenSize] = route_table[index];
                *pu16ChildrenSize = (*pu16ChildrenSize)+1;
            }
        }
    }
    return err;
}

/*
esp_err_t errSendPacket(mesh_addr_t* dest, struct ota_mesh_packet* packet)
{
    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 *)packet, sizeof(struct ota_mesh_packet));
    err = esp_mesh_send(dest, &data, MESH_DATA_P2P, NULL, 0);
    return err;
}
*/

void esp_mesh_p2p_rx_main(void *arg)
{
    esp_err_t err;
    mesh_addr_t from;
    mesh_data_t data;
    int flag = 0;
    data.data = rx_buf;
    data.size = RX_SIZE;

    while (true)
    {
        data.size = RX_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;
        }

        struct ota_mesh_packet packet;
        memcpy(&packet, (uint8_t *)rx_buf, sizeof(struct ota_mesh_packet));

        switch (packet.type)
        {
        case APP_Version_Request:
            ESP_LOGI(LOG_TAG, "recv: APP_Version_Request");
            packet.type=APP_Version_Response;
            packet.au8Payload[0] = 42; //TODO get current running version
            // ESP_ERROR_CHECK (esp_mesh_send_packet(&from, &packet)); //send back to parent
            break;
        case APP_Version_Response:
            ESP_LOGI(LOG_TAG, "recv: APP_Version_Response - App Version %i: ", packet.au8Payload[0]);
            //check if node is out-dated
            // esp_mesh_ota_send(&from);
            break;
        case OTA_Data:
            ESP_LOGI(LOG_TAG, "recv: OTA_Data");
            //esp_mesh_ota_receive(&from, &packet);
            packet.type=OTA_ACK;
            //ESP_ERROR_CHECK (esp_mesh_send_packet(&from, &packet)); //send back to parent
            break;
        case OTA_ACK:
            ESP_LOGI(LOG_TAG, "recv: OTA_ACK");
            //esp_mesh_ota_send(&from);
            break;
        case OTA_Complete:
            ESP_LOGI(LOG_TAG, "recv: OTA_Complete");
            break;
        default:
            ESP_LOGE(LOG_TAG, "recv: something");
            break;
        }

    } //end while
    vTaskDelete(NULL);
}


esp_err_t esp_mesh_comm_p2p_start(void)
{
    static bool is_comm_p2p_started = false;
    if (!is_comm_p2p_started)
    {
        is_comm_p2p_started = true;
        xTaskCreate(esp_mesh_p2p_rx_main, "MPRX", 7000, NULL, 5, NULL);
    }
    return ESP_OK;
}

void vMeshEventHandler(void *arg, esp_event_base_t event_base, int32_t i32EventID, void* vpEventData)
{
    mesh_addr_t id = {0,};
    static uint16_t last_layer = 0;

    switch (i32EventID)
    {
    case MESH_EVENT_STARTED:
    {
        esp_mesh_get_id(&id);
        ESP_LOGI(LOG_TAG, "<MESH_EVENT_MESH_STARTED>ID:"MACSTR"", MAC2STR(id.addr));
is_mesh_connected = false;
mesh_layer = esp_mesh_get_layer();
}
break;
    case MESH_EVENT_STOPPED: {
ESP_LOGI(LOG_TAG, "<MESH_EVENT_STOPPED>");
is_mesh_connected = false;
mesh_layer = esp_mesh_get_layer();
}
break;
    case MESH_EVENT_CHILD_CONNECTED: {
mesh_event_child_connected_t *child_connected = (mesh_event_child_connected_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_CHILD_CONNECTED>aid:%d, "MACSTR"",
 child_connected->aid,
 MAC2STR(child_connected->mac));
}
break;
    case MESH_EVENT_CHILD_DISCONNECTED: {
mesh_event_child_disconnected_t *child_disconnected = (mesh_event_child_disconnected_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_CHILD_DISCONNECTED>aid:%d, "MACSTR"",
 child_disconnected->aid,
 MAC2STR(child_disconnected->mac));
}
break;
    case MESH_EVENT_ROUTING_TABLE_ADD: {
mesh_event_routing_table_change_t *routing_table = (mesh_event_routing_table_change_t *)vpEventData;
ESP_LOGW(LOG_TAG, "<MESH_EVENT_ROUTING_TABLE_ADD>add %d, new:%d, layer:%d",
 routing_table->rt_size_change,
 routing_table->rt_size_new, mesh_layer);
}
break;
    case MESH_EVENT_ROUTING_TABLE_REMOVE: {
mesh_event_routing_table_change_t *routing_table = (mesh_event_routing_table_change_t *)vpEventData;
ESP_LOGW(LOG_TAG, "<MESH_EVENT_ROUTING_TABLE_REMOVE>remove %d, new:%d, layer:%d",
 routing_table->rt_size_change,
 routing_table->rt_size_new, mesh_layer);
}
break;
    case MESH_EVENT_NO_PARENT_FOUND: {
mesh_event_no_parent_found_t *no_parent = (mesh_event_no_parent_found_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_NO_PARENT_FOUND>scan times:%d",
 no_parent->scan_times);
}
    /* TODO handler for the failure */
break;
    case MESH_EVENT_PARENT_CONNECTED: {
mesh_event_connected_t *connected = (mesh_event_connected_t *)vpEventData;
esp_mesh_get_id(&id);
mesh_layer = connected->self_layer;
memcpy(&mesh_parent_addr.addr, connected->connected.bssid, 6);
ESP_LOGI(LOG_TAG,
 "<MESH_EVENT_PARENT_CONNECTED>layer:%d-->%d, parent:"MACSTR"%s, ID:"MACSTR", duty:%d",
 last_layer, mesh_layer, MAC2STR(mesh_parent_addr.addr),
 esp_mesh_is_root() ? "<ROOT>" :
 (mesh_layer == 2) ? "<layer2>" : "", MAC2STR(id.addr), connected->duty);
last_layer = mesh_layer;
 //  mesh_connected_indicator(mesh_layer);
is_mesh_connected = true;
if (esp_mesh_is_root()) {
esp_netif_dhcpc_start(netif_sta);
}
esp_mesh_comm_p2p_start();//start receiving
}
break;
    case MESH_EVENT_PARENT_DISCONNECTED: {
mesh_event_disconnected_t *disconnected = (mesh_event_disconnected_t *)vpEventData;
ESP_LOGI(LOG_TAG,
 "<MESH_EVENT_PARENT_DISCONNECTED>reason:%d",
 disconnected->reason);
is_mesh_connected = false;
   // mesh_disconnected_indicator();
mesh_layer = esp_mesh_get_layer();
}
break;
    case MESH_EVENT_LAYER_CHANGE: {
mesh_event_layer_change_t *layer_change = (mesh_event_layer_change_t *)vpEventData;
mesh_layer = layer_change->new_layer;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_LAYER_CHANGE>layer:%d-->%d%s",
 last_layer, mesh_layer,
 esp_mesh_is_root() ? "<ROOT>" :
 (mesh_layer == 2) ? "<layer2>" : "");
last_layer = mesh_layer;
  //  mesh_connected_indicator(mesh_layer);
}
break;
    case MESH_EVENT_ROOT_ADDRESS: {
mesh_event_root_address_t *root_addr = (mesh_event_root_address_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_ROOT_ADDRESS>root address:"MACSTR"",
 MAC2STR(root_addr->addr));
}
break;
    case MESH_EVENT_VOTE_STARTED: {
mesh_event_vote_started_t *vote_started = (mesh_event_vote_started_t *)vpEventData;
ESP_LOGI(LOG_TAG,
 "<MESH_EVENT_VOTE_STARTED>attempts:%d, reason:%d, rc_addr:"MACSTR"",
 vote_started->attempts,
 vote_started->reason,
 MAC2STR(vote_started->rc_addr.addr));
}
break;
    case MESH_EVENT_VOTE_STOPPED: {
ESP_LOGI(LOG_TAG, "<MESH_EVENT_VOTE_STOPPED>");
break;
}
    case MESH_EVENT_ROOT_SWITCH_REQ: {
mesh_event_root_switch_req_t *switch_req = (mesh_event_root_switch_req_t *)vpEventData;
ESP_LOGI(LOG_TAG,
 "<MESH_EVENT_ROOT_SWITCH_REQ>reason:%d, rc_addr:"MACSTR"",
 switch_req->reason,
 MAC2STR( switch_req->rc_addr.addr));
}
break;
    case MESH_EVENT_ROOT_SWITCH_ACK: {
/* new root */
mesh_layer = esp_mesh_get_layer();
esp_mesh_get_parent_bssid(&mesh_parent_addr);
ESP_LOGI(LOG_TAG, "<MESH_EVENT_ROOT_SWITCH_ACK>layer:%d, parent:"MACSTR"", mesh_layer, MAC2STR(mesh_parent_addr.addr));
}
break;
    case MESH_EVENT_TODS_STATE: {
mesh_event_toDS_state_t *toDs_state = (mesh_event_toDS_state_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_TODS_REACHABLE>state:%d", *toDs_state);
}
break;
    case MESH_EVENT_ROOT_FIXED: {
mesh_event_root_fixed_t *root_fixed = (mesh_event_root_fixed_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_ROOT_FIXED>%s",
 root_fixed->is_fixed ? "fixed" : "not fixed");
}
break;
    case MESH_EVENT_ROOT_ASKED_YIELD: {
mesh_event_root_conflict_t *root_conflict = (mesh_event_root_conflict_t *)vpEventData;
ESP_LOGI(LOG_TAG,
 "<MESH_EVENT_ROOT_ASKED_YIELD>"MACSTR", rssi:%d, capacity:%d",
 MAC2STR(root_conflict->addr),
 root_conflict->rssi,
 root_conflict->capacity);
}
break;
    case MESH_EVENT_CHANNEL_SWITCH: {
mesh_event_channel_switch_t *channel_switch = (mesh_event_channel_switch_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_CHANNEL_SWITCH>new channel:%d", channel_switch->channel);
}
break;
    case MESH_EVENT_SCAN_DONE: {
mesh_event_scan_done_t *scan_done = (mesh_event_scan_done_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_SCAN_DONE>number:%d",
 scan_done->number);
}
break;
    case MESH_EVENT_NETWORK_STATE: {
mesh_event_network_state_t *network_state = (mesh_event_network_state_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_NETWORK_STATE>is_rootless:%d",
 network_state->is_rootless);
}
break;
    case MESH_EVENT_STOP_RECONNECTION: {
ESP_LOGI(LOG_TAG, "<MESH_EVENT_STOP_RECONNECTION>");
}
break;
    case MESH_EVENT_FIND_NETWORK: {
mesh_event_find_network_t *find_network = (mesh_event_find_network_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_FIND_NETWORK>new channel:%d, router BSSID:"MACSTR"",
 find_network->channel, MAC2STR(find_network->router_bssid));
}
break;
    case MESH_EVENT_ROUTER_SWITCH: {
mesh_event_router_switch_t *router_switch = (mesh_event_router_switch_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_ROUTER_SWITCH>new router:%s, channel:%d, "MACSTR"",
 router_switch->ssid, router_switch->channel, MAC2STR(router_switch->bssid));
}
break;
    case MESH_EVENT_PS_PARENT_DUTY: {
mesh_event_ps_duty_t *ps_duty = (mesh_event_ps_duty_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_PS_PARENT_DUTY>duty:%d", ps_duty->duty);
}
break;
    case MESH_EVENT_PS_CHILD_DUTY: {
mesh_event_ps_duty_t *ps_duty = (mesh_event_ps_duty_t *)vpEventData;
ESP_LOGI(LOG_TAG, "<MESH_EVENT_PS_CHILD_DUTY>cidx:%d, "MACSTR", duty:%d", ps_duty->child_connected.aid-1,
MAC2STR(ps_duty->child_connected.mac), ps_duty->duty);
}
break;
    default:
        ESP_LOGI(LOG_TAG, "unknown id:%d", i32EventID);
        break;
    }
}

void vIPEventHandler(void *arg, esp_event_base_t event_base, int32_t i32EventID, void *vpEventData)
{
    ip_event_got_ip_t *event = (ip_event_got_ip_t *) vpEventData;
    ESP_LOGI(LOG_TAG, "<IP_EVENT_STA_GOT_IP>IP:" IPSTR, IP2STR(&event->ip_info.ip));
}