/****************************************************************************
 *
 *   Copyright (c) 2019-2022 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.
 *
 ****************************************************************************/

#include "logged_topics.h"
#include "messages.h"

#include <parameters/param.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/px4_config.h>
#include <uORB/topics/uORBTopics.hpp>

#include <string.h>

using namespace px4::logger;

void LoggedTopics::add_default_topics()
{
	add_topic("action_request");
	add_topic("actuator_armed");
	add_optional_topic("actuator_controls_status_0", 300);
	add_topic("airspeed", 1000);
	add_optional_topic("airspeed_validated", 200);
	add_optional_topic("autotune_attitude_control_status", 100);
	add_topic_multi("battery_info", 5000, 3);
	add_optional_topic("camera_capture");
	add_optional_topic("camera_trigger");
	add_topic("cellular_status", 200);
	add_topic("commander_state");
	add_topic("config_overrides");
	add_topic("cpuload");
	add_topic("distance_sensor_mode_change_request");
	add_topic_multi("dronecan_node_status", 250);
	add_optional_topic("external_ins_attitude");
	add_optional_topic("external_ins_global_position");
	add_optional_topic("external_ins_local_position");
	// add_optional_topic("esc_status", 250);
	add_topic("esc_status");
	add_topic("failure_detector_status", 100);
	add_topic("failsafe_flags");
	add_optional_topic("follow_target", 500);
	add_optional_topic("follow_target_estimator", 200);
	add_optional_topic("follow_target_status", 400);
	add_optional_topic("flaps_setpoint", 1000);
	add_optional_topic("flight_phase_estimation", 1000);
	add_optional_topic("fuel_tank_status", 10);
	add_topic("gimbal_manager_set_attitude", 500);
	add_optional_topic("generator_status");
	add_optional_topic("gps_dump");
	add_optional_topic("gimbal_controls", 200);
	add_optional_topic("gripper");
	add_optional_topic("heater_status");
	add_topic("home_position");
	add_topic("hover_thrust_estimate", 100);
	add_topic("input_rc", 500);
	add_optional_topic("internal_combustion_engine_control", 10);
	add_optional_topic("internal_combustion_engine_status", 10);
	add_optional_topic("iridiumsbd_status", 1000);
	add_optional_topic("irlock_report", 1000);
	add_optional_topic("landing_gear", 200);
	add_optional_topic("landing_gear_wheel", 100);
	add_optional_topic("landing_target_pose", 1000);
	add_optional_topic("launch_detection_status", 200);
	add_optional_topic("magnetometer_bias_estimate", 200);
	add_topic("manual_control_setpoint", 200);
	add_topic("manual_control_switches");
	add_topic("mission_result");
	add_topic("navigator_mission_item");
	add_topic("navigator_status");
	add_topic("offboard_control_mode", 100);
	add_topic("onboard_computer_status", 10);
	add_topic("parameter_update");
	add_topic("position_controller_status", 500);
	add_topic("position_controller_landing_status", 100);
	add_optional_topic("pure_pursuit_status", 100);
	add_topic("goto_setpoint", 200);
	add_topic("position_setpoint_triplet", 200);
	add_optional_topic("px4io_status");
	add_topic("radio_status");
	add_optional_topic("rover_attitude_setpoint", 100);
	add_optional_topic("rover_attitude_status", 100);
	add_optional_topic("rover_position_setpoint", 100);
	add_optional_topic("rover_rate_setpoint", 100);
	add_optional_topic("rover_rate_status", 100);
	add_optional_topic("rover_speed_setpoint", 100);
	add_optional_topic("rover_speed_status", 100);
	add_optional_topic("rover_steering_setpoint", 100);
	add_optional_topic("rover_throttle_setpoint", 100);
	add_topic("rtl_time_estimate", 1000);
	add_topic("rtl_status", 2000);
	add_optional_topic("sensor_airflow", 100);
	add_topic("sensor_combined");
	add_optional_topic("sensor_correction");
	add_optional_topic("sensor_gyro_fft", 50);
	add_topic("sensor_selection");
	add_topic("sensors_status_imu", 200);
	add_optional_topic("sensor_temp", 10);
	add_optional_topic("spoilers_setpoint", 1000);
	add_topic("system_power", 500);
	add_optional_topic("takeoff_status", 1000);
	add_optional_topic("tecs_status", 200);
	add_optional_topic("tiltrotor_extra_controls", 100);
	add_topic("trajectory_setpoint", 200);
	add_topic("transponder_report");
	add_topic("vehicle_acceleration", 50);
	add_topic("vehicle_air_data", 200);
	add_topic("vehicle_angular_velocity", 20);
	add_topic("vehicle_attitude", 50);
	add_topic("vehicle_attitude_setpoint", 50);
	add_topic("vehicle_command");
	add_topic("vehicle_command_ack");
	add_topic("vehicle_constraints", 1000);
	add_topic("vehicle_control_mode");
	add_topic("vehicle_global_position", 200);
	add_topic("vehicle_gps_position", 100);
	add_topic("vehicle_land_detected");
	add_topic("vehicle_local_position", 100);
	add_topic("vehicle_local_position_setpoint", 100);
	add_topic("vehicle_magnetometer", 200);
	add_topic("vehicle_rates_setpoint", 20);
	add_topic("vehicle_roi", 1000);
	add_topic("vehicle_status");
	add_optional_topic("vtol_vehicle_status", 200);
	add_topic("wind", 1000);
	add_topic("fixed_wing_lateral_setpoint");
	add_topic("fixed_wing_longitudinal_setpoint");
	add_topic("longitudinal_control_configuration");
	add_topic("lateral_control_configuration");
	add_optional_topic("fixed_wing_lateral_guidance_status", 100);
	add_optional_topic("fixed_wing_lateral_status", 100);
	add_optional_topic("fixed_wing_runway_control", 100);

	// multi topics
	add_optional_topic_multi("actuator_outputs", 100, 3);
	add_optional_topic_multi("airspeed_wind", 1000, 4);
	add_optional_topic_multi("control_allocator_status", 200, 2);
	add_optional_topic_multi("rate_ctrl_status", 200, 2);
	add_optional_topic_multi("sensor_hygrometer", 500, 4);
	add_optional_topic_multi("rpm", 200);
	add_topic_multi("timesync_status", 1000, 3);
	add_optional_topic_multi("telemetry_status", 1000, 4);

	// EKF multi topics
	{
		// optionally log all estimator* topics at minimal rate
		const uint16_t kEKFVerboseIntervalMilliseconds = 500; // 2 Hz
		const struct orb_metadata *const *topic_list = orb_get_topics();

		for (size_t i = 0; i < orb_topics_count(); i++) {
			if (strncmp(topic_list[i]->o_name, "estimator", 9) == 0) {
				add_optional_topic_multi(topic_list[i]->o_name, kEKFVerboseIntervalMilliseconds);
			}
		}
	}

	// important EKF topics (higher rate)
	add_optional_topic("estimator_selector_status", 10);
	add_optional_topic_multi("estimator_event_flags", 10);
	add_optional_topic_multi("estimator_optical_flow_vel", 200);
	add_optional_topic_multi("estimator_sensor_bias", 1000);
	add_optional_topic_multi("estimator_status", 200);
	add_optional_topic_multi("estimator_status_flags", 10);
	add_optional_topic_multi("yaw_estimator_status", 1000);

	// log all raw sensors at minimal rate (at least 1 Hz)
	add_topic_multi("battery_status", 200, 3);
	add_topic_multi("differential_pressure", 1000, 2);
	add_topic_multi("distance_sensor", 1000, 2);
	add_optional_topic_multi("sensor_accel", 1000, 4);
	add_topic_multi("sensor_baro", 1000, 4);
	add_topic_multi("sensor_gps", 1000, 2);
	add_topic_multi("sensor_gnss_relative", 1000, 1);
	add_optional_topic_multi("sensor_gyro", 1000, 4);
	add_topic_multi("sensor_mag", 1000, 4);
	add_topic_multi("sensor_optical_flow", 1000, 2);

	add_topic_multi("vehicle_imu", 500, 4);
	add_topic_multi("vehicle_imu_status", 1000, 4);
	add_optional_topic_multi("vehicle_magnetometer", 500, 4);
	add_topic("vehicle_optical_flow", 500);
	add_topic("aux_global_position", 500);
	//add_optional_topic("vehicle_optical_flow_vel", 100);
	add_optional_topic("pps_capture");

	// additional control allocation logging
	add_topic("actuator_motors", 100);
	add_topic("actuator_servos", 100);
	add_topic_multi("vehicle_thrust_setpoint", 20, 2);
	add_topic_multi("vehicle_torque_setpoint", 20, 2);

	// SYS_HITL: default ground truth logging for simulation
	int32_t sys_hitl = 0;
	param_get(param_find("SYS_HITL"), &sys_hitl);

	if (sys_hitl >= 1) {
		add_topic("vehicle_angular_velocity_groundtruth", 10);
		add_topic("vehicle_attitude_groundtruth", 10);
		add_topic("vehicle_global_position_groundtruth", 100);
		add_topic("vehicle_local_position_groundtruth", 20);
	}

#ifdef CONFIG_ARCH_BOARD_PX4_SITL
	add_topic("fw_virtual_attitude_setpoint");
	add_topic("mc_virtual_attitude_setpoint");
	add_optional_topic("vehicle_torque_setpoint_virtual_mc");
	add_optional_topic("vehicle_torque_setpoint_virtual_fw");
	add_optional_topic("vehicle_thrust_setpoint_virtual_mc");
	add_optional_topic("vehicle_thrust_setpoint_virtual_fw");
	add_topic("time_offset");
	add_topic("vehicle_angular_velocity", 10);
	add_topic("vehicle_angular_velocity_groundtruth", 10);
	add_topic("vehicle_attitude_groundtruth", 10);
	add_topic("vehicle_global_position_groundtruth", 100);
	add_topic("vehicle_local_position_groundtruth", 20);

	// EKF replay
	{
		// optionally log all estimator* topics at minimal rate
		const uint16_t kEKFVerboseIntervalMilliseconds = 10; // 100 Hz
		const struct orb_metadata *const *topic_list = orb_get_topics();

		for (size_t i = 0; i < orb_topics_count(); i++) {
			if (strncmp(topic_list[i]->o_name, "estimator", 9) == 0) {
				add_optional_topic_multi(topic_list[i]->o_name, kEKFVerboseIntervalMilliseconds);
			}
		}
	}

	add_topic("vehicle_attitude");
	add_topic("vehicle_global_position");
	add_topic("vehicle_local_position");
	add_topic("wind");
	add_optional_topic_multi("yaw_estimator_status");

#endif /* CONFIG_ARCH_BOARD_PX4_SITL */

#ifdef CONFIG_BOARD_UAVCAN_INTERFACES
	add_topic_multi("can_interface_status", 100, CONFIG_BOARD_UAVCAN_INTERFACES);
#endif
}

