#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
//#include "freertos/queue.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "freertos/event_groups.h"
#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include <lwip/netdb.h>
#include "lwip/dns.h"
#include "mqtt_client.h"
#include "driver/gpio.h"
#include <errno.h>
#include <ctype.h>
#include <a2o_datatypes.h>
#include "esp_ota_ops.h"
#include "lwip/apps/sntp.h"
#include <time.h>

uint8_t MQTT_event = 0;
bool wifi_st_changed = false;
//char server_url[100]="";

/* errno : variable to indicate file system error code*/
extern int errno ;
uint8_t local_wifi_config_mode = 0;

/*Buffer to send response when tcp socket is connected*/
char tcp_response[1024] = "0";
uint32_t wifi_connection_count = 0;
char reg_msg_response[1024] = "0";

/* FreeRTOS event group to signal when we are connected*/
static EventGroupHandle_t s_wifi_event_group;

#define PORT  3333
#define BROKER_URL "mqtt://test.mosquitto.org:1883"//"mqtt://mqtt.eclipse.org:1883"
extern const uint8_t mqtt_cert_pem_start[]   asm("_binary_AmazonRootCA1_pem_start");
extern const uint8_t mqtt_cert_pem_end[]   asm("_binary_AmazonRootCA1_pem_end");

extern const uint8_t mqtt_client_cert_pem_start[]   asm("_binary_certificate_crt_start");
extern const uint8_t mqtt_client_cert_pem_start[]   asm("_binary_certificate_crt_end");

extern const uint8_t mqtt_client_key_pem_start[]   asm("_binary_private_key_start");
extern const uint8_t mqtt_client_key_pem_start[]   asm("_binary_private_key_end");

//a2o_mqtt_struct_t data_from_cloud;
a2o_mqtt_struct_t * last_msg = NULL;
a2o_mqtt_struct_t * current_msg = NULL;
a2o_mqtt_struct_t * new_msg = NULL;
a2o_mqtt_struct_t * prev_msg = NULL;

uint8_t msg_published_flag = 1;

esp_mqtt_client_handle_t client;

TaskHandle_t tcp_server_Handle = NULL;
TaskHandle_t mqtt_parser_Handle = NULL;

//Test variable
wifi_ap_record_t apinfo;

int listen_sock;
int sock;

wifi_mode_t current_wifi_mode;
wifi_ap_record_t ap_records[30];
uint16_t ap_num;
int tcp_server_connected_flag = 0;

enum wifi_status_t wifi_status = WIFI_NOT_CONNECTED;
char msg_response[550] = "0";
//static uint8_t mqtt_connection_initiated = 0;
static uint8_t mqttClientStart_flag = 0;
static uint8_t connectionEvent = 0;
uint8_t mqtt_connected = 0;

char command_received[256] = "0";
uint32_t current_time_from_app = 0, gl_set_time = 0;
timeElements_t setting_time;
char a2o_rtc_time[20] = "dd/mm/yy,hh:mm:ss";
uint8_t msgId = 0;
uint8_t sntp_init_status = false;

static const char *TAG = "My_WiFi";

/**
  * @fn       mqtt_event_handler_cb
  * @brief    Callback function for MQTT events.
  *           
  * @param    esp_mqtt_event_handle_t : Struct for MQTT event.
  */
