/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name          : LapProcess.h
* Department         : Sensor Algorithm Team
* Current Version    : V1.0
* Author             : logzhan
* Date of Issued     : 2020.8.4
* Comments           : ÅÜȦģʽÓÅ»¯
********************************************************************************/
#ifndef _LAPPROCESS_H_
#define	_LAPPROCESS_H_
#ifdef __cplusplus
extern "C" {
#endif
/* Header File Including ------------------------------------------------------------------------ */
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "Utils.h"
#include "pdr_sensor.h"
#include "PDRBase.h"

/* Macro Definition ----------------------------------------------------------------------------- */

#define FIND_CIRCLE 1
#define NO_CIRCLE 0
#define ADJ_CIRCLE 1
#define NADJ_CIRCLE 0
#define DIST_THRESHOLD 25

/* Function Declaration ------------------------------------------------------------------------- */
void lapInit();

void lapProcess(GNSS_t pgnss, LctFs_t *result);

void pdr_cacheTrajs(GNSS_t pgnss, LctFs_t*result);

int pdr_adjustTraj(double trajs[][2], int count);

double pdr_getPointYaw(double point1[], double point2[]);

void pdr_pathMap(TrajSign* trajs, double point[2], double yaw, double projPoint[2]);

double pdr_optiFromTrajs(GNSS_t pgnss, LctFs_t*lct_fusion);

int pdr_findTun(TrajSign* refTrajs, double point[], double heading);

int pdr_fuseRefTraj(TrajSign* tunnelTrajs, TrajSign* refTrajs, double newRefLoc[][2]);

unsigned char  pdr_point2line(double point[], double lineStart[], double lineEnd[], double projLoc[]);

void ProjPointOfLoop(double point1[], double yaw, double point2[], double result[]);

double lap_calDistance(double lla1[2], double lla2[2]);
#ifdef __cplusplus
}
#endif
#endif // !_LAPPROCESS_H_