/*
 * Possible Bug: We assume Home Position is already set by commander.
 */
Search::Search() {
	// Clear out stuct data
	memset(&search_mission, 0, sizeof (struct mission_s));
	memset(&next_point, 0, sizeof (struct mission_item_s));
	memset(&last, 0, sizeof (struct home_position_s));
	memset(&home, 0, sizeof (struct home_position_s));
	// Get Current Mission!
	int mission_sub = orb_subscribe(ORB_ID(offboard_mission));
	orb_copy(ORB_ID(offboard_mission), mission_sub, &search_mission);
	orb_unsubscribe(mission_sub);
	// Set Start Position to Home Position
	int home_sub = orb_subscribe(ORB_ID(home_position));
	orb_copy(ORB_ID(home_position), home_sub, &home);
	orb_unsubscribe(home_sub);
	// Read in the last waypoint on the offboard mission, store it to a local variable
	dm_read(DM_KEY_WAYPOINTS_OFFBOARD(offboard), 0, &last, sizeof (struct mission_item_s));
	offboard = 1;
	// Initialize Next Waypoint
	next_point.altitude_is_relative = true;
	next_point.altitude = home.alt + 7;
	next_point.autocontinue = true;
	next_point.nav_cmd = NAV_CMD_TAKEOFF;
	next_point.acceptance_radius = 5;
	next_point.lat = home.lat;
	next_point.lon = home.lon;
	next_point.time_inside = 0;
	next_point.frame = last.frame;
	mission_pub = orb_advertise(ORB_ID(offboard_mission), &search_mission);
}

CRSFTelemetry::~CRSFTelemetry()
{
	orb_unsubscribe(_vehicle_gps_position_sub);
	orb_unsubscribe(_battery_status_sub);
	orb_unsubscribe(_vehicle_attitude_sub);
	orb_unsubscribe(_vehicle_status_sub);
}

bool MicroBenchORB::time_px4_uorb()
{
	int fd_status = orb_subscribe(ORB_ID(vehicle_status));
	int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position));
	int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro));

	int ret = 0;
	bool updated = false;
	uint64_t time = 0;

	PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 1000);
	PERF("orb_stat vehicle_status", ret = orb_stat(fd_status, &time), 1000);
	PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status), fd_status, &status), 1000);

	PERF("orb_check vehicle_local_position", ret = orb_check(fd_lpos, &updated), 1000);
	PERF("orb_stat vehicle_local_position", ret = orb_stat(fd_lpos, &time), 1000);
	PERF("orb_copy vehicle_local_position", ret = orb_copy(ORB_ID(vehicle_local_position), fd_lpos, &lpos), 1000);

	PERF("orb_check sensor_gyro", ret = orb_check(fd_gyro, &updated), 1000);
	PERF("orb_stat sensor_gyro", ret = orb_stat(fd_gyro, &time), 1000);
	PERF("orb_copy sensor_gyro", ret = orb_copy(ORB_ID(sensor_gyro), fd_gyro, &lpos), 1000);

	PERF("orb_exists sensor_accel 0", ret = orb_exists(ORB_ID(sensor_accel), 0), 100);
	PERF("orb_exists sensor_accel 1", ret = orb_exists(ORB_ID(sensor_accel), 1), 100);
	PERF("orb_exists sensor_accel 2", ret = orb_exists(ORB_ID(sensor_accel), 2), 100);
	PERF("orb_exists sensor_accel 3", ret = orb_exists(ORB_ID(sensor_accel), 3), 100);
	PERF("orb_exists sensor_accel 4", ret = orb_exists(ORB_ID(sensor_accel), 4), 100);

	orb_unsubscribe(fd_status);
	orb_unsubscribe(fd_lpos);
	orb_unsubscribe(fd_gyro);

	return true;
}

InputMavlinkROI::~InputMavlinkROI()
{
	if (_vehicle_roi_sub >= 0) {
		orb_unsubscribe(_vehicle_roi_sub);
	}

	if (_position_setpoint_triplet_sub >= 0) {
		orb_unsubscribe(_position_setpoint_triplet_sub);
	}
}