static esp_err_t mqtt_event_handler_cb(esp_mqtt_event_handle_t event)
{
    esp_mqtt_client_handle_t client = event->client;
	
    // your_context_t *context = event->context;
    switch (event->event_id) {
        case MQTT_EVENT_CONNECTED:
            ESP_LOGI(TAG, "MQTT_EVENT_CONNECTED");
			//Subscribe to the required topics
			char topic[50] = "/topic/";
			strcat(topic, device_id);
			printf("topic : %s\n", topic);
			esp_mqtt_client_subscribe(client, topic, 1);
			
			if(!cloud_provisioning)
			{
				strcpy(msg_response, "\"");
				strcat(msg_response, device_id);
				strcat(msg_response, "&10&provision\"");
				//printf("msg response : %s , %d\n", msg_response, strlen(msg_response));
				if(mqtt_connected)
				{
					esp_mqtt_client_publish(client, "/topic/hello", msg_response, 0, 1, 0);
				}
			}
			// Check if OTA upgrade was initiated
			if(get_otaUpgrade_initiated_flag())
			{
				strcpy(msg_response, "\"");
				strcat(msg_response, device_id);
				strcat(msg_response, "&11&ota&upgrade&");
				//check if upgrade was successfull by comparing the last and current fw version
				char last_version[10]="";
				get_saved_fw_version(last_version);
				
				const esp_partition_t *running = esp_ota_get_running_partition();
				esp_app_desc_t running_app_info;
				esp_ota_get_partition_description(running, &running_app_info);
				printf("Current version is %s, last version is %s\n",running_app_info.version,last_version);
				/* if version is same, OTA was unsuccessful*/
				if(strcmp(running_app_info.version, last_version) == 0)
				{
					strcat(msg_response, "fail");
				}
				else
				{
					strcat(msg_response, "success");
				}
				if(mqtt_connected)
				{
					esp_mqtt_client_publish(client, "/topic/data0", msg_response, 0, 1, 0);
				}
				/* Reset the ota initiated flag*/
				set_otaUpgrade_initiated_flag(OTA_NOT_INITIATED);
			}
			
			//Create MQTT command parser task
			//mqtt_connection_initiated = 1;
			mqtt_connected = 1;
			wifi_status = WIFI_CONNECTED;
			wifi_st_changed = true;
			/*if(is_lcd_enabled())
			{
				display_wifi_status(WIFI_CONNECTED);
			}*/
			
			if(mqtt_parser_Handle == NULL)
			{
				xTaskCreate(mqtt_command_parser, "mqtt_command_parser", 4096, NULL, 0, &mqtt_parser_Handle);		
			}				
            break;
			
        case MQTT_EVENT_DISCONNECTED:
            ESP_LOGI(TAG, "MQTT_EVENT_DISCONNECTED");
			//mqtt_connection_initiated = 0;
			mqtt_connected = 0;
			if(wifi_status == WIFI_CONNECTED)
			{
				wifi_status = WIFI_CONNECTED_WEAK;
				wifi_st_changed = true;
				//display_wifi_status(WIFI_CONNECTED_WEAK);
			}
			
			//vTaskDelete(mqtt_parser_Handle);
            break;

        case MQTT_EVENT_SUBSCRIBED:
            ESP_LOGI(TAG, "MQTT_EVENT_SUBSCRIBED, msg_id=%d", event->msg_id);
            break;
			
        case MQTT_EVENT_UNSUBSCRIBED:
            ESP_LOGI(TAG, "MQTT_EVENT_UNSUBSCRIBED, msg_id=%d", event->msg_id);
            break;
			
        case MQTT_EVENT_PUBLISHED:
            ESP_LOGI(TAG, "MQTT_EVENT_PUBLISHED, msg_id=%d", event->msg_id);
			msg_published_flag = 1;
            break;
			
        case MQTT_EVENT_DATA:
            ESP_LOGI(TAG, "MQTT_EVENT_DATA");
			//event->topic[event->topic_len] = 0;
			//event->data[event->data_len] = 0;
			printf("DATA=%s \r\n", event->data);
			/*for(int i=0; i<event->data_len;i++)
			{
				 printf("DATA= %x\r\n", event->data[i]);
			}
            printf("DATA LENGTH=%d\n", event->data_len);*/
			add_to_mqtt_queue(event->topic, event->topic_len, event->data, event->data_len);
            break;
			
        case MQTT_EVENT_ERROR:
            ESP_LOGI(TAG, "MQTT_EVENT_ERROR");
			//printf("\n Server Url = %s ", server_url);
            break;
			
        default:
            ESP_LOGI(TAG, "Other event id:%d", event->event_id);
			mqtt_connected = 0;
			if(wifi_status == WIFI_CONNECTED)
			{
				wifi_status = WIFI_CONNECTED_WEAK;
				wifi_st_changed = true;
				//display_wifi_status(WIFI_CONNECTED_WEAK);
			}
            break;
    }
    return ESP_OK;
}

/**
  * @fn       mqtt_event_handler
  * @brief    This is a handler function for MQTT events.
  *
  *           Callback function is called with the event data.
  *      
  * @see	  mqtt_event_handler_cb()
  *
  */
static void mqtt_event_handler(void *handler_args, esp_event_base_t base, int32_t event_id, void *event_data) {
    ESP_LOGD(TAG, "Event dispatched from event loop base=%s, event_id=%d", base, event_id);
	/* Execute the callback */
    mqtt_event_handler_cb(event_data);
}

