/****************************************************************************
 *
 *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file offline_task_manager.cpp
 *
 * Offline Task Manager implementation
 */

#include "offline_task_manager.hpp"

#include <px4_platform_common/log.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/posix.h>
#include <stdio.h>
#include <string.h>

/* Task file path */
#define OTM_TASK_FILE_PATH "/tmp/offline_tasks.json"

namespace offline_task_manager

{

OfflineTaskManager::OfflineTaskManager() :
	ModuleParams(nullptr),
	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
	_cycle_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": cycle");

	/* Initialize simulated battery from parameter */
	_sim_battery_percent = _param_otm_batt_init.get();
	_last_battery_update = hrt_absolute_time();

	PX4_INFO("offline_task_manager initialized, sim battery: %.1f%%", (double)_sim_battery_percent);
}

OfflineTaskManager::~OfflineTaskManager()
{
	perf_free(_cycle_perf);
}

void OfflineTaskManager::Run()
{
	if (_task_should_exit.load()) {
		ScheduleClear();
		exit_and_cleanup();
		return;
	}

	perf_begin(_cycle_perf);

	/* Update parameters */
	ModuleParams::updateParams();

	/* Update simulated battery */
	update_simulated_battery();

	/* Update uORB subscriptions */
	update_subscriptions();

	/* Check if module is enabled */
	if (_param_otm_enable.get() == 0) {
		if (_current_state != OTMState::IDLE) {
			PX4_INFO("module disabled, returning to idle");
			reset_state_machine();
		}
		perf_end(_cycle_perf);
		ScheduleDelayed(1000000); /* 1 second delay when idle */
		return;
	}

	/* Check for start trigger */
	if (_param_otm_start.get() == 0) {
		if (_current_state != OTMState::IDLE && _current_state != OTMState::COMPLETED
		    && _current_state != OTMState::FAILED) {
			PX4_INFO("start disabled, returning to idle");
			reset_state_machine();
		}
		perf_end(_cycle_perf);
		ScheduleDelayed(500000); /* 0.5 second delay when waiting */
		return;
	}

	/* State machine execution */
	switch (_current_state) {
	case OTMState::IDLE:
		/* Transition to wait for home when start is enabled */
		if (_param_otm_start.get() != 0) {
			PX4_INFO("starting offline task execution");
			transition_to_state(OTMState::WAIT_FOR_HOME);
		}
		break;

	case OTMState::WAIT_FOR_HOME:
		if (home_position_valid()) {
			PX4_INFO("home position valid (%.6f, %.6f)",
				 (double)_home_pos.lat, (double)_home_pos.lon);
			transition_to_state(OTMState::WAIT_FOR_POSITION);
		} else {
			static hrt_abstime last_warn = 0;
			hrt_abstime now = hrt_absolute_time();
			if (now - last_warn > 3000000) { /* 3 seconds */
				PX4_INFO("waiting for home position...");
				last_warn = now;
			}
		}
		break;

	case OTMState::WAIT_FOR_POSITION:
		if (global_position_valid()) {
			PX4_INFO("global position valid (%.6f, %.6f)",
				 (double)_global_pos.lat, (double)_global_pos.lon);
			transition_to_state(OTMState::LOAD_TASK_FILE);
		} else {
			static hrt_abstime last_warn = 0;
			hrt_abstime now = hrt_absolute_time();
			if (now - last_warn > 3000000) { /* 3 seconds */
				PX4_INFO("waiting for global position...");
				last_warn = now;
			}
		}
		break;

	case OTMState::LOAD_TASK_FILE:
		if (load_tasks()) {
			transition_to_state(OTMState::VALIDATE_TASKS);
		} else {
			transition_to_state(OTMState::FAILED);
		}
		break;

	case OTMState::VALIDATE_TASKS:
		if (_raw_task_count > 0) {
			PX4_INFO("validated %d tasks", _raw_task_count);
			transition_to_state(OTMState::PLAN_TASKS);
		} else {
			PX4_ERR("no valid tasks to execute");
			transition_to_state(OTMState::FAILED);
		}
		break;

	case OTMState::PLAN_TASKS:
		if (plan_tasks()) {
			transition_to_state(OTMState::WRITE_MISSION);
		} else {
			PX4_ERR("task planning failed");
			transition_to_state(OTMState::FAILED);
		}
		break;

	case OTMState::WRITE_MISSION:
		if (write_mission_to_dataman()) {
			transition_to_state(OTMState::START_MISSION);
		} else {
			PX4_ERR("mission writing failed");
			transition_to_state(OTMState::FAILED);
		}
		break;

	case OTMState::START_MISSION:
		start_mission_execution();
		transition_to_state(OTMState::MONITOR_MISSION);
		break;

	case OTMState::MONITOR_MISSION:
		/* TODO: Implement mission monitoring
		 * For now, just stay in this state and print status periodically
		 */
		{
			static hrt_abstime last_status = 0;
			hrt_abstime now = hrt_absolute_time();
			if (now - last_status > 5000000) { /* 5 seconds */
				PX4_INFO("mission monitoring: %d tasks planned, battery: %.1f%%",
					 _planned_task_count, (double)get_battery_percentage());
				last_status = now;
			}
		}
		/* TODO: Detect mission completion and transition to COMPLETED */
		break;

	case OTMState::COMPLETED:
		/* Mission completed successfully - stay here */
		break;

	case OTMState::FAILED:
		/* Mission failed - stay here for debugging */
		break;
	}

	perf_end(_cycle_perf);

	/* Schedule next run - 100ms when active, 1s when idle */
	if (_current_state == OTMState::IDLE ||
	    _current_state == OTMState::COMPLETED ||
	    _current_state == OTMState::FAILED) {
		ScheduleDelayed(1000000);
	} else {
		ScheduleDelayed(100000);
	}
}

