#include "Kalman.h"
#include <iostream>
#include <vector>

//---------------------------------------------------------------------------
TKalmanFilter::TKalmanFilter(
        tracking::KalmanType type,
	    bool useAcceleration,
        track_t deltaTime, // time increment (lower values makes target more "massive")
        track_t accelNoiseMag
        )
    :
      m_type(type),
      m_initialized(false),
      m_deltaTime(deltaTime),
      m_deltaTimeMin(deltaTime),
      m_deltaTimeMax(2 * deltaTime),
      m_lastDist(0),
      m_accelNoiseMag(accelNoiseMag),
	  m_useAcceleration(useAcceleration)
{
    m_deltaStep = (m_deltaTimeMax - m_deltaTimeMin) / m_deltaStepsCount;
}

//---------------------------------------------------------------------------
void TKalmanFilter::CreateLinear(cv::Point3f xy0, cv::Point3f xyv0)
{
    // We don't know acceleration, so, assume it to process noise.
    // But we can guess, the range of acceleration values thich can be achieved by tracked object.
    // Process noise. (standard deviation of acceleration: m/s^2)
    // shows, woh much target can accelerate.

    // 4 state variables, 2 measurements
    m_linearKalman.init(4, 2, 0, El_t);
    // Transition cv::Matrix
    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(4, 4) <<
                                        1, 0, m_deltaTime, 0,
                                        0, 1, 0, m_deltaTime,
                                        0, 0, 1, 0,
                                        0, 0, 0, 1);

    // init...
    m_lastPointResult = xy0;
    m_linearKalman.statePre.at<track_t>(0) = xy0.x;  // x
    m_linearKalman.statePre.at<track_t>(1) = xy0.y;  // y
    m_linearKalman.statePre.at<track_t>(2) = xyv0.x; // vx
    m_linearKalman.statePre.at<track_t>(3) = xyv0.y; // vy

    m_linearKalman.statePost.at<track_t>(0) = xy0.x;
    m_linearKalman.statePost.at<track_t>(1) = xy0.y;
    m_linearKalman.statePost.at<track_t>(2) = xyv0.x;
    m_linearKalman.statePost.at<track_t>(3) = xyv0.y;

    cv::setIdentity(m_linearKalman.measurementMatrix);

    m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(4, 4) <<
                                       pow(m_deltaTime,4.0)/4.0	,0						,pow(m_deltaTime,3.0)/2.0		,0,
                                       0						,pow(m_deltaTime,4.0)/4.0	,0							,pow(m_deltaTime,3.0)/2.0,
                                       pow(m_deltaTime,3.0)/2.0	,0						,pow(m_deltaTime,2.0)			,0,
                                       0						,pow(m_deltaTime,3.0)/2.0	,0							,pow(m_deltaTime,2.0));


    m_linearKalman.processNoiseCov *= m_accelNoiseMag;

    cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));

    cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));

    m_initialized = true;
}
//---------------------------------------------------------------------------
void TKalmanFilter::CreateLinear(cv::Rect_<track_t> rect0, Point_t rectv0)
{
    // We don't know acceleration, so, assume it to process noise.
    // But we can guess, the range of acceleration values thich can be achieved by tracked object.
    // Process noise. (standard deviation of acceleration: m/s^2)
    // shows, woh much target can accelerate.

    // 8 state variables (x, y, vx, vy, width, height, vw, vh), 4 measurements (x, y, width, height)
    m_linearKalman.init(8, 4, 0, El_t);
    // Transition cv::Matrix
    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(8, 8) <<
                                        1, 0, 0, 0, m_deltaTime, 0,           0,           0,
                                        0, 1, 0, 0, 0,           m_deltaTime, 0,           0,
                                        0, 0, 1, 0, 0,           0,           m_deltaTime, 0,
                                        0, 0, 0, 1, 0,           0,           0,           m_deltaTime,
                                        0, 0, 0, 0, 1,           0,           0,           0,
                                        0, 0, 0, 0, 0,           1,           0,           0,
                                        0, 0, 0, 0, 0,           0,           1,           0,
                                        0, 0, 0, 0, 0,           0,           0,           1);

    // init...
    m_linearKalman.statePre.at<track_t>(0) = rect0.x;      // x
    m_linearKalman.statePre.at<track_t>(1) = rect0.y;      // y
    m_linearKalman.statePre.at<track_t>(2) = rect0.width;  // width
    m_linearKalman.statePre.at<track_t>(3) = rect0.height; // height
    m_linearKalman.statePre.at<track_t>(4) = rectv0.x;     // dx
    m_linearKalman.statePre.at<track_t>(5) = rectv0.y;     // dy
    m_linearKalman.statePre.at<track_t>(6) = 0;            // dw
    m_linearKalman.statePre.at<track_t>(7) = 0;            // dh

    m_linearKalman.statePost.at<track_t>(0) = rect0.x;
    m_linearKalman.statePost.at<track_t>(1) = rect0.y;
    m_linearKalman.statePost.at<track_t>(2) = rect0.width;
    m_linearKalman.statePost.at<track_t>(3) = rect0.height;
    m_linearKalman.statePost.at<track_t>(4) = rectv0.x;
    m_linearKalman.statePost.at<track_t>(5) = rectv0.y;
    m_linearKalman.statePost.at<track_t>(6) = 0;
    m_linearKalman.statePost.at<track_t>(7) = 0;

    cv::setIdentity(m_linearKalman.measurementMatrix);

    track_t n1 = pow(m_deltaTime, 4.f) / 4.f;
    track_t n2 = pow(m_deltaTime, 3.f) / 2.f;
    track_t n3 = pow(m_deltaTime, 2.f);
    m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(8, 8) <<
                                       n1, 0,  0,  0,  n2, 0,  0,  0,
                                       0,  n1, 0,  0,  0,  n2, 0,  0,
                                       0,  0,  n1, 0,  0,  0,  n2, 0,
                                       0,  0,  0,  n1, 0,  0,  0,  n2,
                                       n2, 0,  0,  0,  n3, 0,  0,  0,
                                       0,  n2, 0,  0,  0,  n3, 0,  0,
                                       0,  0,  n2, 0,  0,  0,  n3, 0,
                                       0,  0,  0,  n2, 0,  0,  0,  n3);

    m_linearKalman.processNoiseCov *= m_accelNoiseMag;

    cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));

    cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));

    m_initialized = true;
}
//---------------------------------------------------------------------------
void TKalmanFilter::CreateLinear3D(Rect3D rect0, cv::Point3f rectv0)
{
    // We don't know acceleration, so, assume it to process noise.
    // But we can guess, the range of acceleration values thich can be achieved by tracked object.
    // Process noise. (standard deviation of acceleration: m/s^2)
    // shows, woh much target can accelerate.

    // 14 state variables (x, y, z, width, height, length, yaw, d...), 7 measurements (x, y, z, width, height, length, yaw)
    m_linearKalman.init(14, 7, 0, El_t);
    // Transition cv::Matrix
    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(14, 14) <<
                                        1, 0, 0, 0, 0, 0, 0, m_deltaTime, 0,           0,           0,           0,           0,           0,
                                        0, 1, 0, 0, 0, 0, 0, 0,           m_deltaTime, 0,           0,           0,           0,           0,
                                        0, 0, 1, 0, 0, 0, 0, 0,           0,           m_deltaTime, 0,           0,           0,           0,
                                        0, 0, 0, 1, 0, 0, 0, 0,           0,           0,           m_deltaTime, 0,           0,           0,
                                        0, 0, 0, 0, 1, 0, 0, 0,           0,           0,           0,           m_deltaTime, 0,           0,
                                        0, 0, 0, 0, 0, 1, 0, 0,           0,           0,           0,           0,           m_deltaTime, 0,
                                        0, 0, 0, 0, 0, 0, 1, 0,           0,           0,           0,           0,           0,           m_deltaTime,
                                        0, 0, 0, 0, 0, 0, 0, 1,           0,           0,           0,           0,           0,           0,
                                        0, 0, 0, 0, 0, 0, 0, 0,           1,           0,           0,           0,           0,           0,
                                        0, 0, 0, 0, 0, 0, 0, 0,           0,           1,           0,           0,           0,           0,
                                        0, 0, 0, 0, 0, 0, 0, 0,           0,           0,           1,           0,           0,           0,
                                        0, 0, 0, 0, 0, 0, 0, 0,           0,           0,           0,           1,           0,           0,
                                        0, 0, 0, 0, 0, 0, 0, 0,           0,           0,           0,           0,           1,           0,
                                        0, 0, 0, 0, 0, 0, 0, 0,           0,           0,           0,           0,           0,           1);

    // init...
    m_linearKalman.statePre.at<track_t>(0) = rect0.center.x;      // x
    m_linearKalman.statePre.at<track_t>(1) = rect0.center.y;      // y
    m_linearKalman.statePre.at<track_t>(2) = rect0.center.z;      // z
    m_linearKalman.statePre.at<track_t>(3) = rect0.size.width;  // width
    m_linearKalman.statePre.at<track_t>(4) = rect0.size.height; // height
    m_linearKalman.statePre.at<track_t>(5) = rect0.size.length; // length
    m_linearKalman.statePre.at<track_t>(6) = rect0.yaw; // yaw
    m_linearKalman.statePre.at<track_t>(7) = rectv0.x;     // dx
    m_linearKalman.statePre.at<track_t>(8) = rectv0.y;     // dy
    m_linearKalman.statePre.at<track_t>(9) = 0;            // dz
    m_linearKalman.statePre.at<track_t>(10) = 0;            // dw
    m_linearKalman.statePre.at<track_t>(11) = 0;            // dh
    m_linearKalman.statePre.at<track_t>(12) = 0;            // dl
    m_linearKalman.statePre.at<track_t>(13) = 0;            // dyaw

    m_linearKalman.statePost.at<track_t>(0) = rect0.center.x;
    m_linearKalman.statePost.at<track_t>(1) = rect0.center.y;
    m_linearKalman.statePost.at<track_t>(2) = rect0.center.z;
    m_linearKalman.statePost.at<track_t>(3) = rect0.size.width;
    m_linearKalman.statePost.at<track_t>(4) = rect0.size.height;
    m_linearKalman.statePost.at<track_t>(5) = rect0.size.length;
    m_linearKalman.statePost.at<track_t>(6) = rect0.yaw;
    m_linearKalman.statePost.at<track_t>(7) = rectv0.x;
    m_linearKalman.statePost.at<track_t>(8) = rectv0.y;
    m_linearKalman.statePost.at<track_t>(9) = 0;
    m_linearKalman.statePost.at<track_t>(10) = 0;
    m_linearKalman.statePost.at<track_t>(11) = 0;
    m_linearKalman.statePost.at<track_t>(12) = 0;
    m_linearKalman.statePost.at<track_t>(13) = 0;

    cv::setIdentity(m_linearKalman.measurementMatrix);

    track_t n1 = pow(m_deltaTime, 4.f) / 4.f;
    track_t n2 = pow(m_deltaTime, 3.f) / 2.f;
    track_t n3 = pow(m_deltaTime, 2.f);
    m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(14, 14) <<
                                       n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,
                                       0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,
                                       0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,
                                       0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,
                                       0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,
                                       0,  0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,
                                       0,  0,  0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2,
                                       n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,  0,  0,
                                       0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,  0,
                                       0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,
                                       0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,
                                       0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,
                                       0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,
                                       0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3);

    m_linearKalman.processNoiseCov *= m_accelNoiseMag;

    cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));

    cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));

    m_initialized = true;
}