void LoggedTopics::add_high_rate_topics()
{
	// maximum rate to analyze fast maneuvers (e.g. for racing)
	add_topic("manual_control_setpoint");
	add_topic_multi("rate_ctrl_status", 20, 2);
	add_topic("sensor_combined");
	add_topic("vehicle_angular_velocity");
	add_topic("vehicle_attitude");
	add_topic("vehicle_attitude_setpoint");
	add_topic("vehicle_rates_setpoint");

	add_topic("esc_status", 5);
	add_topic("actuator_motors");
	add_topic("actuator_outputs_debug");
	add_topic("actuator_servos");
	add_topic_multi("vehicle_thrust_setpoint", 0, 2);
	add_topic_multi("vehicle_torque_setpoint", 0, 2);
}

void LoggedTopics::add_debug_topics()
{
	add_topic("debug_array");
	add_topic("debug_key_value");
	add_topic("debug_value");
	add_topic("debug_vect");
	add_topic_multi("satellite_info", 1000, 2);
	add_topic("mag_worker_data");
	add_topic("sensor_preflight_mag", 500);
	add_topic("actuator_test", 500);
	add_topic("neural_control", 50);
}

void LoggedTopics::add_estimator_replay_topics()
{
	// for estimator replay (need to be at full rate)
	add_topic("ekf2_timestamps");

	// current EKF2 subscriptions
	add_topic("airspeed");
	add_topic("airspeed_validated");
	add_topic("vehicle_optical_flow");
	add_topic("sensor_combined");
	add_topic("sensor_selection");
	add_topic("vehicle_air_data");
	add_topic("vehicle_gps_position");
	add_topic("vehicle_land_detected");
	add_topic("vehicle_magnetometer");
	add_topic("vehicle_status");
	add_topic("vehicle_visual_odometry");
	add_topic("aux_global_position");
	add_topic_multi("distance_sensor");
}