OutputBase::~OutputBase()
{
	if (_vehicle_attitude_sub >= 0) {
		orb_unsubscribe(_vehicle_attitude_sub);
	}

	if (_vehicle_global_position_sub >= 0) {
		orb_unsubscribe(_vehicle_global_position_sub);
	}
}

UavcanNode::~UavcanNode()
{

	fw_server(Stop);

	if (_task != -1) {

		/* tell the task we want it to go away */
		_task_should_exit = true;

		unsigned i = 10;

		do {
			/* wait 5ms - it should wake every 10ms or so worst-case */
			usleep(5000);

			/* if we have given up, kill it */
			if (--i == 0) {
				task_delete(_task);
				break;
			}

		} while (_task != -1);
	}

	(void)orb_unsubscribe(_armed_sub);
	(void)orb_unsubscribe(_test_motor_sub);
	(void)orb_unsubscribe(_actuator_direct_sub);

	// Removing the sensor bridges
	auto br = _sensor_bridges.getHead();

	while (br != nullptr) {
		auto next = br->getSibling();
		delete br;
		br = next;
	}

	_instance = nullptr;

	perf_free(_perfcnt_node_spin_elapsed);
	perf_free(_perfcnt_esc_mixer_output_elapsed);
	perf_free(_perfcnt_esc_mixer_total_elapsed);
	pthread_mutex_destroy(&_node_mutex);
	px4_sem_destroy(&_server_command_sem);

	// Is it allowed to delete it like that?
	if (_mixers != nullptr) {
		delete _mixers;
	}
}

int
UavcanNode::teardown()
{
	px4_sem_post(&_server_command_sem);

	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
		if (_control_subs[i] > 0) {
			orb_unsubscribe(_control_subs[i]);
			_control_subs[i] = -1;
		}
	}

	return (_armed_sub >= 0) ? orb_unsubscribe(_armed_sub) : 0;
}

OutputBase::~OutputBase()
{
	if (_vehicle_attitude_sub >= 0) {
		orb_unsubscribe(_vehicle_attitude_sub);
	}

	if (_vehicle_global_position_sub >= 0) {
		orb_unsubscribe(_vehicle_global_position_sub);
	}

	if (_mount_orientation_pub) {
		orb_unadvertise(_mount_orientation_pub);
	}
}

// return false if the magnetomer measurements are inconsistent
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
{
	// get the sensor preflight data
	int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
	struct sensor_preflight_s sensors = {};

	if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != 0) {
		// can happen if not advertised (yet)
		return true;
	}

	orb_unsubscribe(sensors_sub);

	// Use the difference between sensors to detect a bad calibration, orientation or magnetic interference.
	// If a single sensor is fitted, the value being checked will be zero so this check will always pass.
	float test_limit;
	param_get(param_find("COM_ARM_MAG"), &test_limit);

	if (sensors.mag_inconsistency_ga > test_limit) {
		if (report_status) {
			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT");
		}

		return false;
	}

	return true;
}

void
UavcanNode::subscribe()
{
	// Subscribe/unsubscribe to required actuator control groups
	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;

	// the first fd used by CAN
	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
		if (sub_groups & (1 << i)) {
			warnx("subscribe to actuator_controls_%d", i);
			_control_subs[i] = orb_subscribe(_control_topics[i]);
		}

		if (unsub_groups & (1 << i)) {
			warnx("unsubscribe from actuator_controls_%d", i);
			orb_unsubscribe(_control_subs[i]);
			_control_subs[i] = -1;
		}

		if (_control_subs[i] > 0) {
			_poll_ids[i] = add_poll_fd(_control_subs[i]);
		}
	}
}

void TAP_ESC::work_stop()
{
	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
		if (_control_subs[i] > 0) {
			orb_unsubscribe(_control_subs[i]);
			_control_subs[i] = -1;
		}
	}

	orb_unsubscribe(_armed_sub);
	_armed_sub = -1;
	orb_unsubscribe(_test_motor_sub);
	_test_motor_sub = -1;

	DEVICE_LOG("stopping");
	_initialized = false;
}