//---------------------------------------------------------------------------
void TKalmanFilter::CreateLinearAcceleration(cv::Point3f xy0, cv::Point3f xyv0)
{
	// 6 state variables, 2 measurements
	m_linearKalman.init(6, 2, 0, El_t);
	// Transition cv::Matrix
	const track_t dt = m_deltaTime;
	const track_t dt2 = 0.5f * m_deltaTime * m_deltaTime;
	m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(6, 6) <<
		1, 0, dt, 0,  dt2, 0,
		0, 1, 0,  dt, 0,   dt2,
		0, 0, 1,  0,  dt,  0,
		0, 0, 0,  1,  0,   dt,
	    0, 0, 0,  0,  1,   0,
	    0, 0, 0,  0,  0,   1);

	// init...
	m_lastPointResult = xy0;
	m_linearKalman.statePre.at<track_t>(0) = xy0.x;  // x
	m_linearKalman.statePre.at<track_t>(1) = xy0.y;  // y
	m_linearKalman.statePre.at<track_t>(2) = xyv0.x; // vx
	m_linearKalman.statePre.at<track_t>(3) = xyv0.y; // vy
	m_linearKalman.statePre.at<track_t>(4) = 0;      // ax
	m_linearKalman.statePre.at<track_t>(5) = 0;      // ay

	m_linearKalman.statePost.at<track_t>(0) = xy0.x;
	m_linearKalman.statePost.at<track_t>(1) = xy0.y;
	m_linearKalman.statePost.at<track_t>(2) = xyv0.x;
	m_linearKalman.statePost.at<track_t>(3) = xyv0.y;
	m_linearKalman.statePost.at<track_t>(4) = 0;
	m_linearKalman.statePost.at<track_t>(5) = 0;

	cv::setIdentity(m_linearKalman.measurementMatrix);

	track_t n1 = pow(m_deltaTime, 4.f) / 4.f;
	track_t n2 = pow(m_deltaTime, 3.f) / 2.f;
	track_t n3 = pow(m_deltaTime, 2.f);
	m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(6, 6) <<
		n1, 0, n2, 0, n2, 0,
		0, n1, 0, n2, 0, n2,
		n2, 0, n3, 0, n3, 0,
		0, n2, 0, n3, 0, n3,
		0, 0, n2, 0, n3, 0,
		0, 0, 0, n2, 0, n3);

	m_linearKalman.processNoiseCov *= m_accelNoiseMag;

	cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));

	cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));

	m_initialized = true;
}
//---------------------------------------------------------------------------
void TKalmanFilter::CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t rectv0)
{
    // 12 state variables (x, y, vx, vy, ax, ay, width, height, vw, vh, aw, ah), 4 measurements (x, y, width, height)
    m_linearKalman.init(12, 4, 0, El_t);
    // Transition cv::Matrix
    const track_t dt = m_deltaTime;
    const track_t dt2 = 0.5f * m_deltaTime * m_deltaTime;
    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(12, 12) <<
        1, 0, 0, 0, dt, 0,  0,  0,  dt2, 0,   dt2, 0,
        0, 1, 0, 0, 0,  dt, 0,  0,  0,   dt2, 0,   dt2,
        0, 0, 1, 0, 0,  0,  dt, 0,  0,   0,   dt2, 0,
        0, 0, 0, 1, 0,  0,  0,  dt, 0,   0,   0,   dt2,
        0, 0, 0, 0, 1,  0,  0,  0,  dt,  0,   0,   0,
        0, 0, 0, 0, 0,  1,  0,  0,  0,   dt,  0,   0,
        0, 0, 0, 0, 0,  0,  1,  0,  0,   0,   dt,  0,
        0, 0, 0, 0, 0,  0,  0,  1,  0,   0,   0,   dt,
        0, 0, 0, 0, 0,  0,  0,  0,  1,   0,   0,   0,
        0, 0, 0, 0, 0,  0,  0,  0,  0,   1,   0,   0,
        0, 0, 0, 0, 0,  0,  0,  0,  0,   0,   0,   1);

    // init...
    m_linearKalman.statePre.at<track_t>(0) = rect0.x;      // x
    m_linearKalman.statePre.at<track_t>(1) = rect0.y;      // y
    m_linearKalman.statePre.at<track_t>(2) = rect0.width;  // width
    m_linearKalman.statePre.at<track_t>(3) = rect0.height; // height
    m_linearKalman.statePre.at<track_t>(4) = rectv0.x;     // dx
    m_linearKalman.statePre.at<track_t>(5) = rectv0.y;     // dy
    m_linearKalman.statePre.at<track_t>(6) = 0;            // dw
    m_linearKalman.statePre.at<track_t>(7) = 0;            // dh
    m_linearKalman.statePre.at<track_t>(8) = 0;            // ax
    m_linearKalman.statePre.at<track_t>(9) = 0;            // ay
    m_linearKalman.statePre.at<track_t>(10) = 0;           // aw
    m_linearKalman.statePre.at<track_t>(11) = 0;           // ah

    m_linearKalman.statePost.at<track_t>(0) = rect0.x;
    m_linearKalman.statePost.at<track_t>(1) = rect0.y;
    m_linearKalman.statePost.at<track_t>(2) = rect0.width;
    m_linearKalman.statePost.at<track_t>(3) = rect0.height;
    m_linearKalman.statePost.at<track_t>(4) = rectv0.x;
    m_linearKalman.statePost.at<track_t>(5) = rectv0.y;
    m_linearKalman.statePost.at<track_t>(6) = 0;
    m_linearKalman.statePost.at<track_t>(7) = 0;
    m_linearKalman.statePost.at<track_t>(8) = 0;
    m_linearKalman.statePost.at<track_t>(9) = 0;
    m_linearKalman.statePost.at<track_t>(10) = 0;
    m_linearKalman.statePost.at<track_t>(11) = 0;

    cv::setIdentity(m_linearKalman.measurementMatrix);

    track_t n1 = pow(m_deltaTime, 4.f) / 4.f;
    track_t n2 = pow(m_deltaTime, 3.f) / 2.f;
    track_t n3 = pow(m_deltaTime, 2.f);
    m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(12, 12) <<
        n1, 0, 0, 0, n2, 0, 0, 0, n2, 0, n2,
        0, n1, 0, 0, 0, n2, 0, 0, 0, n2, 0, n2,
        0, 0, n1, 0, 0, 0, n2, 0, 0, 0, n2, 0,
        0, 0, 0, n1, 0, 0, 0, n2, 0, 0, 0, n2,
        n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, n3, 0,
        0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, n3,
        0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0,
        0, 0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3,
        n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, 0, 0,
        0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, 0,
        0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0,
        0, 0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3);

    m_linearKalman.processNoiseCov *= m_accelNoiseMag;

    cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));

    cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));

    m_initialized = true;
}
//---------------------------------------------------------------------------
void TKalmanFilter::CreateLinearAcceleration3D(Rect3D rect0, cv::Point3f rectv0)
{
	// 12 state variables (x, y, vx, vy, ax, ay, width, height, vw, vh, aw, ah), 4 measurements (x, y, width, height)
	m_linearKalman.init(12, 4, 0, El_t);
	// Transition cv::Matrix
	const track_t dt = m_deltaTime;
	const track_t dt2 = 0.5f * m_deltaTime * m_deltaTime;
	m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(12, 12) <<
		1, 0, 0, 0, dt, 0,  0,  0,  dt2, 0,   dt2, 0,
		0, 1, 0, 0, 0,  dt, 0,  0,  0,   dt2, 0,   dt2,
		0, 0, 1, 0, 0,  0,  dt, 0,  0,   0,   dt2, 0,
		0, 0, 0, 1, 0,  0,  0,  dt, 0,   0,   0,   dt2,
		0, 0, 0, 0, 1,  0,  0,  0,  dt,  0,   0,   0,
		0, 0, 0, 0, 0,  1,  0,  0,  0,   dt,  0,   0,
		0, 0, 0, 0, 0,  0,  1,  0,  0,   0,   dt,  0,
		0, 0, 0, 0, 0,  0,  0,  1,  0,   0,   0,   dt,
		0, 0, 0, 0, 0,  0,  0,  0,  1,   0,   0,   0,
		0, 0, 0, 0, 0,  0,  0,  0,  0,   1,   0,   0,
		0, 0, 0, 0, 0,  0,  0,  0,  0,   0,   0,   1);

	// init...
    m_linearKalman.statePre.at<track_t>(0) = rect0.center.x;      // x
    m_linearKalman.statePre.at<track_t>(1) = rect0.center.y;      // y
    m_linearKalman.statePre.at<track_t>(2) = rect0.size.width;  // width
    m_linearKalman.statePre.at<track_t>(3) = rect0.size.height; // height
	m_linearKalman.statePre.at<track_t>(4) = rectv0.x;     // dx
	m_linearKalman.statePre.at<track_t>(5) = rectv0.y;     // dy
	m_linearKalman.statePre.at<track_t>(6) = 0;            // dw
	m_linearKalman.statePre.at<track_t>(7) = 0;            // dh
	m_linearKalman.statePre.at<track_t>(8) = 0;            // ax
	m_linearKalman.statePre.at<track_t>(9) = 0;            // ay
	m_linearKalman.statePre.at<track_t>(10) = 0;           // aw
	m_linearKalman.statePre.at<track_t>(11) = 0;           // ah

    m_linearKalman.statePost.at<track_t>(0) = rect0.center.x;
    m_linearKalman.statePost.at<track_t>(1) = rect0.center.y;
    m_linearKalman.statePost.at<track_t>(2) = rect0.size.width;
    m_linearKalman.statePost.at<track_t>(3) = rect0.size.height;
	m_linearKalman.statePost.at<track_t>(4) = rectv0.x;
	m_linearKalman.statePost.at<track_t>(5) = rectv0.y;
	m_linearKalman.statePost.at<track_t>(6) = 0;
	m_linearKalman.statePost.at<track_t>(7) = 0;
	m_linearKalman.statePost.at<track_t>(8) = 0;
	m_linearKalman.statePost.at<track_t>(9) = 0;
	m_linearKalman.statePost.at<track_t>(10) = 0;
	m_linearKalman.statePost.at<track_t>(11) = 0;

	cv::setIdentity(m_linearKalman.measurementMatrix);

	track_t n1 = pow(m_deltaTime, 4.f) / 4.f;
	track_t n2 = pow(m_deltaTime, 3.f) / 2.f;
	track_t n3 = pow(m_deltaTime, 2.f);
	m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(12, 12) <<
		n1, 0, 0, 0, n2, 0, 0, 0, n2, 0, n2,
		0, n1, 0, 0, 0, n2, 0, 0, 0, n2, 0, n2,
		0, 0, n1, 0, 0, 0, n2, 0, 0, 0, n2, 0,
		0, 0, 0, n1, 0, 0, 0, n2, 0, 0, 0, n2,
		n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, n3, 0,
		0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, n3,
		0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0,
		0, 0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3,
		n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, 0, 0,
		0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, 0,
		0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0,
		0, 0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3);

	m_linearKalman.processNoiseCov *= m_accelNoiseMag;

	cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));

	cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));

	m_initialized = true;
}

