/****************************************************************************
 *
 *   Copyright (c) 2018 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 WindEstimator.hpp
 * A wind and airspeed scale estimator.
 */

#pragma once

#include <ecl.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>

class WindEstimator
{
public:
	WindEstimator() = default;
	~WindEstimator() = default;

	// no copy, assignment, move, move assignment
	WindEstimator(const WindEstimator &) = delete;
	WindEstimator &operator=(const WindEstimator &) = delete;
	WindEstimator(WindEstimator &&) = delete;
	WindEstimator &operator=(WindEstimator &&) = delete;

	void update(uint64_t time_now);

	void fuse_airspeed(uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI,
			   const matrix::Vector2f &velIvar);
	void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att);

	void get_wind(float wind[2])
	{
		wind[0] = _state(w_n);
		wind[1] = _state(w_e);
	}

	bool is_estimate_valid() { return _initialised; }

	bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
				       uint64_t &time_meas_rejected, bool &reinit_filter);

	float get_tas_scale() { return _state(tas); }
	float get_tas_innov() { return _tas_innov; }
	float get_tas_innov_var() { return _tas_innov_var; }
	float get_beta_innov() { return _beta_innov; }
	float get_beta_innov_var() { return _beta_innov_var; }
	void get_wind_var(float wind_var[2])
	{
		wind_var[0] = _P(0, 0);
		wind_var[1] = _P(1, 1);
	}
	bool get_wind_estimator_reset() { return _wind_estimator_reset; }

	void set_wind_p_noise(float wind_sigma) { _wind_p_var = wind_sigma * wind_sigma; }
	void set_tas_scale_p_noise(float tas_scale_sigma) { _tas_scale_p_var = tas_scale_sigma * tas_scale_sigma; }
	void set_tas_noise(float tas_sigma) { _tas_var = tas_sigma * tas_sigma; }
	void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; }
	void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; }
	void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; }

	// Inhibit learning of the airspeed scale factor and force the estimator to use _enforced_airspeed_scale as scale factor.
	// Negative input values enable learning of the airspeed scale factor.
	void enforce_airspeed_scale(float scale) {_enforced_airspeed_scale = scale; }

private:
	enum {
		w_n = 0,
		w_e,
		tas
	};	///< enum which can be used to access state.

	matrix::Vector3f _state;		///< state vector
	matrix::Matrix3f _P;		///< state covariance matrix

	float _tas_innov{0.0f};	///< true airspeed innovation
	float _tas_innov_var{0.0f};	///< true airspeed innovation variance

	float _beta_innov{0.0f};	///< sideslip innovation
	float _beta_innov_var{0.0f};	///< sideslip innovation variance

	bool _initialised{false};	///< True: filter has been initialised

	float _wind_p_var{0.1f};	///< wind process noise variance
	float _tas_scale_p_var{0.0001f};	///< true airspeed scale process noise variance
	float _tas_var{1.4f};		///< true airspeed measurement noise variance
	float _beta_var{0.5f};	///< sideslip measurement noise variance
	uint8_t _tas_gate{3};	///< airspeed fusion gate size
	uint8_t _beta_gate{1};	///< sideslip fusion gate size

	uint64_t _time_last_airspeed_fuse = 0;	///< timestamp of last airspeed fusion
	uint64_t _time_last_beta_fuse = 0;		///< timestamp of last sideslip fusion
	uint64_t _time_last_update = 0;			///< timestamp of last covariance prediction
	uint64_t _time_rejected_beta = 0;		///< timestamp of when sideslip measurements have consistently started to be rejected
	uint64_t _time_rejected_tas =
		0;		///<timestamp of when true airspeed measurements have consistently started to be rejected

	bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle
	float _enforced_airspeed_scale{-1.0f}; ///< by default we want to estimate the true airspeed scale factor (see enforce_airspeed_scale(...) )

	// initialise state and state covariance matrix
	bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas);

	void run_sanity_checks();
};