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

extern orb_advert_t mavlink_log_pub;

// required number of samples for sensor
// to initialize
static const int 		REQ_SONAR_INIT_COUNT = 10;
static const uint32_t 	SONAR_TIMEOUT =   5000000; // 2.0 s
static const float  	SONAR_MAX_INIT_STD =   0.3f; // meters

void BlockLocalPositionEstimator::sonarInit()
{
	// measure
	Vector<float, n_y_sonar> y;

	if (_sonarStats.getCount() == 0) {
		_time_init_sonar = _timeStamp;
	}

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

	// if finished
	if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) {
		if (_sonarStats.getStdDev()(0) > SONAR_MAX_INIT_STD) {
			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init std > min");
			_sonarStats.reset();

		} else if ((_timeStamp - _time_init_sonar) > SONAR_TIMEOUT) {
			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init timeout ");
			_sonarStats.reset();

		} else {
			PX4_INFO("[lpe] sonar init "
				 "mean %d cm std %d cm",
				 int(100 * _sonarStats.getMean()(0)),
				 int(100 * _sonarStats.getStdDev()(0)));
			_sonarInitialized = true;
			_sonarFault = FAULT_NONE;
		}
	}
}

int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
{
	// measure
	float d = _sub_sonar->get().current_distance;
	float eps = 0.01f; // 1 cm
	float min_dist = _sub_sonar->get().min_distance + eps;
	float max_dist = _sub_sonar->get().max_distance - eps;

	// prevent driver from setting min dist below eps
	if (min_dist < eps) {
		min_dist = eps;
	}

	// check for bad data
	if (d > max_dist || d < min_dist) {
		return -1;
	}

	// update stats
	_sonarStats.update(Scalarf(d));
	_time_last_sonar = _timeStamp;
	y.setZero();
	y(0) = (d + _sonar_z_offset.get()) *
	       cosf(_sub_att.get().roll) *
	       cosf(_sub_att.get().pitch);
	return OK;
}

void BlockLocalPositionEstimator::sonarCorrect()
{
	// measure
	Vector<float, n_y_sonar> y;

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

	// do not use sonar if lidar is active
	//if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) { return; }

	// calculate covariance
	float cov = _sub_sonar->get().covariance;

	if (cov < 1.0e-3f) {
		// use sensor value if reasoanble
		cov = _sonar_z_stddev.get() * _sonar_z_stddev.get();
	}

	// sonar measurement matrix and noise matrix
	Matrix<float, n_y_sonar, n_x> C;
	C.setZero();
	// y = -(z - tz)
	// TODO could add trig to make this an EKF correction
	C(Y_sonar_z, X_z) = -1; // measured altitude, negative down dir.
	C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir.

	// covariance matrix
	SquareMatrix<float, n_y_sonar> R;
	R.setZero();
	R(0, 0) = cov;

	// residual
	Vector<float, n_y_sonar> r = y - C * _x;
	_pub_innov.get().hagl_innov = r(0);
	_pub_innov.get().hagl_innov_var = R(0, 0);

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

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

	if (beta > BETA_TABLE[n_y_sonar]) {
		if (_sonarFault < FAULT_MINOR) {
			_sonarFault = FAULT_MINOR;
			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar fault,  beta %5.2f", double(beta));
		}

		// abort correction
		return;

	} else if (_sonarFault) {
		_sonarFault = FAULT_NONE;
		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar OK");
	}

	// kalman filter correction if no fault
	if (_sonarFault < fault_lvl_disable) {
		Matrix<float, n_x, n_y_sonar> K =
			_P * C.transpose() * S_I;
		Vector<float, n_x> dx = K * r;
		correctionLogic(dx);
		_x += dx;
		_P -= K * C * _P;
	}

}

void BlockLocalPositionEstimator::sonarCheckTimeout()
{
	if (_timeStamp - _time_last_sonar > SONAR_TIMEOUT) {
		if (_sonarInitialized) {
			_sonarInitialized = false;
			_sonarStats.reset();
			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar timeout ");
		}
	}
}