void
UavcanNode::print_info()
{
	if (!_instance) {
		warnx("not running, start first");
	}

	(void)pthread_mutex_lock(&_node_mutex);

	// ESC mixer status
	printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
	       (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
	printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");

	if (_outputs.noutputs != 0) {
		printf("ESC output: ");

		for (uint8_t i = 0; i < _outputs.noutputs; i++) {
			printf("%d ", (int)(_outputs.output[i] * 1000));
		}

		printf("\n");

		// ESC status
		int esc_sub = orb_subscribe(ORB_ID(esc_status));
		struct esc_status_s esc;
		memset(&esc, 0, sizeof(esc));
		orb_copy(ORB_ID(esc_status), esc_sub, &esc);

		printf("ESC Status:\n");
		printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n");

		for (uint8_t i = 0; i < _outputs.noutputs; i++) {
			printf("%d\t",    esc.esc[i].esc_address);
			printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
			printf("%3.2f\t", (double)esc.esc[i].esc_current);
			printf("%3.2f\t", (double)esc.esc[i].esc_temperature);
			printf("%3.2f\t", (double)esc.esc[i].esc_setpoint);
			printf("%d\t",    esc.esc[i].esc_rpm);
			printf("%d",      esc.esc[i].esc_errorcount);
			printf("\n");
		}

		orb_unsubscribe(esc_sub);
	}

	// Sensor bridges
	auto br = _sensor_bridges.getHead();

	while (br != nullptr) {
		printf("Sensor '%s':\n", br->get_name());
		br->print_status();
		printf("\n");
		br = br->getSibling();
	}

	(void)pthread_mutex_unlock(&_node_mutex);
}

int uORBTest::UnitTest::test_multi2()
{

	test_note("Testing multi-topic 2 test (queue simulation)");
	//test: first subscribe, then advertise

	_thread_should_exit = false;
	const int num_instances = 3;
	int orb_data_fd[num_instances];
	int orb_data_next = 0;

	for (int i = 0; i < num_instances; ++i) {
//		PX4_WARN("subscribe %i, t=%" PRIu64, i, hrt_absolute_time());
		orb_data_fd[i] = orb_subscribe_multi(ORB_ID(orb_test_medium_multi), i);
	}

	char *const args[1] = { NULL };
	int pubsub_task = px4_task_spawn_cmd("uorb_test_multi",
					     SCHED_DEFAULT,
					     SCHED_PRIORITY_MAX - 5,
					     1500,
					     (px4_main_t)&uORBTest::UnitTest::pub_test_multi2_entry,
					     args);

	if (pubsub_task < 0) {
		return test_fail("failed launching task");
	}

	hrt_abstime last_time = 0;

	while (!_thread_should_exit) {

		bool updated = false;
		int orb_data_cur_fd = orb_data_fd[orb_data_next];
		orb_check(orb_data_cur_fd, &updated);

		if (updated) {
			struct orb_test_medium msg;
			orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg);
			usleep(1000);

			if (last_time >= msg.time && last_time != 0) {
				return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time);
			}

			last_time = msg.time;

//			PX4_WARN("      got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time);
			orb_data_next = (orb_data_next + 1) % num_instances;
		}
	}

	for (int i = 0; i < num_instances; ++i) {
		orb_unsubscribe(orb_data_fd[i]);
	}

	return test_note("PASS multi-topic 2 test (queue simulation)");
}

MavlinkULog::~MavlinkULog()
{
	if (_ulog_stream_ack_pub) {
		orb_unadvertise(_ulog_stream_ack_pub);
	}
	if (_ulog_stream_sub >= 0) {
		orb_unsubscribe(_ulog_stream_sub);
	}
}

MavlinkParametersManager::~MavlinkParametersManager()
{
	if (_uavcan_parameter_value_sub >= 0) {
		orb_unsubscribe(_uavcan_parameter_value_sub);
	}

	if (_uavcan_parameter_request_pub) {
		orb_unadvertise(_uavcan_parameter_request_pub);
	}
}

