/*
 * challenge_com.c
 *
 *  Created on: 05.12.2022
 *      Author: jonas
 */

#include <stdbool.h>

#include "challenge_com.h"
#include "challenge_app.h"
#include "platform.h"
#include "esp_log.h"
#include "McuUtility.h"
#include "mjson.h"
#include "mqtt_client.h"
#include "myMqtt.h"
#include "splitflap_wrapper.h"
#include "robo_wrapper.h"

#define TAG "CHALLENGE_COM" /* tag for logging with ESP_LOG */

/* TOPICS */
const char MQTT_TOPIC_SF_DISPLAY[] = "/splitFlap/cmd/display/";
const char MQTT_TOPIC_SF_INITALL[] = "/splitFlap/cmd/init/";
const char MQTT_TOPIC_SF_CONFIG_SETUP[] = "/splitFlap/config/setup/";
const char MQTT_TOPIC_ROBO_MODE[] = "/mobile/cmd/mode/";
const char MQTT_TOPIC_ROBO_NAV_TURN[] = "/mobile/cmd/nav/turn/";
const char MQTT_TOPIC_ROBO_NAV_MOVE[] = "/mobile/cmd/nav/move/";
const char MQTT_TOPIC_ROBO_NAV_STOP[] = "/mobile/cmd/nav/stop/";
const char MQTT_TOPIC_ROBO_CALIB_CMD[] = "/mobile/cmd/line_sens/calib";
const char MQTT_TOPIC_ROBO_GET_CALIB[] = "/mobile/cmd/line_sens/get_calib";
const char MQTT_TOPIC_ROBO_CALIB_STATE[] = "/mobile/state/line_sens/calib_data";
const char MQTT_TOPIC_REQ_BATTERY[] = "/both/cmd/battery/get_volt";
const char MQTT_TOPIC_ROBO_RESP_BATTERY[] = "/mobile/state/battery/voltage";
const char MQTT_TOPIC_STAT_RESP_BATTERY[] = "/stationary/state/battery/voltage";