void LoggedTopics::add_thermal_calibration_topics()
{
	add_topic_multi("sensor_accel", 100, 4);
	add_topic_multi("sensor_baro", 100, 4);
	add_topic_multi("sensor_gyro", 100, 4);
	add_topic_multi("sensor_mag", 100, 4);
}

void LoggedTopics::add_sensor_comparison_topics()
{
	add_topic_multi("sensor_accel", 100, 4);
	add_topic_multi("sensor_baro", 100, 4);
	add_topic_multi("sensor_gyro", 100, 4);
	add_topic_multi("sensor_mag", 100, 4);
}

void LoggedTopics::add_vision_and_avoidance_topics()
{
	add_topic("collision_constraints");
	add_topic_multi("distance_sensor");
	add_topic("obstacle_distance_fused");
	add_topic("obstacle_distance");
	add_topic("vehicle_mocap_odometry", 30);
	add_topic("vehicle_visual_odometry", 30);
}

void LoggedTopics::add_raw_imu_gyro_fifo()
{
	add_topic("sensor_gyro_fifo");
}

void LoggedTopics::add_raw_imu_accel_fifo()
{
	add_topic("sensor_accel_fifo");
}

void LoggedTopics::add_system_identification_topics()
{
	// for system id need to log imu and controls at full rate
	add_topic("sensor_combined");
	add_topic("vehicle_angular_velocity");
	add_topic("vehicle_torque_setpoint");
	add_topic("vehicle_acceleration");
	add_topic("actuator_motors");
}