//---------------------------------------------------------------------------
cv::Point3f TKalmanFilter::GetPointPrediction()
{
    if (m_initialized)
    {
        cv::Mat prediction;

        switch (m_type)
        {
        case tracking::KalmanLinear:
            prediction = m_linearKalman.predict();
            break;
        }

        m_lastPointResult = cv::Point3f(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2));
    }
    else
    {

    }
    return m_lastPointResult;
}

//---------------------------------------------------------------------------
cv::Point3f TKalmanFilter::Update(cv::Point3f pt, bool dataCorrect)
{
    if (!m_initialized)
    {
        if (m_initialPoints.size() < MIN_INIT_VALS)
        {
            if (dataCorrect)
            {
                m_initialPoints.push_back(pt);
                m_lastPointResult = pt;
            }
        }
        if (m_initialPoints.size() == MIN_INIT_VALS)
        {
            track_t kx = 0;
            track_t bx = 0;
            track_t ky = 0;
            track_t by = 0;
            get_lin_regress_params(m_initialPoints, 0, MIN_INIT_VALS, kx, bx, ky, by);//predict p,v
            cv::Point3f xy0(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, m_lastPointResult.z);
            cv::Point3f xyv0(kx, ky,0);

            switch (m_type)
            {
            case tracking::KalmanLinear:
                if (m_useAcceleration)
					CreateLinearAcceleration(xy0, xyv0);
				else
					CreateLinear(xy0, xyv0);
                break;
            }
            m_lastDist = 0;
        }
    }

    if (m_initialized)
    {
        cv::Mat measurement(2, 1, Mat_t(1));
        if (!dataCorrect)
        {
            measurement.at<track_t>(0) = m_lastPointResult.x;  //update using prediction
            measurement.at<track_t>(1) = m_lastPointResult.y;
        }
        else
        {
            measurement.at<track_t>(0) = pt.x;  //update using measurements
            measurement.at<track_t>(1) = pt.y;
        }
        // Correction
        cv::Mat estimated;
        switch (m_type)
        {
        case tracking::KalmanLinear:
        {
            estimated = m_linearKalman.correct(measurement);

            // Inertia correction
			if (!m_useAcceleration)
			{
				track_t currDist = sqrtf(sqr(estimated.at<track_t>(0) - pt.x) + sqr(estimated.at<track_t>(1) - pt.y));
				if (currDist > m_lastDist)
				{
					m_deltaTime = std::min(m_deltaTime + m_deltaStep, m_deltaTimeMax);
				}
				else
				{
					m_deltaTime = std::max(m_deltaTime - m_deltaStep, m_deltaTimeMin);
				}
				m_lastDist = currDist;

				m_linearKalman.transitionMatrix.at<track_t>(0, 2) = m_deltaTime;
				m_linearKalman.transitionMatrix.at<track_t>(1, 3) = m_deltaTime;
			}
            break;
        }
        }

        m_lastPointResult.x = estimated.at<track_t>(0);   //update using measurements
        m_lastPointResult.y = estimated.at<track_t>(1);
    }
    else
    {
        if (dataCorrect)
        {
            m_lastPointResult = pt;
        }
    }
    return m_lastPointResult;
}
//---------------------------------------------------------------------------
cv::Rect TKalmanFilter::GetRectPrediction()
{
    if (m_initialized)
    {
        cv::Mat prediction;

        switch (m_type)
        {
        case tracking::KalmanLinear:
            prediction = m_linearKalman.predict();
            break;
        }

        m_lastRectResult = cv::Rect_<track_t>(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2), prediction.at<track_t>(3));
    }
    else
    {

    }
    return cv::Rect(static_cast<int>(m_lastRectResult.x), static_cast<int>(m_lastRectResult.y), static_cast<int>(m_lastRectResult.width), static_cast<int>(m_lastRectResult.height));
}

