/****************************************************************************
 *
 *   Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.cpp
 * A terrain estimation kalman filter.
 */

#include "terrain_estimator.h"
#include <lib/ecl/geo/geo.h>
#include <px4_platform_common/defines.h>

#define DISTANCE_TIMEOUT 100000		// time in usec after which laser is considered dead

TerrainEstimator::TerrainEstimator() :
	_distance_last(0.0f),
	_terrain_valid(false),
	_time_last_distance(0),
	_time_last_gps(0)
{
	_x.zero();
	_u_z = 0.0f;
	_P.setIdentity();
}

bool TerrainEstimator::is_distance_valid(float distance)
{
	return (distance < 40.0f && distance > 0.00001f);
}

void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude,
			       const struct sensor_combined_s *sensor,
			       const struct distance_sensor_s *distance)
{
	matrix::Dcmf R_att = matrix::Quatf(attitude->q);
	matrix::Vector3f a{sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2]};
	matrix::Vector<float, 3> u;
	u = R_att * a;
	_u_z = u(2) + CONSTANTS_ONE_G; // compensate for gravity

	// dynamics matrix
	matrix::Matrix<float, n_x, n_x> A;
	A.setZero();
	A(0, 1) = 1;
	A(1, 2) = 1;

	// input matrix
	matrix::Matrix<float, n_x, 1>  B;
	B.setZero();
	B(1, 0) = 1;

	// input noise variance
	float R = 0.135f;

	// process noise convariance
	matrix::Matrix<float, n_x, n_x>  Q;
	Q(0, 0) = 0;
	Q(1, 1) = 0;

	// do prediction
	matrix::Vector<float, n_x>  dx = (A * _x) * dt;
	dx(1) += B(1, 0) * _u_z * dt;

	// propagate state and covariance matrix
	_x += dx;
	_P += (A * _P + _P * A.transpose() +
	       B * R * B.transpose() + Q) * dt;
}

void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps,
		const struct distance_sensor_s *distance,
		const struct vehicle_attitude_s *attitude)
{
	// terrain estimate is invalid if we have range sensor timeout
	if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) {
		_terrain_valid = false;
	}

	if (distance->timestamp > _time_last_distance) {
		matrix::Quatf q(attitude->q);
		matrix::Eulerf euler(q);
		float d = distance->current_distance;

		matrix::Matrix<float, 1, n_x> C;
		C(0, 0) = -1; // measured altitude,

		float R = 0.009f;

		matrix::Vector<float, 1> y;
		y(0) = d * cosf(euler.phi()) * cosf(euler.theta());

		// residual
		matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());
		S_I(0, 0) += R;
		S_I = matrix::inv<float, 1> (S_I);
		matrix::Vector<float, 1> r = y - C * _x;

		matrix::Matrix<float, n_x, 1> K = _P * C.transpose() * S_I;

		// some sort of outlayer rejection
		if (fabsf(distance->current_distance - _distance_last) < 1.0f) {
			_x += K * r;
			_P -= K * C * _P;
		}

		// if the current and the last range measurement are bad then we consider the terrain
		// estimate to be invalid
		if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) {
			_terrain_valid = false;

		} else {
			_terrain_valid = true;
		}

		_time_last_distance = distance->timestamp;
		_distance_last = distance->current_distance;
	}

	if (gps->timestamp > _time_last_gps && gps->fix_type >= 3) {
		matrix::Matrix<float, 1, n_x> C;
		C(0, 1) = 1;

		float R = 0.056f;

		matrix::Vector<float, 1> y;
		y(0) = gps->vel_d_m_s;

		// residual
		matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());
		S_I(0, 0) += R;
		S_I = matrix::inv<float, 1>(S_I);
		matrix::Vector<float, 1> r = y - C * _x;

		matrix::Matrix<float, n_x, 1> K = _P * C.transpose() * S_I;
		_x += K * r;
		_P -= K * C * _P;

		_time_last_gps = gps->timestamp;
	}

	// reinitialise filter if we find bad data
	bool reinit = false;

	for (int i = 0; i < n_x; i++) {
		if (!PX4_ISFINITE(_x(i))) {
			reinit = true;
		}
	}

	for (int i = 0; i < n_x; i++) {
		for (int j = 0; j < n_x; j++) {
			if (!PX4_ISFINITE(_P(i, j))) {
				reinit = true;
			}
		}
	}

	if (reinit) {
		_x.zero();
		_P.setZero();
		_P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f;
	}

}