void LoggedTopics::add_high_rate_sensors_topics()
{
	add_topic_multi("distance_sensor", 0, 4);
	add_topic_multi("sensor_optical_flow", 0, 2);
	add_topic_multi("sensor_gps", 0, 4);
	add_topic_multi("sensor_mag", 0, 4);
}

void LoggedTopics::add_mavlink_tunnel()
{
	add_topic("mavlink_tunnel");
}

int LoggedTopics::add_topics_from_file(const char *fname)
{
	int ntopics = 0;

	/* open the topic list file */
	FILE *fp = fopen(fname, "r");

	if (fp == nullptr) {
		return -1;
	}

	/* call add_topic for each topic line in the file */
	for (;;) {
		/* get a line, bail on error/EOF */
		char line[80];
		line[0] = '\0';

		if (fgets(line, sizeof(line), fp) == nullptr) {
			break;
		}

		/* skip comment lines */
		if ((strlen(line) < 2) || (line[0] == '#')) {
			continue;
		}

		// read line with format: <topic_name>[ <interval>[ <instance>]]
		char topic_name[80];
		uint32_t interval_ms = 0;
		uint32_t instance = 0;
		int nfields = sscanf(line, "%s %" PRIu32 " %" PRIu32, topic_name, &interval_ms, &instance);

		if (nfields > 0) {
			int name_len = strlen(topic_name);

			if (name_len > 0 && topic_name[name_len - 1] == ',') {
				topic_name[name_len - 1] = '\0';
			}

			/* add topic with specified interval_ms */
			if ((nfields > 2 && add_topic(topic_name, interval_ms, instance))
			    || add_topic_multi(topic_name, interval_ms)) {
				ntopics++;

			} else {
				PX4_ERR("Failed to add topic %s", topic_name);
			}
		}
	}

	fclose(fp);
	return ntopics;
}

void LoggedTopics::initialize_mission_topics(MissionLogType mission_log_type)
{
	if (mission_log_type == MissionLogType::Complete) {
		add_mission_topic("camera_capture");
		add_mission_topic("mission_result");
		add_mission_topic("vehicle_global_position", 1000);
		add_mission_topic("vehicle_status", 1000);

	} else if (mission_log_type == MissionLogType::Geotagging) {
		add_mission_topic("camera_capture");
	}
}

void LoggedTopics::add_mission_topic(const char *name, uint16_t interval_ms)
{
	if (add_topic(name, interval_ms)) {
		++_num_mission_subs;
	}
}