static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
{
	bool success = true; // start with a pass and change to a fail if any test fails
	float test_limit = 1.0f; // pass limit re-used for each test

	// Get sensor_preflight data if available and exit with a fail recorded if not
	int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
	sensor_preflight_s sensors = {};

	if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != PX4_OK) {
		goto out;
	}

	// Use the difference between IMU's to detect a bad calibration.
	// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
	param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);

	if (sensors.accel_inconsistency_m_s_s > test_limit) {
		if (report_status) {
			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL");
		}

		success = false;
		goto out;

	} else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) {
		if (report_status) {
			mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: ACCELS INCONSISTENT - CHECK CAL");
		}
	}

	// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
	param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);

	if (sensors.gyro_inconsistency_rad_s > test_limit) {
		if (report_status) {
			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL");
		}

		success = false;
		goto out;

	} else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) {
		if (report_status) {
			mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: GYROS INCONSISTENT - CHECK CAL");
		}
	}

out:
	orb_unsubscribe(sensors_sub);
	return success;
}

void listener(const orb_id_t &id, unsigned num_msgs, unsigned topic_instance, unsigned topic_interval)
{
	if (orb_exists(id, topic_instance) != 0) {
		printf("never published\n");
		return;
	}

	int sub = orb_subscribe_multi(id, topic_instance);
	orb_set_interval(sub, topic_interval);

	bool updated = false;
	unsigned i = 0;
	hrt_abstime start_time = hrt_absolute_time();

	while (i < num_msgs) {
		orb_check(sub, &updated);

		if (i == 0) {
			updated = true;

		} else {
			usleep(500);
		}

		if (updated) {
			start_time = hrt_absolute_time();
			i++;

			printf("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i);

			T container;

			if (orb_copy(id, sub, &container) == PX4_OK) {
				print_message(container);

			} else {
				PX4_ERR("orb_copy failed");
			}

		} else {
			if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) {
				printf("Waited for 2 seconds without a message. Giving up.\n");
				break;
			}
		}
	}

	orb_unsubscribe(sub);
}

static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool prearm)
{
	bool success = true;

	if (!prearm) {
		// Ignore power check after arming.
		return true;

	} else {
		int system_power_sub = orb_subscribe(ORB_ID(system_power));

		system_power_s system_power;

		if (orb_copy(ORB_ID(system_power), system_power_sub, &system_power) == PX4_OK) {

			if (hrt_elapsed_time(&system_power.timestamp) < 200000) {

				/* copy avionics voltage */
				float avionics_power_rail_voltage = system_power.voltage5V_v;

				// avionics rail
				// Check avionics rail voltages
				if (avionics_power_rail_voltage < 4.5f) {
					success = false;

					if (report_fail) {
						mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage);
					}

				} else if (avionics_power_rail_voltage < 4.9f) {
					if (report_fail) {
						mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage);
					}

				} else if (avionics_power_rail_voltage > 5.4f) {
					if (report_fail) {
						mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)avionics_power_rail_voltage);
					}
				}
			}
		}

		orb_unsubscribe(system_power_sub);
	}

	return success;
}

void Search::printDebug() {
	int mission_sub = orb_subscribe(ORB_ID(offboard_mission));
	orb_copy(ORB_ID(offboard_mission), mission_sub, &true_mission);
	orb_unsubscribe(mission_sub);
	// Timing for Navigator message to display properly
	usleep(20000);
	warnx("Mission count: %d", true_mission.count);
	warnx("Mission current sequence: %d", search_mission.current_seq);
	warnx("Start Point Latitude: %.7f", next_point.lat);
	warnx("Start Point Longitude: %.7f", next_point.lon);
	warnx("Last Point Frame is: %d", last.frame);
	warnx("Home Latitude: %.7f", home.lat);
	warnx("Home Longitude: %.7f", home.lon);
	if ((home.lat - next_point.lat) < 0.0001 && (home.lon - next_point.lon) < 0.0001)
		warnx("Start Point correctly copied Home's Lat/Lon!");
	else
		warnx("Start Point did not copy Home's Lat/Lon");
	warnx("========================================================");
	warnx("Last Waypoint Latitude: %.7f", last.lat);
	warnx("Last Waypoint Longitude: %.7f", last.lon);
}

MavlinkOrbSubscription::~MavlinkOrbSubscription()
{
	if (_fd >= 0) {
		orb_unsubscribe(_fd);
	}
}