/*
 * Copyright (c) 2017 Samsung Electronics Co., Ltd.
 *
 * Contact: Jeonghoon Park <jh1979.park@samsung.com>
 *
 * Licensed under the Flora License, Version 1.1 (the License);
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://floralicense.org/license/
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an AS IS BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <glib.h>
#include <service_app.h>
#include "log.h"
#include "resource.h"
#include "net-util.h"
#include "config.h"
#include "cloud/cloud_communication.h"
#include "messages/message_manager.h"
#include "controller_connection_manager.h"
#include "lap_counter/lap_counter.h"
#include "command.h"

#define ENABLE_MOTOR 1
#define STERING_SERVO_CENTER 340
#define STERING_SERVO_RANGE 125

#define MAX_UDP_INPUT 10000

#define CONFIG_GRP_CAR "Car"
#define CONFIG_GRP_STEERING "Steering"
#define CONFIG_GRP_CAMERA "Camera"
#define CONFIG_GRP_ENGINE "Engine"
#define CONFIG_GRP_RPI "Rpi"

#define CONFIG_KEY_ID "Id"
#define CONFIG_KEY_NAME "Name"

#define CONFIG_KEY_STEERING_CENTER "Center"
#define CONFIG_KEY_STEERING_RANGE "Range"
#define CONFIG_KEY_CAMERA_AZIMUTH_CENTER "Azimuth.Center"
#define CONFIG_KEY_CAMERA_AZIMUTH_RANGE "Azimuth.Range"
#define CONFIG_KEY_CAMERA_ELEVATION_MIN "Elevation.Min"
#define CONFIG_KEY_CAMERA_ELEVATION_MAX "Elevation.Max"
#define CONFIG_KEY_CAMERA_ENGINE_MIN "Min"
#define CONFIG_KEY_CAMERA_ENGINE_MAX "Max"

#define CONFIG_KEY_RPI_PIN_STERING "Stering"
#define CONFIG_KEY_RPI_PIN_CAMERA_AZIMUTH "Azimuth"
#define CONFIG_KEY_RPI_PIN_CAMERA_ELEVATION "Elevation"

#define CONFIG_KEY_RPI_PIN_ENGINE_1_FORWARD "1.forward"
#define CONFIG_KEY_RPI_PIN_ENGINE_1_BACK "1.back"
#define CONFIG_KEY_RPI_ENGINE_CHANNEL_1 "Channel.1"
#define CONFIG_KEY_RPI_PIN_ENGINE_2_FORWARD "2.forward"
#define CONFIG_KEY_RPI_PIN_ENGINE_2_BACK "2.back"
#define CONFIG_KEY_RPI_ENGINE_CHANNEL_2 "Channel.2"

#define CLOUD_REQUESTS_FREQUENCY 15

#define ELEVATION_MIN 200
#define ELEVATION_MAX 400
#define AZIMUTH_MIN 200
#define AZIMUTH_MAX 700

enum {
	DIR_STATE_S,
	DIR_STATE_F,
	DIR_STATE_B,
};

typedef struct app_data_s {
	unsigned int f_value;
	unsigned int r_value;
	unsigned int dir_state;
	const char *user_name;
	guint idle_h;

	int stering_center;
	int stering_range;

	int camera_azimuth_center;
	int camera_azimuth_range;

	int camera_elevation_min;
	int camera_elevation_max;

	int engine_min;
	int engine_max;

	int stering_pin;

	int elevation_pin;
	int azimuth_pin;

	int engine_1_forward_pin;
	int engine_1_back_pin;
	int engine_1_channel;

	int engine_2_forward_pin;
	int engine_2_back_pin;
	int engine_2_channel;
} app_data;

static app_data s_info = {
		.stering_center = STERING_SERVO_CENTER,
		.stering_range = STERING_SERVO_RANGE,

		.camera_azimuth_center = (AZIMUTH_MAX + AZIMUTH_MIN) /2,
		.camera_azimuth_range = AZIMUTH_MAX - ((AZIMUTH_MAX + AZIMUTH_MIN) /2),

		.camera_elevation_min = ELEVATION_MIN,
		.camera_elevation_max = ELEVATION_MAX,

		.engine_min = -4095,
		.engine_max = 4095,

		.stering_pin = 0,

		.elevation_pin = 14,
		.azimuth_pin = 15,

		.engine_1_forward_pin = 6,
		.engine_1_back_pin = 5,
		.engine_1_channel = 5,

		.engine_2_forward_pin = 21,
		.engine_2_back_pin = 20,
		.engine_2_channel = 4,
};

static void _initialize_components(app_data *ad);
static void _initialize_config();

static void service_app_lang_changed(app_event_info_h event_info, void *user_data)
{
	return;
}

static void service_app_region_changed(app_event_info_h event_info, void *user_data)
{
	return;
}

static void service_app_low_battery(app_event_info_h event_info, void *user_data)
{
	_E("low battery! exit app");
	service_app_exit();

	return;
}

static void service_app_low_memory(app_event_info_h event_info, void *user_data)
{
	return;
}

static inline double __map_round(double val)
{
	return floor(val + 0.5);
}

static int __map_range_val(int v_min, int v_max, int d_min, int d_max, int val)
{
	int rval = 0;
	double slope = 0;
	slope = 1.0 * (d_max - d_min) / (v_max - v_min);

	rval = d_min + __map_round(slope * (val - v_min));

	return rval;
}

static inline int ___map_speed_val(int speed)
{
	return __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT, s_info.engine_min, s_info.engine_max, speed);
}

static inline int ___map_servo_val(int servo)
{
	return __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT,
			s_info.stering_center - s_info.stering_range,
			s_info.stering_center + s_info.stering_range, servo);
}

static int __driving_motors(int servo, int speed)
{
	int val_speed;
	int val_servo;

	val_servo = ___map_servo_val(servo);
	val_speed = ___map_speed_val(speed);

	_D("control motor - servo[%4d : %4d], speed[%4d : %4d]",
		servo, val_servo, speed, val_speed);
#if ENABLE_MOTOR
	resource_set_servo_motor_value(0, val_servo);
	resource_set_motor_driver_L298N_speed(MOTOR_ID_1, val_speed);
	resource_set_motor_driver_L298N_speed(MOTOR_ID_2, val_speed);
#endif

	return 0;
}

static void __camera(int azimuth, int elevation)
{
	int val_azimuth = __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT,
			s_info.camera_azimuth_center - s_info.camera_azimuth_range,
			s_info.camera_azimuth_center + s_info.camera_azimuth_range, azimuth);

	int val_elevation = __map_range_val(0, MAX_UDP_INPUT, s_info.camera_elevation_min, s_info.camera_elevation_max, elevation); // No need to look upside down

	_D("camera - azimuth[%4d : %4d], elevation[%4d : %4d]", azimuth, val_azimuth, elevation, val_elevation);
#if ENABLE_MOTOR
	resource_set_servo_motor_value(s_info.elevation_pin, val_elevation);
	resource_set_servo_motor_value(s_info.azimuth_pin, val_azimuth);
#endif
}

static void __command_received_cb(command_s command) {
	switch(command.type) {
	case COMMAND_TYPE_DRIVE:
		__driving_motors(command.data.steering.direction, command.data.steering.speed);
		break;
	case COMMAND_TYPE_CAMERA:
		__camera(command.data.camera_position.camera_azimuth, command.data.camera_position.camera_elevation);
		break;
	case COMMAND_TYPE_DRIVE_AND_CAMERA:
		__driving_motors(command.data.steering_and_camera.direction, command.data.steering_and_camera.speed);
		__camera(command.data.steering_and_camera.camera_azimuth, command.data.steering_and_camera.camera_elevation);
		break;
	case COMMAND_TYPE_NONE:
		break;
	default:
		_E("Unknown command type");
		break;
	}
}

static void __user_name_received_cb(const char *name)
{
	_D("User name received: %s", name);
	lap_counter_set_user_name(name);
	lap_counter_set_start_lap();
}

static void _initialize_config()
{
	net_util_init();

	config_init();

	char *id = NULL;
	char *name = NULL;
	gboolean modified = false;

	if (config_get_string(CONFIG_GRP_CAR, CONFIG_KEY_ID, &id) != 0) {
		char *uuid = g_uuid_string_random();
		config_set_string(CONFIG_GRP_CAR, CONFIG_KEY_ID, uuid);
		g_free(uuid);
		modified = true;
	}

	char *uuid = g_uuid_string_random();
	modified |= config_get_string_or_set_default(CONFIG_GRP_CAR, CONFIG_KEY_ID, uuid, &id);
	g_free(uuid);

	modified |= config_get_string_or_set_default(CONFIG_GRP_CAR, CONFIG_KEY_NAME, "Passerati", &name);

	modified |= config_get_int_with_default(CONFIG_GRP_STEERING, CONFIG_KEY_STEERING_CENTER, s_info.stering_center, &s_info.stering_center);
	modified |= config_get_int_with_default(CONFIG_GRP_STEERING, CONFIG_KEY_STEERING_RANGE, s_info.stering_range, &s_info.stering_range);

	modified |= config_get_int_with_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_AZIMUTH_CENTER, s_info.camera_azimuth_center, &s_info.camera_azimuth_center);
	modified |= config_get_int_with_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_AZIMUTH_RANGE, s_info.camera_azimuth_range, &s_info.camera_azimuth_range);
	modified |= config_get_int_with_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_ELEVATION_MIN, s_info.camera_elevation_min, &s_info.camera_elevation_min);
	modified |= config_get_int_with_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_ELEVATION_MAX, s_info.camera_elevation_max, &s_info.camera_elevation_max);

	modified |= config_get_int_with_default(CONFIG_GRP_ENGINE, CONFIG_KEY_CAMERA_ENGINE_MIN, s_info.engine_min, &s_info.engine_min);
	modified |= config_get_int_with_default(CONFIG_GRP_ENGINE, CONFIG_KEY_CAMERA_ENGINE_MAX, s_info.engine_max, &s_info.engine_max);

	modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_STERING, s_info.stering_pin, &s_info.stering_pin);
	modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_CAMERA_AZIMUTH, s_info.azimuth_pin, &s_info.azimuth_pin);
	modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_CAMERA_ELEVATION, s_info.elevation_pin, &s_info.elevation_pin);

	modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_1_FORWARD, s_info.engine_1_forward_pin, &s_info.engine_1_forward_pin);
	modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_1_BACK, s_info.engine_1_back_pin, &s_info.engine_1_back_pin);
	modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_ENGINE_CHANNEL_1, s_info.engine_1_channel, &s_info.engine_1_channel);

	modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_2_FORWARD, s_info.engine_2_forward_pin, &s_info.engine_2_forward_pin);
	modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_2_BACK, s_info.engine_2_back_pin, &s_info.engine_2_back_pin);
	modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_ENGINE_CHANNEL_2, s_info.engine_2_channel, &s_info.engine_2_channel);

	if (modified == true) {
		config_save();
	}

	free(id);
	free(name);
}

static void _initialize_components(app_data *ad)
{
	net_util_init();
	_initialize_config();
	cloud_communication_init();
	message_manager_init();
	controller_connection_manager_listen();
	lap_counter_init();

	_D("Car settings: Stering[%d +/- %d] Engine[%d - %d], CamAzimuth[%d +/- %d], CamElev[%d - %d]",
			s_info.stering_center, s_info.stering_range,
			s_info.engine_min, s_info.engine_max,
			s_info.camera_azimuth_center, s_info.camera_azimuth_range,
			s_info.camera_elevation_min, s_info.camera_elevation_max);

	_D("RPI: Stering[%d], Azimuth[%d], Elev[%d]; ENG_1[%d, %d, %d]; ENG_2[%d, %d, %d];",
			s_info.stering_pin, s_info.azimuth_pin, s_info.elevation_pin,
			s_info.engine_1_forward_pin, s_info.engine_1_back_pin, s_info.engine_1_channel,
			s_info.engine_2_forward_pin, s_info.engine_2_back_pin, s_info.engine_2_channel);
}

static bool service_app_create(void *data)
{
	int ret = 0;
	app_data *ad = data;

	_D("-----------------------=======================================================================-----------------------");
	_D("-----------------------=======================================================================-----------------------");
	_D("-----------------------============================== APP %s %s ==============================-----------------------", __DATE__, __TIME__);
	_D("-----------------------=======================================================================-----------------------");
	_D("-----------------------=======================================================================-----------------------");

	/*
	 * if you want to use default configuration,
	 * Do not need to call resource_set_motor_driver_L298N_configuration(),
	 *
	*/
