#ifndef _LOCATE_ALGORITHM_H
#define _LOCATE_ALGORITHM_H

#include <stdio.h>
#include <stdlib.h>
#include "classdef.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(STATION *s,int sta_num,int ant,double dist,INFO_PRE info_pre);
	//static RESULT* GetPos(STATION *s,int sta_num,int ant,double dist,int i);
	static POS* Pos(ReaderPathMap rpm,int sta_num,int ant,double dist,INFO_PRE info_pre);
	static RESULT* GetPos(ReaderPathMap rpm,int sta_num,int ant,double dist,int i);
	//TDOA算法函数
	static POS* Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm);
	static POS* Pos(POS* pos,TDOAReaderPathMap trpm);
	static RESULT* GetPos(ReaderPath * pRP,double dist,int i);
	static bool CheckPosInValid(POS* pos,ReceiveDataMap* pRdm,double dScale);
	static bool IsOnMap(POS* pos,TDOAReaderPathMap trpm);
};

#endif