#include "stdafx.h"
#include "taylor_wls_tdoa.h"

using namespace HostServer;
using namespace Eigen;

/*
 * 求广义逆矩阵
 *
 * param
 *		m		待求矩阵
 *
 * return
 *	m的广义逆矩阵
 *
*/
Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>& Tdoa::TaylorWls::pinv(Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>& m)
{
	auto svd = m.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
	const auto & singular_values = svd.singularValues();
	Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign> singular_values_inv(m.cols(),m.rows());
	singular_values_inv.setZero();
	double pinvtoler = 1E-6;
	for (unsigned int i = 0;i < singular_values.size();++i)
	{
		if (singular_values(i) > pinvtoler)
		{
			singular_values_inv(i,i) = 1.0f/singular_values(i);
		}else{
			singular_values_inv(i,i) = 0.0f;
		}
	}

	Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign> pinvmat = svd.matrixV()*singular_values_inv*svd.matrixU().transpose();

	return std::move(pinvmat);
}

/**/
double Tdoa::TaylorWls::gauss_rand(double mu, double sigma)
{
	/*const double epsilon = std::numeric_limits<double>::min();
	const double two_pi = 2.0*3.14159265358979323846;

	static double z0, z1;
	static bool generate;
	generate = !generate;

	if (!generate)
	return z1 * sigma + mu;

	double u1, u2;
	do
	{
	u1 = rand() * (1.0 / RAND_MAX);
	u2 = rand() * (1.0 / RAND_MAX);
	} while (u1 <= epsilon);

	z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);
	z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);
	return z0 * sigma + mu;*/
	return 0;
}

/*
 * 生成服从正太分布的随机数
 *
 * param
 *		无
 *
 * return
 *		返回生成的随机数
 *
*/
double Tdoa::TaylorWls::gauss_rand()
{
	static double V1, V2, S;
	static int phase = 0;
	double X;

	if (phase == 0) {
		do {
			double U1 = (double)rand() / RAND_MAX;
			double U2 = (double)rand() / RAND_MAX;

			V1 = 2 * U1 - 1;
			V2 = 2 * U2 - 1;
			S = V1 * V1 + V2 * V2;
		} while (S >= 1 || S == 0);

		X = V1 * sqrt(-2 * log(S) / S);
	}
	else
		X = V2 * sqrt(-2 * log(S) / S);

	phase = 1 - phase;

	return X;
}

/*
 * 计算坐标
 *
 * param
 *		pRdm	分站数据
 *
 * return
 *		定位坐标
 *
*/
HostServer::Position& Tdoa::TaylorWls::calc_position(std::shared_ptr<algorithm::base::ReceiveDataUnorderedMap>& pRdm)
{
	Position pos;
	int n = 0;
	n = pRdm->size();

	if (n == 0 && !is_initial)
	{
		pos.x = pos.y = pos.z = -1000.0;
		return std::move(pos);
	}

	init_params(pRdm);

	pos = taylor_wls(anchors,T,iter_pos,n);

	return std::move(pos);
}

/*
 * 泰勒+加权最小二乘算法
 *
 * param
 *		anchors		分站坐标
 *		T			分站的时间同步戳值
 *		pos			迭代的初始坐标值
 *		size		分站数据条数
 *
 * return
 *		定位坐标
 *
*/
HostServer::Position& Tdoa::TaylorWls::taylor_wls(Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>& anchors,Matrix<unsigned long long,Eigen::Dynamic,1,Eigen::DontAlign>& T,const Position& pos,unsigned int size)
{
	unsigned int iter_times = 0;
	double delta = 99999.9;

	//构造协方差矩阵
	Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign> Q;
	Q = Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>::Random(size-1,size-1);
	Q.setIdentity();
	Q *=1000.0;

	Position calc_pos;
	calc_pos = pos;

	while(delta > accuracy){
		Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign> d;
		d = Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>::Random(size,1);
		d.setZero();
		for (unsigned int i = 0;i<size;i++)
		{
			d(i,0) = sqrt(pow(calc_pos.x - anchors(i,0),2) + pow(calc_pos.y - anchors(i,1),2)) + gauss_rand();// + random()
			double ddd = 0.0;
			ddd = d(i,0);
			if (d(i,0) < 0.01)
			{
				d(i,0) = 0.01;
			}
		}

		Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign> a;
		a = Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>::Random(size-1,2);
		Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign> lambada;
		lambada = Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>::Random(size-1,1);
		for (unsigned int i = 0;i<size-1;i++)
		{	
			a(i,0) = (calc_pos.x - anchors(i+1,0))/d(i,0) - (calc_pos.x - anchors(0,0))/d(0,0);
			a(i,1) = (calc_pos.y - anchors(i+1,1))/d(i,0) - (calc_pos.y - anchors(0,1))/d(0,0);
			lambada(i,0) = CFunctions::getDistance(T(i+1,0) - T(0,0),CFunctions::TDOA);
		}

		Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign> H;
		H = Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>::Random(size-1,1);
		Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign> tmp_d;
		tmp_d = Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>::Random(size-1,1);
		for (unsigned int i = 0;i < size - 1;i++)
		{
			tmp_d(i,0) = d(i+1,0) - d(0,0);
		}
		H = lambada - tmp_d;
		Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign> p;
		p = Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>::Random(2,2);
		p.setZero();

		//p = a.transpose()*pinv(Q)*a;
		p = a.transpose()*Q.inverse()*a;

		if(p.determinant() == 0){
			delta = 99999.9;
			break;
		}

		Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign> x_update;
		x_update = Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>::Random(2,1);
		//x_update = pinv(p)*a.transpose()*Q.inverse()*H;
		x_update = p.inverse()*a.transpose()*Q.inverse()*H;
		delta = sqrt(pow(x_update(0,0),2) + pow(x_update(1,0),2));
		calc_pos.x += x_update(0,0);
		calc_pos.y += x_update(1,0);

		iter_times++;
		if (iter_times > iterator_times)
		{
			delta = 99999.9;
			break;
		}
	}

	return std::move(calc_pos);
}

/*
 * 初始化分站坐标和分站的时间同步戳值
 *
 * param
 *		pRdm	分站数据
 *
 * return
 *		执行成功返回0
 *
*/
int Tdoa::TaylorWls::init_params(std::shared_ptr<algorithm::base::ReceiveDataUnorderedMap> pRdm)
{
	//需要检查anchors是不是每次都是动态的
	int i = 0;
	int n = 0;
	n = pRdm->size();

	anchors = Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>::Random(n,2);
	T = Matrix<unsigned long long,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>::Random(n,1);

	for (algorithm::base::ReceiveDataUnorderedMap::iterator it = pRdm->begin(); it != pRdm->end();++it)
	{
		anchors(i,0) = it->second->x;
		anchors(i,1) = it->second->y;
		T(i,0) = it->second->rec_time_stamp;
		i++;
	}

	return 0;
}