void Challenge_Com_ParseMqtt(void *handler_args, esp_event_base_t base, int32_t event_id, void *event_data){
	/* INFO
	 * TOPIC: 			event->topic      ==> do not directly use, string is not limited
	 * TOPIC LENGTH: 	event->topic_len
	 * DATA: 			event->data       ==> do not directly use, string is not limited
	 * DATA LENGTH: 	event->data_len
	 * CLIENT: 			event->client
	 */

    esp_mqtt_event_handle_t event = event_data;
    // cut strings to length
    char topic[event->topic_len+1]; char data[event->data_len+1];
    McuUtility_strcpy((unsigned char*)topic, sizeof(topic), (unsigned char*)event->topic);
    McuUtility_strcpy((unsigned char*)data, sizeof(data), (unsigned char*)event->data);

    bool handled = false;

    // check if event id is MQTT_EVENT_DATA => break if not
    if((esp_mqtt_event_id_t)event_id != MQTT_EVENT_DATA) {
        ESP_LOGE(TAG, "Wrong data received. Can only handle MQTT_EVENT_DATA (event id %i). But received event id: %i", MQTT_EVENT_DATA, (esp_mqtt_event_id_t)event_id);
        return;
    }

    /* check topic and call related command if allowed by robot mode */
    /* stationary robot allowed commands */
    if(Challenge_App_GetRobotMode() == true) {
    	// CMD: CONFIG SETUP
        if(McuUtility_strcmp(topic, MQTT_TOPIC_SF_CONFIG_SETUP)==0){
        	handled = true;
        	// parse json
        	int setupId = 0;
        	int hwId = 0;
        	struct json_attr_t json_attrs[] = {
        	    {"setupId",    t_integer, .addr.integer = &setupId},
        	    {"hardwareId", t_integer, .addr.integer = &hwId,},
        	    {NULL},
        	};
        	int err = json_read_object(data, json_attrs, NULL);
        	if(err != 0){
        		ESP_LOGE(TAG, "Parsing JSON data of CONFIG SETUP message failed. Event data was = %s", data);
        		ESP_LOGE(TAG, "Parse error was: %s", json_error_string(err));
        	} else{  // successfully parsed
        		// check values
        		if(setupId >= 0 && setupId <= 10 && hwId >= 0 && hwId <= 50){
    				SplitFlap_Wrapper_SetHardwareIdentifier(setupId, hwId);
        		} else{
        			ESP_LOGE(TAG, "SetupID or HardwareID out of range. Event data was = %s", data);
        		}
        	}
        }
        // CMD: INIT ALL
        else if(McuUtility_strcmp(topic, MQTT_TOPIC_SF_INITALL)==0){
        	handled = true;
        	SplitFlap_Wrapper_MoveAllToZeroPosition();
        }
        // CMD: DISPLAY
        else if(McuUtility_strcmp(topic, MQTT_TOPIC_SF_DISPLAY)==0){
        	handled = true;
        	// parse json
        	char message[10] = "TEST";
        	struct json_attr_t json_attrs[] = {
				{"message", t_string, .addr.string = message, .len = sizeof(message)},
				{NULL},
			};
        	int err = json_read_object(data, json_attrs, NULL);
        	if(err != 0){
				ESP_LOGE(TAG, "Parsing JSON data of DISPLAY message failed. Event data was = %s", data);
				ESP_LOGE(TAG, "Parse error was: %s", json_error_string(err));
			} else{  // successfully parsed
	        	SplitFlap_Wrapper_Display((unsigned char*)message);
			}
        }
    }
    /* mobile robot allowed commands */
    else{
    	// CMD: ROBO MOBILE MODE
    	if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_MODE)==0){
    		handled = true;
			// parse json
			bool modeAutomatic = false;
			struct json_attr_t json_attrs[] = {
				{"automatic",    t_boolean, .addr.boolean = &modeAutomatic},
				{NULL},
			};
			int err = json_read_object(data, json_attrs, NULL);
			if(err != 0){
				ESP_LOGE(TAG, "Parsing JSON data of ROBO MOBILE MODE message failed. Event data was = %s", data);
				ESP_LOGE(TAG, "Parse error was: %s", json_error_string(err));
			} else{  // successfully parsed
				Robo_Wrapper_SetMode(modeAutomatic);
			}
    	}
    	// CMD: ROBO MOBILE NAV TURN
    	else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_NAV_TURN)==0){
    		handled = true;
    		// data pointer for xatio
			const unsigned char *dataPtr = (unsigned char*)data;
			// parse value
    		int32_t angle = 0; // default value 0
    		McuUtility_xatoi(&dataPtr, &angle);
			if(angle == 0){
				ESP_LOGE(TAG, "Parsing data of ROBO MOBILE NAV TURN message failed. Event data was = %s", data);
			} else if (angle > 32767 || angle < -32768){ // check range
				ESP_LOGE(TAG, "Data of ROBO MOBILE NAV TURN was out of range. Event data was = %s", data);
			} else{  // successfully parsed
				Robo_Wrapper_Nav_Turn((int16_t)angle);
			}
    	}
    	// CMD: ROBO MOBILE NAV MOVE
		else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_NAV_MOVE)==0){
    		handled = true;
    		// data pointer for xatio
			const unsigned char *dataPtr = (unsigned char*)data;
			// parse value
    		int32_t speed = 40000; // default value => to identify
    		McuUtility_xatoi(&dataPtr, &speed);
			if(speed == 40000){
				ESP_LOGE(TAG, "Parsing data of ROBO MOBILE NAV MOVE message failed. Event data was = %s", data);
			} else if (speed > 32767 || speed < -32768){ // check range
				ESP_LOGE(TAG, "Data of ROBO MOBILE NAV MOVE was out of range. Event data was = %s", data);
			} else{  // successfully parsed
				Robo_Wrapper_Nav_Move((int16_t)speed);
			}
		}
    	// CMD: ROBO MOBILE NAV STOP
    	else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_NAV_STOP)==0){
        	handled = true;
        	Robo_Wrapper_Nav_Stop();
    	}
    	// CMD: ROBO_CALIB_CMD
    	else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_CALIB_CMD)==0){
        	handled = true;
			// parse json
			bool start = false;
			struct json_attr_t json_attrs[] = {
				{"start",    t_boolean, .addr.boolean = &start},
				{NULL},
			};
			int err = json_read_object(data, json_attrs, NULL);
			if(err != 0){
				ESP_LOGE(TAG, "Parsing JSON data of ROBO_CALIB_CMD message failed. Event data was = %s", data);
				ESP_LOGE(TAG, "Parse error was: %s", json_error_string(err));
			} else{  // successfully parsed
				Robo_Wrapper_Calibration(start);
			}
    	}
    	// CMD: ROBO_GET_CALIB
    	else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_GET_CALIB)==0){
        	handled = true;
        	LineSensorCalibData_t calibData;
        	Robo_Wrapper_GetCalibrationValues(&calibData);
        	ESP_LOGI(TAG, "Read calibration data: state=%s", calibData.state);
        	ESP_LOGI(TAG, "Read calibration data: values=%s", calibData.values);

        	// build json and respond
        	unsigned char payload[100] = "{\"state\": \"";
        	McuUtility_strcat(payload, sizeof(payload), calibData.state);
        	McuUtility_strcat(payload, sizeof(payload), (unsigned char*)"\", \"data\": \"");
        	McuUtility_strcat(payload, sizeof(payload), calibData.values);
        	McuUtility_strcat(payload, sizeof(payload), (unsigned char*)"\"}");

        	MyMqtt_Publish(MQTT_TOPIC_ROBO_CALIB_STATE, (char*)payload);
    	}
    }

    /* both robot modes allowed commands */
	// CMD: REQ_BATTERY
	if(McuUtility_strcmp(topic, MQTT_TOPIC_REQ_BATTERY)==0){
		handled = true;
		Challenge_Com_PublishBatteryVoltage();
	}



    if(handled == false){
        ESP_LOGE(TAG, "Received data could not be handled. Topic was %s", topic);
    }
    return;
}