bool LoggedTopics::add_topic(const orb_metadata *topic, uint16_t interval_ms, uint8_t instance, bool optional)
{
	if (_subscriptions.count >= MAX_TOPICS_NUM) {
		PX4_WARN("Too many subscriptions, failed to add: %s %" PRIu8, topic->o_name, instance);
		return false;
	}

	if (optional && orb_exists(topic, instance) != 0) {
		PX4_DEBUG("Not adding non-existing optional topic %s %i", topic->o_name, instance);

		if (instance == 0 && _subscriptions.num_excluded_optional_topic_ids < MAX_EXCLUDED_OPTIONAL_TOPICS_NUM) {
			_subscriptions.excluded_optional_topic_ids[_subscriptions.num_excluded_optional_topic_ids++] = topic->o_id;
		}

		return false;
	}

	RequestedSubscription &sub = _subscriptions.sub[_subscriptions.count++];
	sub.interval_ms = interval_ms;
	sub.instance = instance;
	sub.id = static_cast<ORB_ID>(topic->o_id);
	return true;
}

bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t instance, bool optional)
{
	interval_ms /= _rate_factor;

	const orb_metadata *const *topics = orb_get_topics();
	bool success = false;

	for (size_t i = 0; i < orb_topics_count(); i++) {
		if (strcmp(name, topics[i]->o_name) == 0) {
			bool already_added = false;

			// check if already added: if so, only update the interval
			for (int j = 0; j < _subscriptions.count; ++j) {
				if (_subscriptions.sub[j].id == static_cast<ORB_ID>(topics[i]->o_id) &&
				    _subscriptions.sub[j].instance == instance) {

					PX4_DEBUG("logging topic %s(%" PRIu8 "), interval: %" PRIu16 ", already added, only setting interval",
						  topics[i]->o_name, instance, interval_ms);

					_subscriptions.sub[j].interval_ms = interval_ms;
					success = true;
					already_added = true;
					break;
				}
			}

			if (!already_added) {
				success = add_topic(topics[i], interval_ms, instance, optional);

				if (success) {
					PX4_DEBUG("logging topic: %s(%" PRIu8 "), interval: %" PRIu16, topics[i]->o_name, instance, interval_ms);
				}

				break;
			}
		}
	}

	return success;
}

bool LoggedTopics::add_topic_multi(const char *name, uint16_t interval_ms, uint8_t max_num_instances, bool optional)
{
	// add all possible instances
	for (uint8_t instance = 0; instance < max_num_instances; instance++) {
		add_topic(name, interval_ms, instance, optional);
	}

	return true;
}

bool LoggedTopics::initialize_logged_topics(SDLogProfileMask profile)
{
	int ntopics = add_topics_from_file(PX4_STORAGEDIR "/etc/logging/logger_topics.txt");

	if (ntopics > 0) {
		PX4_INFO("logging %d topics from logger_topics.txt", ntopics);

	} else {
		initialize_configured_topics(profile);
	}

	return _subscriptions.count > 0;
}

void LoggedTopics::initialize_configured_topics(SDLogProfileMask profile)
{
	// load appropriate topics for profile
	// the order matters: if several profiles add the same topic, the logging rate of the last one will be used
	if (profile & SDLogProfileMask::DEFAULT) {
		add_default_topics();
	}

	if (profile & SDLogProfileMask::ESTIMATOR_REPLAY) {
		add_estimator_replay_topics();
	}

	if (profile & SDLogProfileMask::THERMAL_CALIBRATION) {
		add_thermal_calibration_topics();
	}

	if (profile & SDLogProfileMask::SYSTEM_IDENTIFICATION) {
		add_system_identification_topics();
	}

	if (profile & SDLogProfileMask::HIGH_RATE) {
		add_high_rate_topics();
	}

	if (profile & SDLogProfileMask::DEBUG_TOPICS) {
		add_debug_topics();
	}

	if (profile & SDLogProfileMask::SENSOR_COMPARISON) {
		add_sensor_comparison_topics();
	}

	if (profile & SDLogProfileMask::VISION_AND_AVOIDANCE) {
		add_vision_and_avoidance_topics();
	}

	if (profile & SDLogProfileMask::RAW_IMU_GYRO_FIFO) {
		add_raw_imu_gyro_fifo();
	}

	if (profile & SDLogProfileMask::RAW_IMU_ACCEL_FIFO) {
		add_raw_imu_accel_fifo();
	}

	if (profile & SDLogProfileMask::MAVLINK_TUNNEL) {
		add_mavlink_tunnel();
	}

	if (profile & SDLogProfileMask::HIGH_RATE_SENSORS) {
		add_high_rate_sensors_topics();
	}
}