/****************************************************************************
 *
 *   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.hpp
 *
 * Offline Task Manager for autonomous drone missions
 *
 * This module provides offline task planning and execution for drones.
 * It reads tasks from a local JSON file, plans the optimal path based on
 * task priority and battery level, generates PX4 mission items, and
 * switches to AUTO.MISSION mode for execution.
 *
 * @author PX4 Development Team
 */

#pragma once

#include "offline_task.hpp"
#include "offline_task_parser.hpp"
#include "offline_task_planner.hpp"

#include <dataman/dataman.h>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <navigator/navigation.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>

/* Include custom mode definitions for AUTO.MISSION */
#include <commander/px4_custom_mode.h>

namespace offline_task_manager
{

/**
 * @brief State machine states for offline task management
 */
enum class OTMState {
	IDLE = 0,			/**< Module is idle, waiting for enable */
	WAIT_FOR_HOME,			/**< Waiting for home position to be valid */
	WAIT_FOR_POSITION,		/**< Waiting for global position to be valid */
	LOAD_TASK_FILE,			/**< Loading tasks from JSON file */
	VALIDATE_TASKS,			/**< Validating loaded tasks */
	PLAN_TASKS,			/**< Planning task execution order */
	WRITE_MISSION,			/**< Writing mission items to dataman */
	START_MISSION,			/**< Starting mission execution */
	MONITOR_MISSION,		/**< Monitoring mission execution */
	COMPLETED,			/**< Mission completed successfully */
	FAILED				/**< Mission failed */
};

/**
 * @brief Offline Task Manager class
 *
 * Manages offline task planning and execution for drones.
 */
class OfflineTaskManager : public ModuleBase<OfflineTaskManager>,
			   public ModuleParams,
			   public px4::ScheduledWorkItem
{
public:
	/**
	 * @brief Constructor
	 */
	OfflineTaskManager();

	/**
	 * @brief Destructor
	 */
	~OfflineTaskManager() override;

	/**
	 * @brief Module instantiation (for ModuleBase compatibility)
	 */
	static OfflineTaskManager *instantiate(int argc, char *argv[])
	{
		return new OfflineTaskManager();
	}

	/**
	 * @brief Module initialization
	 * @return 0 on success, -1 on failure
	 */
	int init() { return 0; }

	/**
	 * @brief Main module entry point
	 * @param argc Argument count
	 * @param argv Argument values
	 * @return 0 on success, 1 on failure
	 */
	static int task_spawn(int argc, char *argv[]);

	/**
	 * @brief Print module usage
	 */
	static int print_usage(const char *reason);
	static int print_usage() { return print_usage(nullptr); }

	/**
	 * @brief Start the module
	 * @param argc Argument count
	 * @param argv Argument values
	 * @return 0 on success, 1 on failure
	 */
	static int custom_command(int argc, char *argv[]);

	/**
	 * @brief Print current module status
	 * @return 0
	 */
	int print_status() override;

	/**
	 * @brief Request module to stop
	 */
	void request_stop() { _task_should_exit.store(true); }

private:
	/**
	 * @brief Scheduled work item run function
	 */
	void Run() override;

	/**
	 * @brief Update simulated battery level
	 */
	void update_simulated_battery();

	/**
	 * @brief Get current battery percentage
	 * @return Battery percentage (0-100)
	 */
	float get_battery_percentage();

	/**
	 * @brief Load tasks from JSON file
	 * @return true if successful
	 */
	bool load_tasks();

	/**
	 * @brief Plan task execution order
	 * @return true if successful
	 */
	bool plan_tasks();

	/**
	 * @brief Write planned mission to dataman
	 * @return true if successful
	 */
	bool write_mission_to_dataman();

	/**
	 * @brief Start mission execution by sending AUTO.MISSION command
	 */
	void start_mission_execution();

	/**
	 * @brief Get string representation of state
	 * @param state State enum value
	 * @return State name string
	 */
	const char *state_to_string(OTMState state);

	/**
	 * @brief Reset state machine to idle
	 */
	void reset_state_machine();

	/**
	 * @brief Update uORB subscriptions
	 */
	void update_subscriptions();

	/**
	 * @brief Check if home position is valid
	 * @return true if valid
	 */
	bool home_position_valid() const;

	/**
	 * @brief Check if global position is valid
	 * @return true if valid
	 */
	bool global_position_valid() const;

	/**
	 * @brief Get current position
	 * @param lat Output latitude
	 * @param lon Output longitude
	 * @return true if position available
	 */
	bool get_current_position(double &lat, double &lon) const;

	/**
	 * @brief State transition helper
	 * @param new_state State to transition to
	 */
	void transition_to_state(OTMState new_state);

	/* uORB Subscriptions */
	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
	uORB::Subscription _home_pos_sub{ORB_ID(home_position)};

	/* uORB Publications */
	uORB::Publication<mission_s> _mission_pub{ORB_ID(mission)};
	uORB::Publication<vehicle_command_s> _vehicle_cmd_pub{ORB_ID(vehicle_command)};

	/* Cached uORB data */
	battery_status_s _battery_status{};
	vehicle_global_position_s _global_pos{};
	home_position_s _home_pos{};

	/* Simulated battery */
	float _sim_battery_percent{100.0f};
	hrt_abstime _last_battery_update{0};

	/* Task data */
	static constexpr int MAX_TASKS = 32;
	OfflineTask _raw_tasks[MAX_TASKS];
	int _raw_task_count{0};
	OfflineTask _planned_tasks[MAX_TASKS];
	int _planned_task_count{0};
	float _total_planned_distance{0.0f};

	/* State machine */
	OTMState _current_state{OTMState::IDLE};
	bool _mission_written{false};
	bool _mission_started{false};

	/* Module exit flag */
	px4::atomic_bool _task_should_exit{false};

	/* Performance counter */
	perf_counter_t _cycle_perf{nullptr};

	/* Parameters */
	DEFINE_PARAMETERS(
		(ParamInt<px4::params::OTM_ENABLE>) _param_otm_enable,
		(ParamInt<px4::params::OTM_START>) _param_otm_start,
		(ParamFloat<px4::params::OTM_HOLD_T>) _param_otm_hold_t,
		(ParamFloat<px4::params::OTM_ACC_RAD>) _param_otm_acc_rad,
		(ParamFloat<px4::params::OTM_BATT_INIT>) _param_otm_batt_init,
		(ParamInt<px4::params::OTM_BATT_SIM>) _param_otm_batt_sim,
		(ParamFloat<px4::params::OTM_BATT_DRAIN>) _param_otm_batt_drain,
		(ParamFloat<px4::params::OTM_LOW_BATT>) _param_otm_low_batt,
		(ParamInt<px4::params::OTM_MAX_TASKS>) _param_otm_max_tasks
	);
};

} // namespace offline_task_manager