#if ENABLE_MOTOR
	ret = resource_set_motor_driver_L298N_configuration(MOTOR_ID_1, s_info.engine_1_forward_pin, s_info.engine_1_back_pin, s_info.engine_1_channel);
	if (ret) {
		_E("resource_set_motor_driver_L298N_configuration()");
		service_app_exit();
	}
	ret = resource_set_motor_driver_L298N_configuration(MOTOR_ID_2, s_info.engine_2_forward_pin, s_info.engine_2_back_pin, s_info.engine_2_channel);
	if (ret) {
		_E("resource_set_motor_driver_L298N_configuration()");
		service_app_exit();
	}
#endif

	_initialize_components(ad);
	resource_lap_counter_init();
	cloud_communication_start(CLOUD_REQUESTS_FREQUENCY);

	controller_connection_manager_set_command_received_cb(__command_received_cb);
	controller_connection_manager_set_user_name_received_cb(__user_name_received_cb);

	return true;
}

static void service_app_control(app_control_h app_control, void *data)
{

#if ENABLE_MOTOR
	/* set speed 0, to reduce delay of initializing motor driver */
	resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 0);
	resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 0);
	resource_set_servo_motor_value(s_info.stering_pin, STERING_SERVO_CENTER);
	resource_set_servo_motor_value(s_info.elevation_pin, ELEVATION_MIN);
	resource_set_servo_motor_value(s_info.azimuth_pin, (AZIMUTH_MIN + AZIMUTH_MAX) / 2);