void Challenge_Com_PublishBatteryVoltage(void){
	unsigned char voltage[20] = "";
	Robo_Wrapper_GetBatteryVoltage(voltage);
	ESP_LOGI(TAG, "Read battery voltage=%s", voltage);

	// build json and respond
	unsigned char payload[100] = "{\"voltage\": \"";
	McuUtility_strcat(payload, sizeof(payload), voltage);
	McuUtility_strcat(payload, sizeof(payload), (unsigned char*)"\"}");

	if(Challenge_App_GetRobotMode() == true){
		MyMqtt_Publish(MQTT_TOPIC_STAT_RESP_BATTERY, (char*)payload);
	}
	else{
		MyMqtt_Publish(MQTT_TOPIC_ROBO_RESP_BATTERY, (char*)payload);
	}
}

void Challenge_Com_SubscribeToAllTopics(void){
    /* subscribe to stationary robot interested topics */
	if(Challenge_App_GetRobotMode() == true){
		MyMqtt_Subscribe(MQTT_TOPIC_SF_INITALL);
		MyMqtt_Subscribe(MQTT_TOPIC_SF_DISPLAY);
		MyMqtt_Subscribe(MQTT_TOPIC_SF_CONFIG_SETUP);
	}
	/* subscribe to mobile robot interested topics */
	else{
		MyMqtt_Subscribe(MQTT_TOPIC_ROBO_MODE);
		MyMqtt_Subscribe(MQTT_TOPIC_ROBO_NAV_MOVE);
		MyMqtt_Subscribe(MQTT_TOPIC_ROBO_NAV_TURN);
		MyMqtt_Subscribe(MQTT_TOPIC_ROBO_NAV_STOP);
		MyMqtt_Subscribe(MQTT_TOPIC_ROBO_CALIB_CMD);
		MyMqtt_Subscribe(MQTT_TOPIC_ROBO_GET_CALIB);
	}
	/* subscribe to topics relevant for both modes */
	MyMqtt_Subscribe(MQTT_TOPIC_REQ_BATTERY);
}