/**
  * @fn       publish_to_mqtt
  * @brief    Publish a message response to MQTT.
  *      
  * @param	  data : message to be published
  *
  * @return
  * - pub_st : message publish status
  */
uint8_t publish_to_mqtt(char data[])
{
	uint8_t pub_st = 0;
	//if(msg_published_flag == 1)
	//{
		//msg_published_flag = 0;
		strcpy(reg_msg_response, "\"");  //double quotes added to the start and end of response to be compatible with aws
		strcat(reg_msg_response, device_id);
		strcat(reg_msg_response, "&");
		strcat(reg_msg_response, data);
		strcat(reg_msg_response, "\"");
		printf("publishing : %s\n", reg_msg_response);
		if(mqtt_connected)
		{
			esp_mqtt_client_publish(client, "/topic/data3", reg_msg_response, 0, 1, 0);
		}
		pub_st = 1;
	//}
	
	return pub_st;
}

/**
  * @fn       add_to_mqtt_queue
  * @brief    Adds the command recieved from cloud to a queue.
  *      
  * @param1	  topic 	: topic to which the command was published.
  * @param2	  topic_len : length of topic string.
  * @param3	  data 		: data published on topic.
  * @param4	  data_len  : length of data string.
  *
  */
void add_to_mqtt_queue(char* topic, uint8_t topic_len, char* data, uint8_t data_len)
{	
	// Not the first time
	if(last_msg != NULL)
	{
		new_msg = (struct a2o_mqtt_struct *)malloc(1*sizeof(struct a2o_mqtt_struct));
		if(new_msg != NULL)
		{
			last_msg->next = new_msg;
			strncpy(new_msg->p_data, data, data_len);
			new_msg->p_data[data_len] = '\0';
			printf("1. ******************added to queue : %s",new_msg->p_data);
			strncpy(new_msg->p_topic, topic, topic_len);
			new_msg->p_topic[topic_len] = '\0';
			new_msg->next = NULL;
			// Now this is the last command
			last_msg = new_msg;
		}
	}
	// First time the last_command and current_command would be NULL
	else
	{
		last_msg = (struct a2o_mqtt_struct *)malloc(1*sizeof(struct a2o_mqtt_struct));
		if(last_msg != NULL)
		{
			strncpy(last_msg->p_data, data, data_len);
			last_msg->p_data[data_len] = '\0';
			printf("2. ******************added to queue : %s\n",last_msg->p_data);
			strncpy(last_msg->p_topic, topic, topic_len);
			last_msg->p_topic[topic_len] = '\0';
			last_msg->next = NULL;
			current_msg = last_msg;
		}
	}
}

/**
  * @fn       mqtt_command_parser
  * @brief    Function parses the commands received over
  *			  MQTT.	
  * 
  */

void mqtt_command_parser()
{
	
	while(1)
	{
		// Keep publishing until we hit the last messgae
		if(current_msg != NULL)
		{
			printf("Inside command parser,current_msg != NULL\n");
			// Wait till msg is sent published and flag is set
			if(mqtt_connected == 1)
			{
				// Reset the msg published flag every time we are abount to publish a message
				//msg_published_flag = 0;
				
				strcpy(command_received, current_msg->p_data);
				
				strcpy(msg_response, "\"");  //double quotes added to the start and end of response to be compatible with aws
				strcat(msg_response, device_id);
				strcat(msg_response, "&");
				
				char* token = strtok(command_received, "&");
				// Get the message id from the received command
				msgId = atoi(token);	
				if(msgId != MQTT_KEEP_ALIVE)
				{
					strcat(msg_response, current_msg->p_data);
				}
				
			//	uint8_t alert_no = 0;
				
				printf("Msg ID is %d\n",msgId);
				
				// Switch Message ID
				switch(msgId)
				{
					case MQTT_KEEP_ALIVE:
						
						// This is the first request after mqtt connection
						// We need to send the keepalive
						
						strcat(msg_response, keepAlive_resp);
						strcat(msg_response, "\"");
						if(mqtt_connected)
						{
							esp_mqtt_client_publish(client, "/topic/data1", msg_response, 0, 1, 0);
						}
						//mqtt_connection_initiated = 0;	

						
					break;
				}
				
				printf("2. Msg ID is %d\n",msgId);
				
				if((msgId != MQTT_KEEP_ALIVE))
				{
					strcat(msg_response, "\"");
					//Publish the response for the current msg 
					if(mqtt_connected)
					{
						esp_mqtt_client_publish(client, "/topic2/data2", msg_response, 0, 1, 0);
					}
					MQTT_event = 0;
					printf("Response published\n");
				}
				
				if(current_msg->next != NULL)	
				{
					// Copy the current msg
					prev_msg = current_msg;
					// Update the current msg
					current_msg = current_msg->next;
					free(prev_msg);
					prev_msg = NULL;
					printf("It was not last message, moving to next\n");
				}
				else
				{
					free(current_msg);
					current_msg = NULL;
					// Set the last msg to NULL as we have emptied our link list
					last_msg = NULL;
					printf("It was last message, freeing current message\n");
				}
			}
		}
		vTaskDelay(pdMS_TO_TICKS(200));
	}
}