void OfflineTaskManager::update_simulated_battery()
{
	if (_param_otm_batt_sim.get() == 0) {
		/* Not using simulation - battery updated from subscription */
		return;
	}

	hrt_abstime now = hrt_absolute_time();
	float elapsed_seconds = (now - _last_battery_update) / 1000000.0f;

	if (elapsed_seconds > 0.0f) {
		float drain = _param_otm_batt_drain.get() * elapsed_seconds;
		_sim_battery_percent -= drain;

		/* Clamp to 0-100 range */
		if (_sim_battery_percent < 0.0f) {
			_sim_battery_percent = 0.0f;
		}
		if (_sim_battery_percent > 100.0f) {
			_sim_battery_percent = 100.0f;
		}
	}

	_last_battery_update = now;
}

float OfflineTaskManager::get_battery_percentage()
{
	if (_param_otm_batt_sim.get() != 0) {
		return _sim_battery_percent;
	}

	/* Use real battery status */
	if (_battery_status_sub.update(&_battery_status)) {
		/* battery_status.remaining is 0.0 to 1.0 */
		return _battery_status.remaining * 100.0f;
	}

	/* No update available - return last known value or warning */
	static bool warned = false;
	if (!warned) {
		PX4_WARN("no battery status available, using last known value");
		warned = true;
	}

	return _sim_battery_percent;
}

void OfflineTaskManager::update_subscriptions()
{
	_battery_status_sub.update(&_battery_status);
	_global_pos_sub.update(&_global_pos);
	_home_pos_sub.update(&_home_pos);
}

bool OfflineTaskManager::home_position_valid() const
{
	return _home_pos.valid_hpos;
}

bool OfflineTaskManager::global_position_valid() const
{
	return _global_pos.timestamp > 0 &&
	       PX4_ISFINITE(_global_pos.lat) &&
	       PX4_ISFINITE(_global_pos.lon);
}

bool OfflineTaskManager::get_current_position(double &lat, double &lon) const
{
	if (global_position_valid()) {
		lat = _global_pos.lat;
		lon = _global_pos.lon;
		return true;
	}
	return false;
}

bool OfflineTaskManager::load_tasks()
{
	PX4_INFO("loading tasks from %s", OTM_TASK_FILE_PATH);

	int max_tasks = _param_otm_max_tasks.get();
	if (max_tasks > MAX_TASKS) {
		max_tasks = MAX_TASKS;
	}

	bool success = load_offline_tasks_from_json(
		       OTM_TASK_FILE_PATH,
		       _raw_tasks,
		       max_tasks,
		       _raw_task_count);

	return success;
}

bool OfflineTaskManager::plan_tasks()
{
	double start_lat, start_lon;
	if (!get_current_position(start_lat, start_lon)) {
		PX4_ERR("cannot get current position for planning");
		return false;
	}

	float battery = get_battery_percentage();
	float low_batt = _param_otm_low_batt.get();

	PX4_INFO("starting task planning: battery=%.1f%% low_thresh=%.1f%%",
		 (double)battery, (double)low_batt);

	int max_planned = _param_otm_max_tasks.get();
	if (max_planned > MAX_TASKS) {
		max_planned = MAX_TASKS;
	}

	int result = plan_offline_tasks(
		     _raw_tasks,
		     _raw_task_count,
		     start_lat,
		     start_lon,
		     battery,
		     low_batt,
		     _planned_tasks,
		     max_planned,
		     _total_planned_distance);

	if (result < 0) {
		PX4_ERR("task planning failed");
		return false;
	}

	_planned_task_count = result;
	return true;
}