//---------------------------------------------------------------------------
cv::Rect TKalmanFilter::Update(cv::Rect rect, bool dataCorrect)
{
    if (!m_initialized)
    {
        if (m_initialRects.size() < MIN_INIT_VALS)
        {
            if (dataCorrect)
            {
                m_initialRects.push_back(rect);
                m_lastRectResult.x = static_cast<track_t>(rect.x);
                m_lastRectResult.y = static_cast<track_t>(rect.y);
                m_lastRectResult.width = static_cast<track_t>(rect.width);
                m_lastRectResult.height = static_cast<track_t>(rect.height);
            }
        }
        if (m_initialRects.size() == MIN_INIT_VALS)
        {
            std::vector<Point_t> initialPoints;
            Point_t averageSize(0, 0);
            for (const auto& r : m_initialRects)
            {
                initialPoints.emplace_back(static_cast<track_t>(r.x), static_cast<track_t>(r.y));
                averageSize.x += r.width;
                averageSize.y += r.height;
            }
            averageSize.x /= MIN_INIT_VALS;
            averageSize.y /= MIN_INIT_VALS;

            track_t kx = 0;
            track_t bx = 0;
            track_t ky = 0;
            track_t by = 0;
            get_lin_regress_params(initialPoints, 0, MIN_INIT_VALS, kx, bx, ky, by);
            cv::Rect_<track_t> rect0(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, averageSize.x, averageSize.y);
            Point_t rectv0(kx, ky);

            switch (m_type)
            {
            case tracking::KalmanLinear:
                if (m_useAcceleration)
                    CreateLinearAcceleration(rect0, rectv0);
                else
                    CreateLinear(rect0, rectv0);
                break;
            }
        }
    }

    if (m_initialized)
    {
        cv::Mat measurement(4, 1, Mat_t(1));
        if (!dataCorrect)
        {
            measurement.at<track_t>(0) = m_lastRectResult.x;  // update using prediction
            measurement.at<track_t>(1) = m_lastRectResult.y;
            measurement.at<track_t>(2) = m_lastRectResult.width;
            measurement.at<track_t>(3) = m_lastRectResult.height;
        }
        else
        {
            measurement.at<track_t>(0) = static_cast<track_t>(rect.x);  // update using measurements
            measurement.at<track_t>(1) = static_cast<track_t>(rect.y);
            measurement.at<track_t>(2) = static_cast<track_t>(rect.width);
            measurement.at<track_t>(3) = static_cast<track_t>(rect.height);
        }
        // Correction
        cv::Mat estimated;
        switch (m_type)
        {
        case tracking::KalmanLinear:
        {
            estimated = m_linearKalman.correct(measurement);

            m_lastRectResult.x = estimated.at<track_t>(0);   //update using measurements
            m_lastRectResult.y = estimated.at<track_t>(1);
            m_lastRectResult.width = estimated.at<track_t>(2);
            m_lastRectResult.height = estimated.at<track_t>(3);

            // Inertia correction
            if (!m_useAcceleration)
            {
                track_t currDist = sqrtf(sqr(estimated.at<track_t>(0) - rect.x) + sqr(estimated.at<track_t>(1) - rect.y) + sqr(estimated.at<track_t>(2) - rect.width) + sqr(estimated.at<track_t>(3) - rect.height));
                if (currDist > m_lastDist)
                {
                    m_deltaTime = std::min(m_deltaTime + m_deltaStep, m_deltaTimeMax);
                }
                else
                {
                    m_deltaTime = std::max(m_deltaTime - m_deltaStep, m_deltaTimeMin);
                }
                m_lastDist = currDist;

                m_linearKalman.transitionMatrix.at<track_t>(0, 4) = m_deltaTime;
                m_linearKalman.transitionMatrix.at<track_t>(1, 5) = m_deltaTime;
                m_linearKalman.transitionMatrix.at<track_t>(2, 6) = m_deltaTime;
                m_linearKalman.transitionMatrix.at<track_t>(3, 7) = m_deltaTime;
            }
            break;
        }
        }
    }
    else
    {
        if (dataCorrect)
        {
            m_lastRectResult.x = static_cast<track_t>(rect.x);
            m_lastRectResult.y = static_cast<track_t>(rect.y);
            m_lastRectResult.width = static_cast<track_t>(rect.width);
            m_lastRectResult.height = static_cast<track_t>(rect.height);
        }
    }
    return cv::Rect(static_cast<int>(m_lastRectResult.x), static_cast<int>(m_lastRectResult.y), static_cast<int>(m_lastRectResult.width), static_cast<int>(m_lastRectResult.height));
}
//---------------------------------------------------------------------------
Rect3D TKalmanFilter::GetRect3DPrediction()
{
    if (m_initialized)
    {
        cv::Mat prediction;

        switch (m_type)
        {
        case tracking::KalmanLinear:
            prediction = m_linearKalman.predict();
            break;
        }

        m_lastRect3DResult = Rect3D(cv::Point3f(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2)), Size3D(prediction.at<track_t>(3), prediction.at<track_t>(4), prediction.at<track_t>(5)), prediction.at<track_t>(6));
    }
    else
    {

    }
    return m_lastRect3DResult;
}