#endif

	return;
}

static void service_app_terminate(void *data)
{
	app_data *ad = data;

	resource_set_servo_motor_value(s_info.stering_pin, STERING_SERVO_CENTER);
	resource_set_servo_motor_value(s_info.elevation_pin, ELEVATION_MIN);
	resource_set_servo_motor_value(s_info.azimuth_pin, (AZIMUTH_MIN + AZIMUTH_MAX) / 2);

	resource_lap_counter_destroy();

	if (ad->idle_h)
		g_source_remove(ad->idle_h);

	lap_counter_shutdown();
	controller_connection_manager_release();
	message_manager_shutdown();

	cloud_communication_stop();
	cloud_communication_fini();
	config_shutdown();
	net_util_fini();

	resource_close_all();
	log_file_close();

	_D("Bye ~");

	return;
}

int main(int argc, char* argv[])
{
	app_data *ad = NULL;
	int ret = 0;
	service_app_lifecycle_callback_s event_callback;
	app_event_handler_h handlers[5] = {NULL, };

	log_type_set(LOG_TYPE_DLOG);

	ad = calloc(1, sizeof(app_data));
	retv_if(!ad, -1);

	event_callback.create = service_app_create;
	event_callback.terminate = service_app_terminate;
	event_callback.app_control = service_app_control;

	service_app_add_event_handler(&handlers[APP_EVENT_LOW_BATTERY],
		APP_EVENT_LOW_BATTERY, service_app_low_battery, &ad);
	service_app_add_event_handler(&handlers[APP_EVENT_LOW_MEMORY],
		APP_EVENT_LOW_MEMORY, service_app_low_memory, &ad);
	service_app_add_event_handler(&handlers[APP_EVENT_LANGUAGE_CHANGED],
		APP_EVENT_LANGUAGE_CHANGED, service_app_lang_changed, &ad);
	service_app_add_event_handler(&handlers[APP_EVENT_REGION_FORMAT_CHANGED],
		APP_EVENT_REGION_FORMAT_CHANGED, service_app_region_changed, &ad);

	ret = service_app_main(argc, argv, &event_callback, ad);
	if (ret)
		_E("failed to start app");

	return 0;
}