bool OfflineTaskManager::write_mission_to_dataman()
{
	if (_planned_task_count <= 0) {
		PX4_ERR("no tasks to write");
		return false;
	}

	PX4_INFO("writing %d mission items to dataman", _planned_task_count);

	dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0;

	/* Lock mission state for atomic update */
	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);

	/* Write each mission item */
	for (int i = 0; i < _planned_task_count; i++) {
		mission_item_s item{};

		item.nav_cmd = NAV_CMD_WAYPOINT;
		item.lat = _planned_tasks[i].lat;
		item.lon = _planned_tasks[i].lon;
		item.altitude = _planned_tasks[i].alt;
		item.altitude_is_relative = false;
		item.acceptance_radius = _param_otm_acc_rad.get();
		item.time_inside = _param_otm_hold_t.get();
		item.loiter_radius = 0.0f;
		item.yaw = NAN;
		item.autocontinue = true;
		item.origin = ORIGIN_ONBOARD;
		item.frame = NAV_FRAME_GLOBAL;

		/* Write to dataman */
		ssize_t written = dm_write(
				  dm_item,
				  i,
				  DM_PERSIST_POWER_ON_RESET,
				  &item,
				  sizeof(mission_item_s));

		if (written != sizeof(mission_item_s)) {
			PX4_ERR("failed to write mission item %d", i);
			if (dm_lock_ret == 0) {
				dm_unlock(DM_KEY_MISSION_STATE);
			}
			return false;
		}

		PX4_DEBUG("wrote mission item %d: (%.6f, %.6f) alt=%.1f",
			  i, (double)item.lat, (double)item.lon, (double)item.altitude);
	}

	/* Create and write mission state */
	mission_s mission{};
	mission.timestamp = hrt_absolute_time();
	mission.dataman_id = (uint8_t)dm_item;
	mission.count = (uint16_t)_planned_task_count;
	mission.current_seq = 0;

	ssize_t res = dm_write(
		      DM_KEY_MISSION_STATE,
		      0,
		      DM_PERSIST_POWER_ON_RESET,
		      &mission,
		      sizeof(mission_s));

	if (dm_lock_ret == 0) {
		dm_unlock(DM_KEY_MISSION_STATE);
	}

	if (res != sizeof(mission_s)) {
		PX4_ERR("failed to write mission state");
		return false;
	}

	/* Publish mission topic to notify navigator */
	_mission_pub.publish(mission);

	_mission_written = true;

	PX4_INFO("offline mission published: count=%d dataman_id=%d",
		 _planned_task_count, (int)dm_item);

	return true;
}

void OfflineTaskManager::start_mission_execution()
{
	PX4_INFO("sending ARM and AUTO.MISSION commands");

	/* Step 1: Send ARM command */
	vehicle_command_s arm_cmd{};
	arm_cmd.timestamp = hrt_absolute_time();
	arm_cmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
	arm_cmd.param1 = 1.0f;  /* 1 = arm, 0 = disarm */
	arm_cmd.param2 = 0.0f;  /* 0 = normal arm, 21196 = force arm (not recommended) */
	arm_cmd.target_system = 1;
	arm_cmd.target_component = 1;
	arm_cmd.source_system = 1;
	arm_cmd.source_component = 1;
	arm_cmd.from_external = false;

	_vehicle_cmd_pub.publish(arm_cmd);
	PX4_INFO("arm command sent");

	/* Step 2: Send MISSION mode command */
	vehicle_command_s mode_cmd{};
	mode_cmd.timestamp = hrt_absolute_time();
	mode_cmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
	mode_cmd.param1 = 1.0f;
	mode_cmd.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
	mode_cmd.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
	mode_cmd.target_system = 1;
	mode_cmd.target_component = 1;
	mode_cmd.source_system = 1;
	mode_cmd.source_component = 1;
	mode_cmd.from_external = false;

	_vehicle_cmd_pub.publish(mode_cmd);
	PX4_INFO("AUTO.MISSION command sent");

	_mission_started = true;
}

void OfflineTaskManager::transition_to_state(OTMState new_state)
{
	PX4_INFO("state transition: %s -> %s",
		 state_to_string(_current_state),
		 state_to_string(new_state));

	_current_state = new_state;
}

void OfflineTaskManager::reset_state_machine()
{
	_current_state = OTMState::IDLE;
	_mission_written = false;
	_mission_started = false;
	_raw_task_count = 0;
	_planned_task_count = 0;
}

