#pragma once
//#include "BayesFilter/unsFlt.hpp"
////#include <iostream>
//#include <boost/numeric/ublas/io.hpp>
/********************************************************************
	created:	2008/01/03
	created:	3:1:2008   15:00
	filename: 	c:\boosttest\MyTargetTrack\MyTargetTrack\UncentedKalmanFilt\PredictModel.h
	file path:	c:\boosttest\MyTargetTrack\MyTargetTrack\UncentedKalmanFilt
	file base:	PredictModel
	file ext:	h
	author:		GaoYang
	
	purpose:	預(yù)測模型，不含噪聲的理想模型包含了一個預(yù)測函數(shù)fx，輸入狀態(tài)給出預(yù)測狀態(tài)及方差，存于自身的
				成員變量中,其中狀態(tài)變量中的線速度定義為：距離變遠為正。
*********************************************************************/
//#include <boost/numeric/ublas/matrix.hpp>
//#include <boost/numeric/ublas/vector.hpp>
//#include <boost/numeric/ublas/storage.hpp>
//#include <boost/numeric/ublas/matrix_proxy.hpp>


//#include "BayesFilter/bayesFlt.hpp"
#include "..\ObjectAngleCheck.h"
#include "RobotStruct.h"
#include "VelocityStructFile.h"
#include "UKF_COMMAN_HEAD.h"
#include "..\ShareObject.h"
#define HALF_PI 1.57079632679490

//using namespace std;
//using namespace Bayesian_filter;
//using namespace Bayesian_filter_matrix;
//using namespace boost::numeric::ublas;

using namespace personal_ShareObject_pathPlanUsingSpace;

class CPredictModel :public ObjectAngleCheck//,Bayesian_filter::Functional_predict_model
{
public:
	CPredictModel(void);
public:
	virtual ~CPredictModel(void);
	void VelocityPredict(double dl, double dr);///<預(yù)測函數(shù)
	/**
	*@brief 進行速度預(yù)測的函數(shù)fx_Degree調(diào)用。本函數(shù)中開始，考量時間，另外嚴格用物體的CV模型
	*
	*所有速度都考慮時間的影響，所有速度存儲位置在m_vStateAfPredit中，單位為mm/s,或者 度/s
	*@input dl 左輪位移
	*@input dr 右輪位移
	*@input TimeStep 上一次計算到此次計算的間隔時間步長，
	*/
	void VelocityPredict2WithTimeConsider(double dl, double dr,double TimeStep);
	/**
	*直角坐標系中的XY速度分量向極坐標系中速度分量的轉(zhuǎn)化,速度單位為每秒
	*@parm CurrentPredictX 為本輪預(yù)測中估計的當前位置X坐標
	*@parm CurrentPredictY 為本輪預(yù)測中估計的當前位置Y坐標
	*@parm DeltaX 為本輪預(yù)測中估計的當前位置與前一位置的X坐標差
	*/
	void OrthogonalCoordVel2PolarCoordVel(double& CurrentPredictX,double& CurrentPredictY,
		double DeltX,double DeltY,double& LinearV,double& AngularV,double TimeStep,
		ObjectPoleCoord& CurrenPredictPolePos)
	{
		double NextPredictX=CurrentPredictX+DeltX;
		double NextPredictY=CurrentPredictY+DeltY;
		LinearV=sqrt(pow(NextPredictX,2)+pow(NextPredictY,2));//暫存下一預(yù)測點距離
		AngularV=atan2(NextPredictY,NextPredictX);//暫存下一預(yù)測點角度，-Pi~Pi
		LimitAngleIn0_2Pi(AngularV);//暫存下一預(yù)測點角度，0~2Pi
		LinearV=(LinearV-CurrenPredictPolePos.distance)/TimeStep*1000;
		AngularV=GetAngulardisplacementIn2Angle(CurrenPredictPolePos.angle,AngularV)/TimeStep*1000;
	}
	double m_ddeltaLeftAndRight[2];///<機器人左右輪的位移0左，1右，單位毫米
	double m_ddeltaOrientation;///<相對上一次的朝向，本時刻朝向的角度變化，單位弧度，相對上一次，順時針變?yōu)樨?	int m_iRightNeg_LeftPosit;///<標志位，用于標示相對上一位置，機器人是向左轉(zhuǎn)(值為1)，未轉(zhuǎn)（0），右轉(zhuǎn)（-1）
	STATE_STYLE& fx( STATE_STYLE& x,double dl,double dr)  ;///<接口函數(shù)，輸入x為擴維后的狀態(tài)變量

	STATE_STYLE& fx_Degree( STATE_STYLE& x,double dl,double dr)  ;///<角度型接口函數(shù)，輸入x為擴維后的狀態(tài)變量，其中的角度相關(guān)變量單位為度
	STATE_STYLE& fx_Degree2ConsiderTime( STATE_STYLE& x,double dl,double dr,double TimeStep);///<角度型接口函數(shù)，輸入x為擴維后的狀態(tài)變量，其中的角度相關(guān)變量單位為度,速度單位為每秒


protected:
	/** 
	*計算當前自己朝向相對上一次朝向的變化
	*/
	inline void  GetSelfOriention();
	deltaCoorinatePos m_sdeltaCoorinate;
	ObjectPoleCoord m_sObjectObserveCoord;
	ObjectPoleCoord m_sObjectPoleCoordP1_in_Current;///<預(yù)測物體在當前機器人坐標系中的極坐標
	ObjectPoleCoord m_sObjectPoleCoordP1_in_Pre;///<預(yù)測物體在前一時刻機器人坐標系中的極坐標
	ObjectPoleCoord m_sObjectPoleCoordP0_in_Current;///<上一時刻物體在當前機器人坐標系中的極坐標
	ObjectPoleCoord	m_sObjectPoleCoordP0_in_Pre;///<上一時刻物體在前一時刻機器人坐標系中的極坐標

	ObjectVelocity m_sObject_Velocity_in_Current,m_sObject_Velocity_in_pre;
	STATE_STYLE m_vStateAfPredit;///<經(jīng)過預(yù)測后的狀態(tài)結(jié)果，其中的角度相關(guān)變量分兩種情況，若預(yù)測時用角度制的預(yù)測
	//則其中的角度相關(guān)變量即為度數(shù)單位，若預(yù)測時用弧度制，則其中的角度相關(guān)量即位弧度制。
	/**
	*輸入的狀態(tài)變量x中的角度相關(guān)量單位為弧度，此函數(shù)將狀態(tài)變量保存于內(nèi)部成員變量中，成員變量的角度單位都是弧度
	*/
	bool InputState(const STATE_STYLE& x);
public:
	/**
	*輸入的狀態(tài)變量x中的角度相關(guān)量單位為度，此函數(shù)將狀態(tài)變量保存于內(nèi)部成員變量中，成員變量的角度單位都是弧度
	*/
	bool InputState_Degree(const STATE_STYLE& x);
	
};