/**
  * @fn       wifi_event_handler
  * @brief    Callback function for wifi events.
  *           
  * @param1    event_data : data struct for occured event.
  * @param2    event_id   : id for occured event.
  */
static void wifi_event_handler(void* arg, esp_event_base_t event_base, int32_t event_id, void* event_data)
{	
	if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START)
	{
        ESP_LOGI(TAG, "WIFI_EVENT_STA_START");
		
		esp_wifi_get_mode(&current_wifi_mode);
		if( current_wifi_mode == WIFI_MODE_STA)
		{
			esp_wifi_connect();
		}
		
		//printf("wifi mode : %d\n", current_wifi_mode);
    }
    else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_AP_STACONNECTED) 
	{
		if(!connectionEvent)
		{
			//local_wifi_config_mode = 1;
			ESP_LOGI(TAG, "WIFI_EVENT_AP_STACONNECTED");
			if( tcp_server_Handle != NULL )
			{
				printf("deleting task\n");
				vTaskDelete(tcp_server_Handle);
				tcp_server_Handle = NULL;
				vTaskDelay(2000 / portTICK_PERIOD_MS);
			}
			connectionEvent = 1;
			xTaskCreate(tcp_server_task, "tcp_server", 4*1024, NULL, 0, &tcp_server_Handle);
		}
    } 
	else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_AP_STADISCONNECTED)
	{
		ESP_LOGI(TAG, "WIFI_EVENT_AP_STADISCONNECTED");
        wifi_event_ap_stadisconnected_t* event = (wifi_event_ap_stadisconnected_t*) event_data;
        ESP_LOGI(TAG, "station "MACSTR" leave, AID=%d", MAC2STR(event->mac), event->aid);
		tcp_server_connected_flag = 0;
		local_wifi_config_mode = 0;
		connectionEvent = 0;
		printf("closing socket\n");
		close(sock);
		//closesocket(listen_sock);
		lwip_close(listen_sock);
		printf(" socket closed\n");
		vTaskDelete(tcp_server_Handle);
		tcp_server_Handle = NULL;
			
    }
	else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_SCAN_DONE)
	{
        ESP_LOGI(TAG, "WIFI_EVENT_SCAN_DONE");
		ESP_ERROR_CHECK(esp_wifi_scan_get_ap_num(&ap_num));
		ESP_ERROR_CHECK(esp_wifi_scan_get_ap_records(&ap_num, ap_records)); 
		for(int i = 0; i < ap_num; i++)
		{
			printf("%32s | %4d \n", (char *)ap_records[i].ssid, ap_records[i].rssi);
		}
	}
	else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_CONNECTED)
	{
        ESP_LOGI(TAG, "WIFI_EVENT_STA_CONNECTED");
		esp_wifi_get_mode(&current_wifi_mode);
		if(current_wifi_mode == WIFI_MODE_APSTA)
		{
			//check if station is connected to tcp server
			if(tcp_server_connected_flag)
			{
				//send connection success message over tcp server 
				printf("\n Sent apconnect success \n");
				strcat(tcp_response, "&apCredential&success");
				send(sock, tcp_response, strlen(tcp_response), 0);
			}
			local_wifi_config_mode = 0;
		}		
    }
	else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED)
	{

		ESP_LOGI(TAG, "WIFI_EVENT_STA_DISCONNECTED");
		wifi_event_sta_disconnected_t* event = (wifi_event_sta_disconnected_t*) event_data;
		//add_log_to_queue("Wifi_event->STA disconnected");
		
		wifi_status = WIFI_NOT_CONNECTED;
		wifi_st_changed = true;
		// Update WiFi status on LCD
		/*if(is_lcd_enabled())
		{
			display_wifi_status(WIFI_NOT_CONNECTED);
		}*/
		
		if(tcp_server_connected_flag == 1)
		{
			strcat(tcp_response, "&apCredential&failure");
			//printf("response : %s, %d \n", tcp_response, strlen(tcp_response));
			send(sock, tcp_response, strlen(tcp_response), 0);
		}
		else
		{
			printf("disconnection error: %d\n",event->reason);
			esp_wifi_get_mode(&current_wifi_mode);
			if(current_wifi_mode == WIFI_MODE_STA)
			{
				//Stop MQTT client
				if(mqttClientStart_flag == 1)
				{
					esp_mqtt_client_stop(client);
					mqttClientStart_flag = 0;
				}
				//try reconnect and change to dual mode
				ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_APSTA));
				start_dhcp_server();
				wifi_set_dual_mode_config();
			}
			printf("Error connecting\n");
			if (!local_wifi_config_mode)
			{
				vTaskDelay(2000 / portTICK_PERIOD_MS);
				esp_wifi_connect();
			}
			//Add delay
		}
		
    }
	else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) 
	{
        ip_event_got_ip_t* event = (ip_event_got_ip_t*) event_data;
        ESP_LOGI(TAG, "got ip:%s",
                 ip4addr_ntoa(&event->ip_info.ip));
				 
			//Test Code
			
			memset(&apinfo, 0, sizeof(apinfo));
			esp_wifi_sta_get_ap_info(&apinfo);
			printf("\n SSID: %s\n", apinfo.ssid);
			printf("\n RSSI: %d\n", apinfo.rssi);
				 
			wifi_status = WIFI_CONNECTED;
			wifi_st_changed = true;
			
			add_log_to_queue("Wifi_event->STA connected");
			//change mode
			ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
			// Save wifi credentials in flash 
			save_wifi_credentials_to_flash();

			// Start mqtt client 
			esp_mqtt_client_start(client);
			mqttClientStart_flag = 1;
			
			local_wifi_config_mode = 0;
			
			// Added to init NTP sync
			if (sntp_init_status == false)
			{
				setenv("TZ", "UTC-05:30", 1);
				tzset();
		
				sntp_setoperatingmode(SNTP_OPMODE_POLL); 
				sntp_setservername(0, "pool.ntp.org");
				sntp_init();
				sntp_init_status = true;
				
				vTaskDelay(1000/portTICK_PERIOD_MS);
			}			
	}
}

