/****************************************************************************
 *
 *   Copyright (c) 2015 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 RunwayTakeoff.cpp
 * Runway takeoff handling for fixed-wing UAVs with steerable wheels.
 *
 * @author Roman Bapst <roman@px4.io>
 * @author Andreas Antener <andreas@uaventure.com>
 */

#include <stdbool.h>
#include <stdint.h>
#include <math.h>

#include "RunwayTakeoff.h"
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>

using matrix::Vector2f;

namespace runwaytakeoff
{

RunwayTakeoff::RunwayTakeoff(ModuleParams *parent) :
	ModuleParams(parent),
	_state(),
	_initialized(false),
	_initialized_time(0),
	_init_yaw(0),
	_climbout(false)
{
}

void RunwayTakeoff::init(float yaw, double current_lat, double current_lon)
{
	_init_yaw = yaw;
	_initialized = true;
	_state = RunwayTakeoffState::THROTTLE_RAMP;
	_initialized_time = hrt_absolute_time();
	_climbout = true; // this is true until climbout is finished
	_start_wp(0) = (float)current_lat;
	_start_wp(1) = (float)current_lon;
}

void RunwayTakeoff::update(float airspeed, float alt_agl,
			   double current_lat, double current_lon, orb_advert_t *mavlink_log_pub)
{

	switch (_state) {
	case RunwayTakeoffState::THROTTLE_RAMP:
		if (hrt_elapsed_time(&_initialized_time) > _param_rwto_ramp_time.get() * 1e6f
		    || airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get() * 0.9f) {
			_state = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
		}

		break;

	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
		if (airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get()) {
			_state = RunwayTakeoffState::TAKEOFF;
			mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached");
		}

		break;

	case RunwayTakeoffState::TAKEOFF:
		if (alt_agl > _param_rwto_nav_alt.get()) {
			_state = RunwayTakeoffState::CLIMBOUT;

			/*
			 * If we started in heading hold mode, move the navigation start WP to the current location now.
			 * The navigator will take this as starting point to navigate towards the takeoff WP.
			 */
			if (_param_rwto_hdg.get() == 0) {
				_start_wp(0) = (float)current_lat;
				_start_wp(1) = (float)current_lon;
			}

			mavlink_log_info(mavlink_log_pub, "#Climbout");
		}

		break;

	case RunwayTakeoffState::CLIMBOUT:
		if (alt_agl > _param_fw_clmbout_diff.get()) {
			_climbout = false;
			_state = RunwayTakeoffState::FLY;
			mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint");
		}

		break;

	default:
		break;
	}
}

/*
 * Returns true as long as we're below navigation altitude
 */
bool RunwayTakeoff::controlYaw()
{
	// keep controlling yaw directly until we start navigation
	return _state < RunwayTakeoffState::CLIMBOUT;
}

/*
 * Returns pitch setpoint to use.
 *
 * Limited (parameter) as long as the plane is on runway. Otherwise
 * use the one from TECS
 */
float RunwayTakeoff::getPitch(float tecsPitch)
{
	if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
		return math::radians(_param_rwto_psp.get());
	}

	return tecsPitch;
}

/*
 * Returns the roll setpoint to use.
 */
float RunwayTakeoff::getRoll(float navigatorRoll)
{
	// until we have enough ground clearance, set roll to 0
	if (_state < RunwayTakeoffState::CLIMBOUT) {
		return 0.0f;
	}

	// allow some roll during climbout
	else if (_state < RunwayTakeoffState::FLY) {
		return math::constrain(navigatorRoll,
				       math::radians(-_param_rwto_max_roll.get()),
				       math::radians(_param_rwto_max_roll.get()));
	}

	return navigatorRoll;
}

/*
 * Returns the yaw setpoint to use.
 *
 * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the
 * runway. When it has enough ground clearance we start navigation towards WP.
 */
float RunwayTakeoff::getYaw(float navigatorYaw)
{
	if (_param_rwto_hdg.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) {
		return _init_yaw;

	} else {
		return navigatorYaw;
	}
}

/*
 * Returns the throttle setpoint to use.
 *
 * Ramps up in the beginning, until it lifts off the runway it is set to
 * parameter value, then it returns the TECS throttle.
 */
float RunwayTakeoff::getThrottle(float tecsThrottle)
{
	switch (_state) {
	case RunwayTakeoffState::THROTTLE_RAMP: {
			float throttle = (hrt_elapsed_time(&_initialized_time) / (float)_param_rwto_ramp_time.get() * 1e6f) *
					 _param_rwto_max_thr.get();
			return throttle < _param_rwto_max_thr.get() ?
			       throttle :
			       _param_rwto_max_thr.get();
		}

	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
		return _param_rwto_max_thr.get();

	default:
		return tecsThrottle;
	}
}

bool RunwayTakeoff::resetIntegrators()
{
	// reset integrators if we're still on runway
	return _state < RunwayTakeoffState::TAKEOFF;
}

/*
 * Returns the minimum pitch for TECS to use.
 *
 * In climbout we either want what was set on the waypoint (sp_min) but at least
 * the climbtout minimum pitch (parameter).
 * Otherwise use the minimum that is enforced generally (parameter).
 */
float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min)
{
	if (_state < RunwayTakeoffState::FLY) {
		return math::max(sp_min, climbout_min);
	}

	else {
		return min;
	}
}

/*
 * Returns the maximum pitch for TECS to use.
 *
 * Limited by parameter (if set) until climbout is done.
 */
float RunwayTakeoff::getMaxPitch(float max)
{
	// use max pitch from parameter if set (> 0.1)
	if (_state < RunwayTakeoffState::FLY && _param_rwto_max_pitch.get() > 0.1f) {
		return _param_rwto_max_pitch.get();
	}

	else {
		return max;
	}
}

/*
 * Returns the "previous" (start) WP for navigation.
 */
Vector2f RunwayTakeoff::getStartWP()
{
	return _start_wp;
}

void RunwayTakeoff::reset()
{
	_initialized = false;
	_state = RunwayTakeoffState::THROTTLE_RAMP;
}

}