#include "../BlockLocalPositionEstimator.hpp"
#include <systemlib/mavlink_log.h>
#include <matrix/math.hpp>

extern orb_advert_t mavlink_log_pub;

static const uint64_t 	TARGET_TIMEOUT =   2000000; // [us]

void BlockLocalPositionEstimator::landingTargetInit()
{
	if (_target_mode.get() == Target_Moving) {
		// target is in moving mode, do not initialize
		return;
	}

	Vector<float, n_y_target> y;

	if (landingTargetMeasure(y) == OK) {
		mavlink_and_console_log_info(&mavlink_log_pub, "Landing target init");
		_sensorTimeout &= ~SENSOR_LAND_TARGET;
		_sensorFault &= ~SENSOR_LAND_TARGET;
	}
}

int BlockLocalPositionEstimator::landingTargetMeasure(Vector<float, n_y_target> &y)
{
	if (_target_mode.get() == Target_Stationary) {
		if (_sub_landing_target_pose.get().rel_vel_valid) {
			y(0) = _sub_landing_target_pose.get().vx_rel;
			y(1) = _sub_landing_target_pose.get().vy_rel;
			_time_last_target = _timeStamp;

		} else {
			return -1;
		}

		return OK;

	}

	return -1;
}

void BlockLocalPositionEstimator::landingTargetCorrect()
{
	if (_target_mode.get() == Target_Moving) {
		// nothing to do in this mode
		return;
	}

	// measure
	Vector<float, n_y_target> y;

	if (landingTargetMeasure(y) != OK) { return; }

	// calculate covariance
	float cov_vx = _sub_landing_target_pose.get().cov_vx_rel;
	float cov_vy = _sub_landing_target_pose.get().cov_vy_rel;

	// use sensor value only if reasoanble
	if (cov_vx < _target_min_cov.get() || cov_vy < _target_min_cov.get()) {
		cov_vx = _target_min_cov.get();
		cov_vy = _target_min_cov.get();
	}

	// target measurement matrix and noise matrix
	Matrix<float, n_y_target, n_x> C;
	C.setZero();
	// residual = (y + vehicle velocity)
	// sign change because target velocitiy is -vehicle velocity
	C(Y_target_x, X_vx) = -1;
	C(Y_target_y, X_vy) = -1;

	// covariance matrix
	SquareMatrix<float, n_y_target> R;
	R.setZero();
	R(0, 0) = cov_vx;
	R(1, 1) = cov_vy;

	// residual
	Vector<float, n_y_target> r = y - C * _x;

	// residual covariance, (inverse)
	Matrix<float, n_y_target, n_y_target> S_I =
		inv<float, n_y_target>(C * _P * C.transpose() + R);

	// fault detection
	float beta = (r.transpose()  * (S_I * r))(0, 0);

	if (beta > BETA_TABLE[n_y_target]) {
		if (!(_sensorFault & SENSOR_LAND_TARGET)) {
			mavlink_and_console_log_info(&mavlink_log_pub, "Landing target fault, beta %5.2f", double(beta));
			_sensorFault |= SENSOR_LAND_TARGET;
		}

		// abort correction
		return;

	} else if (_sensorFault & SENSOR_LAND_TARGET) {
		_sensorFault &= ~SENSOR_LAND_TARGET;
		mavlink_and_console_log_info(&mavlink_log_pub, "Landing target OK");
	}

	// kalman filter correction
	Matrix<float, n_x, n_y_target> K =
		_P * C.transpose() * S_I;
	Vector<float, n_x> dx = K * r;
	_x += dx;
	_P -= K * C * _P;

}

void BlockLocalPositionEstimator::landingTargetCheckTimeout()
{
	if (_timeStamp - _time_last_target > TARGET_TIMEOUT) {
		if (!(_sensorTimeout & SENSOR_LAND_TARGET)) {
			_sensorTimeout |= SENSOR_LAND_TARGET;
			mavlink_and_console_log_info(&mavlink_log_pub, "Landing target timeout");
		}
	}
}