//---------------------------------------------------------------------------
Rect3D TKalmanFilter::Update(Rect3D rect, bool dataCorrect)
{
    if (!m_initialized)
    {
        if (m_initialRect3Ds.size() < MIN_INIT_VALS)
        {
            if (dataCorrect)
            {
                m_initialRect3Ds.push_back(rect);
                m_lastRect3DResult.center.x = static_cast<track_t>(rect.center.x);
                m_lastRect3DResult.center.y = static_cast<track_t>(rect.center.y);
                m_lastRect3DResult.center.z = static_cast<track_t>(rect.center.z);
                m_lastRect3DResult.size.width = static_cast<track_t>(rect.size.width);
                m_lastRect3DResult.size.height = static_cast<track_t>(rect.size.height);
                m_lastRect3DResult.size.length = static_cast<track_t>(rect.size.length);
                m_lastRect3DResult.yaw = static_cast<track_t>(rect.yaw);
            }
        }
        if (m_initialRect3Ds.size() == MIN_INIT_VALS)
        {
            std::vector<Point_t> initialPoints;
            cv::Point3f averageSize(0, 0, 0);
            float averageZ = 0;
            float averageYaw = 0;
            for (const auto& r : m_initialRect3Ds)
            {
                initialPoints.emplace_back(static_cast<track_t>(r.center.x), static_cast<track_t>(r.center.y));
                averageZ += r.center.z;
                averageSize.x += r.size.width;
                averageSize.y += r.size.height;
                averageSize.z += r.size.length;
                averageYaw += r.yaw;
            }
            averageZ /= MIN_INIT_VALS;
            averageSize.x /= MIN_INIT_VALS;
            averageSize.y /= MIN_INIT_VALS;
            averageSize.z /= MIN_INIT_VALS;
            averageYaw /= MIN_INIT_VALS;

            track_t kx = 0;
            track_t bx = 0;
            track_t ky = 0;
            track_t by = 0;
            get_lin_regress_params(initialPoints, 0, MIN_INIT_VALS, kx, bx, ky, by);
            Rect3D rect0(cv::Point3f(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, averageZ), Size3D(averageSize.x, averageSize.y,averageSize.z), averageYaw);
            cv::Point3f rectv0(kx, ky, 0);

            switch (m_type)
            {
            case tracking::KalmanLinear:
				if (m_useAcceleration)
                    CreateLinearAcceleration3D(rect0, rectv0);
				else
                    CreateLinear3D(rect0, rectv0);
                break;
            }
        }
    }

    if (m_initialized)
    {
        cv::Mat measurement(7, 1, Mat_t(1));
        if (!dataCorrect)
        {
            measurement.at<track_t>(0) = m_lastRect3DResult.center.x;  // update using prediction
            measurement.at<track_t>(1) = m_lastRect3DResult.center.y;
            measurement.at<track_t>(2) = m_lastRect3DResult.center.z;
            measurement.at<track_t>(3) = m_lastRect3DResult.size.width;
            measurement.at<track_t>(4) = m_lastRect3DResult.size.height;
            measurement.at<track_t>(5) = m_lastRect3DResult.size.length;
            measurement.at<track_t>(6) = m_lastRect3DResult.yaw;
        }
        else
        {
            measurement.at<track_t>(0) = static_cast<track_t>(rect.center.x);  // update using measurements
            measurement.at<track_t>(1) = static_cast<track_t>(rect.center.y);
            measurement.at<track_t>(2) = static_cast<track_t>(rect.center.z);
            measurement.at<track_t>(3) = static_cast<track_t>(rect.size.width);
            measurement.at<track_t>(4) = static_cast<track_t>(rect.size.height);
            measurement.at<track_t>(5) = static_cast<track_t>(rect.size.length);
            measurement.at<track_t>(6) = static_cast<track_t>(rect.yaw);
        }
        // Correction
        cv::Mat estimated;
        switch (m_type)
        {
        case tracking::KalmanLinear:
        {
            estimated = m_linearKalman.correct(measurement);

            m_lastRect3DResult.center.x = estimated.at<track_t>(0);   //update using measurements
            m_lastRect3DResult.center.y = estimated.at<track_t>(1);
            m_lastRect3DResult.center.z = estimated.at<track_t>(2);
            m_lastRect3DResult.size.width = estimated.at<track_t>(3);
            m_lastRect3DResult.size.height = estimated.at<track_t>(4);
            m_lastRect3DResult.size.length = estimated.at<track_t>(5);
            m_lastRect3DResult.yaw = estimated.at<track_t>(6);

            // Inertia correction
			if (!m_useAcceleration)
			{
                track_t currDist = sqrtf(sqr(estimated.at<track_t>(0) - rect.center.x) + sqr(estimated.at<track_t>(1) - rect.center.y)+ sqr(estimated.at<track_t>(2) - rect.center.z) + sqr(estimated.at<track_t>(3) - rect.size.width) + sqr(estimated.at<track_t>(4) - rect.size.height) + sqr(estimated.at<track_t>(5) - rect.size.length) + sqr(estimated.at<track_t>(6) - rect.yaw));
				if (currDist > m_lastDist)
				{
					m_deltaTime = std::min(m_deltaTime + m_deltaStep, m_deltaTimeMax);
				}
				else
				{
					m_deltaTime = std::max(m_deltaTime - m_deltaStep, m_deltaTimeMin);
				}
				m_lastDist = currDist;

                m_linearKalman.transitionMatrix.at<track_t>(0, 7) = m_deltaTime;
                m_linearKalman.transitionMatrix.at<track_t>(1, 8) = m_deltaTime;
                m_linearKalman.transitionMatrix.at<track_t>(2, 9) = m_deltaTime;
                m_linearKalman.transitionMatrix.at<track_t>(3, 10) = m_deltaTime;
                m_linearKalman.transitionMatrix.at<track_t>(4, 11) = m_deltaTime;
                m_linearKalman.transitionMatrix.at<track_t>(5, 12) = m_deltaTime;
                m_linearKalman.transitionMatrix.at<track_t>(6, 13) = m_deltaTime;
			}
            break;
        }
        }
    }
    else
    {
        if (dataCorrect)
        {
            m_lastRect3DResult.center.x = static_cast<track_t>(rect.center.x);
            m_lastRect3DResult.center.y = static_cast<track_t>(rect.center.y);
            m_lastRect3DResult.center.z = static_cast<track_t>(rect.center.z);
            m_lastRect3DResult.size.width = static_cast<track_t>(rect.size.width);
            m_lastRect3DResult.size.height = static_cast<track_t>(rect.size.height);
            m_lastRect3DResult.size.length = static_cast<track_t>(rect.size.length);
            m_lastRect3DResult.yaw = static_cast<track_t>(rect.yaw);
        }
    }
    return m_lastRect3DResult;
}

//---------------------------------------------------------------------------
cv::Vec<track_t, 2> TKalmanFilter::GetVelocity() const
{
    cv::Vec<track_t, 2> res(0, 0);
    if (m_initialized)
    {
        switch (m_type)
        {
        case tracking::KalmanLinear:
        {
            if (m_linearKalman.statePre.rows > 3)
            {
                int indX = 2;
                int indY = 3;
                if (m_linearKalman.statePre.rows > 5)
                {
                    indX = 4;
                    indY = 5;
                    if (m_linearKalman.statePre.rows > 8)
                    {
                        indX = 7;
                        indY = 8;
                    }
                    res[0] = m_linearKalman.statePre.at<track_t>(indX);
                    res[1] = m_linearKalman.statePre.at<track_t>(indY);
                }
            }
            break;
        }

        }
    }
    return res;
}