/**
  * @fn       wifi_tcp_command_parser
  * @brief    Function parses the commands received over
  *			  local wifi.	
  * 	
  * @param1   rx_buffer : buffer containing data recieved.
  * @param2   len       : length of data buffer.
  */
void wifi_tcp_command_parser(char* rx_buffer, int len)
{
	char* token = strtok(rx_buffer, "&"); 
	char* AP_ssid = "\0";
	char* AP_pswrd = "\0";
	
	//tcp_response[0] = '\0';
	strcpy(tcp_response, device_id);
		
	if(strcmp(token, "a") == 0)
	{
		printf("%s\n", token);
		wifi_scan_config_t scan_config = {
			.ssid = 0,
			.bssid = 0,
			.channel = 0,
			.show_hidden = true
		};
		ESP_ERROR_CHECK(esp_wifi_scan_start(&scan_config, true));
		strcat(tcp_response, "&apList");
		for(int i = 0; i < ap_num; i++)
		{
			strcat(tcp_response, "&");
			strcat(tcp_response,(char*)ap_records[i].ssid);
		}
		//printf("response : %s \n", tcp_response);
		send(sock, tcp_response, strlen(tcp_response), 0);
	}
	else if(strcmp(token, "b") == 0)
	{
		token = strtok(NULL, "&");
		AP_ssid = token;	
		token = strtok(NULL, "&");
		AP_pswrd = token;
		
		wifi_set_station_mode_config(AP_ssid, AP_pswrd);
		esp_wifi_connect();
	}
}

/**
  * @fn       update_file
  * @brief    Write data to a file in flash.	
  * 	
  * @param1   file_path : file name to write data to.
  * @param2   data_wr   : data to be written  
  */
