#ifndef _LOCATE_ALGORITHM_H
#define _LOCATE_ALGORITHM_H

#include <stdio.h>
#include <stdlib.h>
#include "classdef.h"
#include "tdoa/TDOAFunctor.h"

#define M_PI       3.14159265358979323846 // 角度转弧度 角度 * pi / 180
#define M_PI_2     1.57079632679489661923
#define M_PI_4     0.785398163397448309616

// 最终定位坐标计算公式
void AOAformula(struct _coordinate* ant, double angle, struct _coordinate* tar, bool is_line = false, double z_offset = 0);
// 两个坐标之间的距离计算公式
double Distanceformula(struct _coordinate* ant, struct _coordinate* last);
// 三点定位
void locatebycordinate(struct _coordinate** coor_list, struct _coordinate* dest);
// 定位算法
void algorithm_locate(struct _coordinate** coor_list, int size, struct _coordinate* hist, struct _coordinate* dest, double dist_limit, double z_offset);

void algorithm_locate_ex(_coordinate** coor_list, int coor_size, _coordinate* hist, _coordinate* dest, double dist_limit);

double getAngleByCoordinate(_coordinate* p1, _coordinate* p2);
double getAngleByAngle(_coordinate* p1, _coordinate* p2);
double getDistance(_coordinate* p1, _coordinate *p2);
_coordinate* getCross(_coordinate* p1, _coordinate* p2);
void calcCoordinate(_coordinate* p1, double dist, double angle, _coordinate* dest);
bool is_all_inline(_coordinate** coor_list, int cnt );
void HeapSort(_coordinate** pCoordinateArray,int nLen);
void BuildMaxHeap(_coordinate** pCoordinateArray,int nLen);
void AdjustMaxHeap(_coordinate** pCoordinateArray,int n,int nHeapSize);
void SwapElement(_coordinate* a,_coordinate*b);
void SelectSort(_coordinate** pCoordinateArray,int nLen);

/*
 * 定位算法
 *
*/
class LocateAlgorithm{
public:
	//TOF算法函数
	static POS* Pos(ReaderPathMap rpm,int sta_num,int ant,double dist,INFO_PRE info_pre);
	static SOLUTION* GetPos(ReaderPathMap rpm,int sta_num,int ant,double dist,int i);
	static std::shared_ptr<POS> tof_locate_1d(std::shared_ptr<TOFReaderPathMap> trpm,int reader_id,int antenna_idx,double dist,TOF_REFER_DATA refer_data);
	static std::shared_ptr<SOLUTION> GetPos(std::shared_ptr<TOFReaderPathMap> trpm,int reader_id,int antenna_idx,double dist,int seg_idx);
	static std::shared_ptr<POS> tof_locate_2d(std::shared_ptr<ReceiveDataTofVector>& pRdtv);
	static std::vector<op::point> calc_pos(const op::point& circle_center, const double& radius, const op::point* pLine);
	//TDOA算法函数
	static std::shared_ptr<POS> Pos(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm);
	static std::unique_ptr<POS> LocatePos(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm);
	static std::shared_ptr<POS> Pos(std::shared_ptr<POS> pos,std::shared_ptr<TDOAReaderPathMap> trpm);
	static std::shared_ptr<POS> MappingToPath(std::shared_ptr<POS> pos, std::shared_ptr<TDOAReaderPathMap> trpm);
	static std::shared_ptr<POS> TdoaLocate2d(std::shared_ptr<ReceiveDataMap> pRdm);
	static std::shared_ptr<POS> TdoaLocate3d(std::shared_ptr<ReceiveDataMap> pRdm);
	static std::shared_ptr<POS> tdoa_locate_2d_by_fang(std::shared_ptr<ReceiveDataMap> pRdm);
	static std::vector<std::shared_ptr<POS>> tdoa_locate_2d_by_fang(std::shared_ptr<ReceiveDataUnorderedMap> pRdm);
	static double cal_real_distance_diff(const double& md,const double& d,const double& h,const int& s);

	static std::unique_ptr<POS> CalcCardPosition(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm);
	static std::shared_ptr<SOLUTION> GetPos(std::shared_ptr<ReaderPath> pRP,double dist,int i);
	static bool CheckPosInValid(POS* pos,ReceiveDataMap* pRdm,double dScale);
	static bool IsOnMap(std::shared_ptr<POS>& pos,std::shared_ptr<TDOAReaderPathMap> trpm);

	static bool IsInTriangle(std::vector<_point> vtp,_point p);
	static double GetTriangleArea(_point p0,_point p1,_point p2);
	static bool IsInLine(_point p,_point start_p,_point end_p);
	static bool PointIsInRect(_point p,_point tp,_point lp,_point bp,_point rp);
	static int CalcTdoaPosition(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm,std::vector<std::shared_ptr<POS>>& udm_pos);
	static int round(double value);
};

#endif