const char *OfflineTaskManager::state_to_string(OTMState state)
{
	switch (state) {
	case OTMState::IDLE:
		return "IDLE";
	case OTMState::WAIT_FOR_HOME:
		return "WAIT_FOR_HOME";
	case OTMState::WAIT_FOR_POSITION:
		return "WAIT_FOR_POSITION";
	case OTMState::LOAD_TASK_FILE:
		return "LOAD_TASK_FILE";
	case OTMState::VALIDATE_TASKS:
		return "VALIDATE_TASKS";
	case OTMState::PLAN_TASKS:
		return "PLAN_TASKS";
	case OTMState::WRITE_MISSION:
		return "WRITE_MISSION";
	case OTMState::START_MISSION:
		return "START_MISSION";
	case OTMState::MONITOR_MISSION:
		return "MONITOR_MISSION";
	case OTMState::COMPLETED:
		return "COMPLETED";
	case OTMState::FAILED:
		return "FAILED";
	default:
		return "UNKNOWN";
	}
}

int OfflineTaskManager::print_status()
{
	PX4_INFO("Offline Task Manager Status:");
	PX4_INFO("  State: %s", state_to_string(_current_state));
	PX4_INFO("  Enabled: %s", _param_otm_enable.get() ? "yes" : "no");
	PX4_INFO("  Start: %s", _param_otm_start.get() ? "yes" : "no");
	PX4_INFO("  Battery: %.1f%%", (double)get_battery_percentage());
	PX4_INFO("  Home valid: %s", home_position_valid() ? "yes" : "no");
	PX4_INFO("  Position valid: %s", global_position_valid() ? "yes" : "no");
	PX4_INFO("  Raw tasks: %d", _raw_task_count);
	PX4_INFO("  Planned tasks: %d", _planned_task_count);
	PX4_INFO("  Mission written: %s", _mission_written ? "yes" : "no");
	PX4_INFO("  Mission started: %s", _mission_started ? "yes" : "no");

	if (_planned_task_count > 0) {
		PX4_INFO("  Planned distance: %.1fm", (double)_total_planned_distance);
	}

	perf_print_counter(_cycle_perf);

	return 0;
}

/* Static module interface functions */

int OfflineTaskManager::print_usage(const char *reason)
{
	if (reason) {
		PX4_WARN("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
Offline Task Manager module for autonomous drone missions.

This module reads task points from a local JSON file, plans the optimal
execution path based on task priority and battery level, generates PX4
mission items, and switches to AUTO.MISSION mode for execution.

### Features
- JSON task file parsing
- Priority-based task filtering
- Nearest neighbor path planning
- Battery-aware task selection
- Mission item generation and dataman storage
- Auto mode switching

### Usage
Start the module:
$ offline_task_manager start

Enable and start execution:
$ param set OTM_ENABLE 1
$ param set OTM_START 1

Check status:
$ offline_task_manager status

Stop the module:
$ offline_task_manager stop

### JSON File Format
Create /tmp/offline_tasks.json:
{
  "version": 1,
  "mission_id": "test_mission",
  "start_mode": "offline_auto",
  "tasks": [
    {
      "task_id": 1,
      "lat": 47.3978,
      "lon": 8.5456,
      "alt": 520.0,
      "priority": 5,
      "must_first": true
    }
  ]
}
)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("offline_task_manager", "module");

	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_ARG("Start the background task", nullptr, false);

	PRINT_MODULE_USAGE_COMMAND("stop");
	PRINT_MODULE_USAGE_ARG("Stop the background task", nullptr, false);

	PRINT_MODULE_USAGE_COMMAND("status");
	PRINT_MODULE_USAGE_ARG("Print module status", nullptr, false);

	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
	return 0;
}

int OfflineTaskManager::custom_command(int argc, char *argv[])
{
	if (!strcmp(argv[0], "start")) {
		return task_spawn(argc, argv);
	}

	if (!is_running()) {
		print_usage("module not running");
		return 1;
	}

	if (!strcmp(argv[0], "stop")) {
		get_instance()->request_stop();
		return 0;
	}

	if (!strcmp(argv[0], "status")) {
		return get_instance()->print_status();
	}

	print_usage("unknown command");
	return 1;
}

int OfflineTaskManager::task_spawn(int argc, char *argv[])
{
	// For ScheduledWorkItem modules, create instance directly and schedule it
	OfflineTaskManager *instance = new OfflineTaskManager();

	if (!instance) {
		PX4_ERR("alloc failed");
		return 1;
	}

	if (instance->init() != 0) {
		PX4_ERR("init failed");
		delete instance;
		return 1;
	}

	// Store in ModuleBase (atomic)
	_object.store(instance);

	// Mark as running on work queue (required for is_running() to return true)
	_task_id = task_id_is_work_queue;

	// Schedule the work item (100ms interval)
	instance->ScheduleOnInterval(100000);

	return 0;
}

} /* namespace offline_task_manager */

/* Module entry point */
extern "C" __EXPORT int offline_task_manager_main(int argc, char *argv[])
{
	return offline_task_manager::OfflineTaskManager::main(argc, argv);
}