uint8_t update_file(char* file_path, char* data_wr)
{
	uint8_t ret = STATUS_OK;
	FILE* f = fopen(file_path, "w");
	if (f != NULL)
	{
		fputs(data_wr, f);
		fclose(f);
	}
	else
	{
		ret = STATUS_ERROR;
	}
	return ret;
}

/**
  * @fn       download_logs
  * @brief    Function reads and sends logs over wifi.	
  * 	
  *           Logs are sent either over mqtt or local wifi.  
  */
uint8_t read_log[400] = {0};
void download_logs(char file_name[])
{
	//struct dirent *de;
	//DIR *dr = opendir(partition_name); 
	//char file_name[30]="";
	FILE* f = NULL;
	uint32_t read_index = 0;
	/*if (dr == NULL)  // opendir returns NULL if couldn't open directory 
	{ 
		send(sock, "no file to read", 15, 0);
	} 
	else
	{   
		while ((de = readdir(dr)) != NULL)
		{
			uint32_t read_index = 8;
			strcpy(file_name, partition_name);
			strcat(file_name, de->d_name);*/
			printf("opening file : %s\n",file_name);
			f = fopen(file_name, "r");
			fseek(f, 0l, SEEK_END);
			long int file_size = ftell(f);
			printf("file size: %ld\n",file_size);
			while(read_index <= file_size)
			{
				fseek(f, read_index, SEEK_SET);
				fread(read_log, 1, 400, f);
				printf("sending log : %s\n",read_log);
				if(mqtt_connected)
				{
					esp_mqtt_client_publish(client, "/topic/data5", (char*)read_log, 0, 1, 0);
				}
				else if(tcp_server_connected_flag)
				{
					send(sock, (char*)read_log, 400, 0);
				}
				printf("sent\n");
				read_index += 400;
				
			}
			fclose(f);
		//}
		
		//closedir(dr);
	//}
	
}

/**
  * @fn       tcp_server_task
  * @brief    task function for tcp communication.
  *	
  *			  This function starts and accepts connection over
  *			  tcp. Used for local wifi communication.
  */
void tcp_server_task(void *pvParameters)
{
	char rx_buffer[512] = "0";
    char addr_str[128] = "0";
    int addr_family;
    int ip_protocol;
	
			printf("Hi from task\n");
   
	while (1) 
	{
		#ifdef CONFIG_EXAMPLE_IPV4
			struct sockaddr_in dest_addr;
			dest_addr.sin_addr.s_addr = htonl(INADDR_ANY);
			dest_addr.sin_family = AF_INET;
			dest_addr.sin_port = htons(PORT);
			addr_family = AF_INET;
			ip_protocol = IPPROTO_IP;
			inet_ntoa_r(dest_addr.sin_addr, addr_str, sizeof(addr_str) - 1);
		#else // IPV6
				struct sockaddr_in6 dest_addr;
				bzero(&dest_addr.sin6_addr.un, sizeof(dest_addr.sin6_addr.un));
				dest_addr.sin6_family = AF_INET6;
				dest_addr.sin6_port = htons(PORT);
				addr_family = AF_INET6;
				ip_protocol = IPPROTO_IPV6;
				inet6_ntoa_r(dest_addr.sin6_addr, addr_str, sizeof(addr_str) - 1);
		#endif
		
			listen_sock = socket(addr_family, SOCK_STREAM, ip_protocol);
			if (listen_sock < 0) {
				ESP_LOGE(TAG, "Unable to create socket: errno %d", errno);
				add_log_to_queue("tcp_server_task->Error creating socket");
				//close(listen_sock);
				//continue;
				break;
			}
			ESP_LOGI(TAG, "Socket created");

			int err = bind(listen_sock, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
			if (err != 0) {
				ESP_LOGE(TAG, "Socket unable to bind: errno %d", errno);
				add_log_to_queue("tcp_server_task->Error binding socket");
				close(listen_sock);
				vTaskDelay(4000 / portTICK_PERIOD_MS);
				continue;
				//break;
			}
			ESP_LOGI(TAG, "Socket bound, port %d", PORT);
		
			err = listen(listen_sock, 1);
			if (err != 0) {
				ESP_LOGE(TAG, "Error occurred during listen: errno %d", errno);
				add_log_to_queue("tcp_server_task->Error during socket listen");
				break;
			}
			ESP_LOGI(TAG, "Socket listening");
			
			struct sockaddr_in6 source_addr; // Large enough for both IPv4 or IPv6
			uint addr_len = sizeof(source_addr);
			
			sock = accept(listen_sock, (struct sockaddr *)&source_addr, &addr_len);
			if (sock < 0) {
				ESP_LOGE(TAG, "Unable to accept connection: errno %d", errno);
				add_log_to_queue("tcp_server_task->Error accepting socket connection");
				break;
			}
			ESP_LOGI(TAG, "Socket accepted");
			
			while (1) 
			{
				local_wifi_config_mode = 1;
				tcp_server_connected_flag = 1;
				printf("Hi from rx loop\n");
				int len = recv(sock, rx_buffer, sizeof(rx_buffer) - 1, 0);
				// Error occurred during receiving
				if (len < 0) {
					ESP_LOGE(TAG, "recv failed: errno %d", errno);
					break;
				}
				// Connection closed
				else if (len == 0) {
					ESP_LOGI(TAG, "Connection closed");
					break;
				}
				// Data received
				else 
				{
					// Get the sender's ip address as string
					if (source_addr.sin6_family == PF_INET) {
						inet_ntoa_r(((struct sockaddr_in *)&source_addr)->sin_addr.s_addr, addr_str, sizeof(addr_str) - 1);
					} else if (source_addr.sin6_family == PF_INET6) {
						inet6_ntoa_r(source_addr.sin6_addr, addr_str, sizeof(addr_str) - 1);
					}
					
					rx_buffer[len] = 0; // Null-terminate whatever we received and treat like a string
					ESP_LOGI(TAG, "Received %d bytes from %s:", len, addr_str);
					ESP_LOGI(TAG, "%s", rx_buffer);
					//send(sock, rx_buffer, len, 0);
			        wifi_tcp_command_parser(rx_buffer,len);
					
				}
			}
			tcp_server_connected_flag = 0;
		if (sock != -1) {
			ESP_LOGE(TAG, "Shutting down socket and restarting...");
			//shutdown(sock, 0);
			close(sock);
			//shutdown(listen_sock, 0);
			close(listen_sock);	
			vTaskDelay(5);
		}
			
    }	
}

/**
  * @fn       a2o_mqtt_init
  * @brief    Initialise MQTT and create the client handle.
  *	
  */
void a2o_mqtt_init(void)
{
	 esp_mqtt_client_config_t mqtt_cfg = {
        .uri = BROKER_URL,
		.cert_pem = (const char *)mqtt_cert_pem_start,
		.client_cert_pem = (const char*)mqtt_client_cert_pem_start,
		.client_key_pem = (const char*)mqtt_client_key_pem_start,
    };
	//printf("--%s",(mqtt_cfg.uri));
    client = esp_mqtt_client_init(&mqtt_cfg);
    esp_mqtt_client_register_event(client, ESP_EVENT_ANY_ID, mqtt_event_handler, client);
   
}

/**
  * @fn       start_dhcp_server
  * @brief    Function assigns static IP to the network
  *			  interface and then starts dhcp server
  *	
  */
void start_dhcp_server()
{
         // stop DHCP server
        ESP_ERROR_CHECK(tcpip_adapter_dhcps_stop(TCPIP_ADAPTER_IF_AP));
        // assign a static IP to the network interface
        tcpip_adapter_ip_info_t info;
        memset(&info, 0, sizeof(info));
        IP4_ADDR(&info.ip, 192, 168, 1, 5);
        IP4_ADDR(&info.gw, 192, 168, 1, 5);//ESP acts as router, so gw addr will be its own addr
        IP4_ADDR(&info.netmask, 255, 255, 255, 0);
        ESP_ERROR_CHECK(tcpip_adapter_set_ip_info(TCPIP_ADAPTER_IF_AP, &info));
        // start the DHCP server   
        ESP_ERROR_CHECK(tcpip_adapter_dhcps_start(TCPIP_ADAPTER_IF_AP));
        printf("DHCP server started \n");
}

/**
  * @fn       a2o_wifi_init
  * @brief    Initialise wifi settings for the device.
  *			  
  * 		  Wifi mode is initialised according to saved
  *			  settings in flash.
  *	
  */
void a2o_wifi_init()
{
	 s_wifi_event_group = xEventGroupCreate();
	 ESP_ERROR_CHECK(esp_event_loop_create_default());
	
	// initialize the tcp stack
	tcpip_adapter_init();
	
	//Initialise MQTT 
	a2o_mqtt_init();
	
    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));

    ESP_ERROR_CHECK(esp_event_handler_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, NULL));
	ESP_ERROR_CHECK(esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL));
	
	//Check if wifi credentials available in flash
	
	char wifi_credentials[100] = "null%null";
	char* wifi_ssid = "0";
	char* wifi_pswrd = "0";
	
	FILE* f = fopen("/spiffs/Wifi_Config/STA_credential.txt", "r");
	if (f == NULL) 
	{
		if(errno == 2)
		{
			f = fopen("/spiffs/Wifi_Config/STA_credential.txt", "w");
			fputs("null%null",f);	
			fclose(f);
			f = fopen("/spiffs/Wifi_Config/STA_credential.txt", "r"); 
			// TODO: Why Fopen Here and Why is DHCP not being started here as we will return after this call.
			// TODO: Or return after mark flash error should be removed.
			/*add_log_to_queue("a2o_wifi_init->wifi init dual mode");
			start_dhcp_server();
			ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_APSTA));
			wifi_set_dual_mode_config();*/
			
		}
		printf("a2o_wifi-> get_wifi_credential_from_flash : Failed to open file for reading");
		//mark_flash_error();
		//return;
	}
	
	if (f != NULL)
	{
		fgets(wifi_credentials, 100, f);	
		fclose(f);
	}
	
	/* If wifi credentials are null, start wifi in dual mode*/
	if(strcmp(wifi_credentials,"null%null") == 0) 
	{
		add_log_to_queue("a2o_wifi_init->wifi init dual mode");
		start_dhcp_server();
		ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_APSTA));
		wifi_set_dual_mode_config();
	}
	/* else start in station mode */
	else 
	{
		add_log_to_queue("a2o_wifi_init->wifi init STA mode");
		//parse the string and separate ssid and password
		char* token = strtok(wifi_credentials, "%"); 
		wifi_ssid = token;
		printf("ssid = %s\n",wifi_ssid);
		token = strtok(NULL, "%"); 
		wifi_pswrd = token;
		printf("pswrd = %s\n",wifi_pswrd);
		ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
		wifi_set_station_mode_config(wifi_ssid, wifi_pswrd);
	}
	wifi_connection_count = get_sensor_reading_count(WIFI_ON_COUNT_OFFSET); //TODO: What is this?
	
	ESP_ERROR_CHECK(esp_wifi_start());
}

/**
  * @fn       wifi_set_dual_mode_config
  * @brief    Set the dual(AP+STA) mode configuration parameters
  *			  for esp.
  *	
  */
void wifi_set_dual_mode_config()
{
	char device_name[50] = "0";
	char* device_password = "";
	FILE* f = fopen("/spiffs/Wifi_Config/AP_credential.txt", "r");
	if(f == NULL)
	{
		strcpy(device_name, "MyEsp32");
	}
	else
	{
		fgets(device_name, 50, f);
		fclose(f);
	}

	wifi_config_t wifi_config = {
        .ap = {
            .ssid = "", 
            .ssid_len = strlen(device_name),
            .password = "",
            .max_connection = 1,
            .authmode = WIFI_AUTH_WPA_WPA2_PSK
        },
    };
	strcpy((char*)(&wifi_config)->ap.ssid, device_name);
	strcpy((char*)(&wifi_config)->ap.password, device_password);
    if (strlen(device_password) == 0) {
        wifi_config.ap.authmode = WIFI_AUTH_OPEN;
    }
    
    ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &wifi_config));
}

/**
  * @fn       wifi_set_station_mode_config
  * @brief    Set the station mode configuration parameters
  *			  for esp.
  *
  * @param1   AP_name  : Access point name.
  * @param2   AP_pswrd : Access point password.
  */
void wifi_set_station_mode_config(char* AP_name, char* AP_pswrd)
{	
	wifi_config_t wifi_config = {
		.sta = {
			.ssid = "",
			.password = ""
		}
	};
	
	strcpy((char*)(&wifi_config)->sta.ssid, AP_name);
	strcpy((char*)(&wifi_config)->sta.password, AP_pswrd);
	
   ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_STA, &wifi_config));
}