// TrackPoints.cpp : Defines the initialization routines for the DLL.
//
#include "TrackPoints.h"
#include "stdio.h"
#include "stdlib.h"
#include "qmath.h"
#include "memory.h"
#include  "string.h"
using namespace std;
// extern "C"
// {
// 	__declspec (dllexport) int TrackPoints(double nDevi,double nAzim,double rlev,double &XE,double &YN,double &ZD,double &HOFF0,double &HOAZ0,double &GTD0,double &AZIM0);
// 	__declspec (dllexport) int initTrackPoints(double azca,double tvd,double dxe,double dyn,double rlev=0,double GLDDDEP0=30,double hoff=0);
// }
double AZA0=0 ; //主投影方位
double AZCA=0; //磁偏角 AZCA
//double DXS;//初始值
//double DYS;//初始值
double TVDS=0;//初始值
double DZDA=0;//全局变量 
double FLSA=0; //FLSA=0.0为最小曲率法  FLSA=1.0为折线法
double PI=3.1415926f;
double RLEA=0.125; //采样间隔
double FLAG=1;  //全局变量
double XE=0;  //东西位移
double YN=0;  //南北位移
double ZD=0; //垂直深度

float OLDDEVI=0;
float OLDAZIM=0;

double DEV1=0,AZIM1=0,DEV2=0,AZIM2=0;//全局变量
double HOFF=0,HOAZ=0;//叫水平位移,后者叫闭合方位
double rlev=0.125; 
double oldnDevi=0;
double oldnAzim=0;
double oldnDevi1[1000];
double oldnAzim1[1000];
double GLDDDEP=30;
double DEP0=0;
int no=0;
int NON=1;
int initTrackPoints(double azca,double tvd,double dxe,double dyn,double rlev0,double GLDDDEP0,double hoff)
{
	//====================================================================
	AZA0=180 ; //主投影方位
	AZCA=azca; //磁偏角 AZCA
	TVDS=tvd;//初始值?????????
	DZDA=0;//全局变量 
	FLSA=0; //FLSA=0.0为最小曲率法  FLSA=1.0为折线法
	FLAG=0;  //全局变量
	XE=dyn;
	YN=dxe;//井斜x,y坐标
	DEV1=0,AZIM1=0,DEV2=0,AZIM2=0;//全局变量
	HOFF=hoff,HOAZ=0;//叫水平位移,后者叫闭合方位	 
	ZD=tvd;//-w.earthEle-w.dEle ;    //??????????
	OLDDEVI=0;
	OLDAZIM=0;
	oldnDevi=0;
	oldnAzim=0;
	memset(oldnDevi1,0,sizeof(double)*1000);
	memset(oldnAzim1,0,sizeof(double)*1000);
	GLDDDEP=GLDDDEP0;
	DEP0=0;
	no=0;
	rlev=rlev0;
	if(rlev) NON=GLDDDEP/rlev+0.5;
	if(NON<1) NON=1;
	return 1;
}

int TrackPoints(double nDevi,double nAzim,double RLEA,double &XE0,double &YN0,double &ZD0,double &HOFF0,double &HOAZ0,double &GTD0,double &AZIM0)
{
	if(no<0) no=0;
	if(nDevi!=-9999.0&&nDevi!=-999.25&&nDevi!=-99999.0)
	{
		oldnDevi=nDevi;
		oldnAzim=nAzim;		
	}
	double DEVI=oldnDevi;
	double AZIM=oldnAzim;
	if(RLEA==0) RLEA=rlev;
	if(AZCA+nAzim<0) AZIM=AZCA+nAzim+360;
	else AZIM=AZCA+nAzim;
	double GTD=0;

	if(FLAG==1)
	{
		ZD=ZD+(1-pow((DEVI-OLDDEVI)*PI/180,2)/24.0)*RLEA*cos((OLDDEVI+DEVI)*PI/360);
		XE=XE+(1-(pow((DEVI-OLDDEVI)*PI/180,2)+
			pow(((AZIM-OLDAZIM<-180?AZIM-OLDAZIM+360:
				(AZIM-OLDAZIM>180?AZIM-OLDAZIM-360:AZIM-OLDAZIM)))*PI/180,2))/24)*RLEA
				*sin((DEVI+OLDDEVI)/2*PI/180)*cos(
					(AZIM-OLDAZIM>180?(AZIM+OLDAZIM-360)*PI/360:
					(AZIM-OLDAZIM<-180?(AZIM+OLDAZIM+360)*PI/360:(AZIM+OLDAZIM)*PI/360)));
		YN=YN+(1-(pow((DEVI-OLDDEVI)*PI/180,2)+pow((
			(AZIM-OLDAZIM<-180?AZIM-OLDAZIM+360:
				(AZIM-OLDAZIM>180,AZIM-OLDAZIM-360,AZIM-OLDAZIM)))*PI/180,2))/24)*RLEA*sin((DEVI+OLDDEVI)/2*PI/180)*sin(
					(AZIM-OLDAZIM>180?(AZIM+OLDAZIM-360)*PI/360:
						(AZIM-OLDAZIM<-180?(AZIM+OLDAZIM+360)*PI/360:(AZIM+OLDAZIM)*PI/360)));
		HOFF=sqrt(XE*XE+YN*YN);

		float val1=(XE>0)?((YN>0)?0:360):0;
		float val2=(XE<0)?180:0;

		HOAZ=atan(YN/XE)*180/PI+val1+val2;
		if(DEP0<GLDDDEP) {
			GTD=0;
		}
		else {
			GTD=sqrt(pow((DEVI-oldnDevi1[0])/GLDDDEP,2)+
			pow((AZIM-oldnAzim1[0]>180?AZIM-oldnAzim1[0]-360:
				AZIM-oldnAzim1[0]<-180?AZIM-oldnAzim1[0]+360:AZIM-oldnAzim1[0])/GLDDDEP,2)*
				pow(sin((DEVI+oldnDevi1[0])/2*PI/180),2))*GLDDDEP;//30
		}
	}
	else {
		HOFF=sqrt(XE*XE+YN*YN)+HOFF;
		float val1=(XE>0)?((YN>0)?0:360):0;
		float val2=(XE<0)?180:0;
		float X=abs(XE);                                                                      
		float Y=abs(YN) ;           
		if((XE>0.0) && (YN>0.0)) HOAZ=atan2(X,Y) ;                                     
		if((XE>0.0) && (YN<0.0)) HOAZ=PI-atan2(X,Y);                                   
		if((XE<0.0) && (YN<0.0)) HOAZ=PI+atan2(X,Y);                                   
		if((XE<0.0) && (YN>0.0)) HOAZ=2*PI-atan2(X,Y) ;                                 
		if((XE==0.0) && (YN>0.0)) HOAZ=0.;                                             
		if((YN==0.0) && (XE>0.0)) HOAZ=0.5*PI;                                          
		if((XE==0.0) && (YN<0.0)) HOAZ=PI  ;                                            
		if((YN==0.0) && (XE<0.0)) HOAZ=1.5*PI ;                                         
		HOAZ=(HOAZ*360.)/(2*PI) ;
		GTD=0;
	}
	FLAG=1;
	OLDDEVI=DEVI;
	OLDAZIM=AZIM;
	DEP0+=RLEA;
	if(DEP0>GLDDDEP) DEP0=GLDDDEP;
	if(rlev) {
		if(no>=NON) {
			no=NON-1;
			memmove(&oldnDevi1[0],&oldnDevi1[1],no*sizeof(double));
			memmove(&oldnAzim1[0],&oldnAzim1[1],no*sizeof(double));
		}
	}
	oldnDevi1[no]=DEVI;
	oldnAzim1[no]=AZIM;
	no++;
	/*
	double oldnDevi=0;
	double oldnAzim=0;
	////////////////////////////////////////////////////////////////////////////
	if(nDevi!=-9999.0)
	{
		oldnDevi=nDevi;
		oldnAzim=nAzim;		
	}


	double DEVI=oldnDevi;
	double AZIM=oldnAzim;
	double RLES,DX,DY,TVD,DDB,DDV,DDE,DDA,X,Y;

	DEVI=(DEVI/180.)*PI;
	AZIM= fmod(AZIM+AZCA+360.0,360.0);  //AZCA=???
	RLES=RLEA;
	AZIM=(AZIM/360.)*(2*PI);   //AZA0=???
	if(RLEA==0.1524) 
	{
		if((AZA0>=90.0) && (AZA0<180.0)) RLES=0.15241f;
		if((AZA0>=180.0) && (AZA0<270.0)) RLES=0.152422f;
		if((AZA0>=270.0) && (AZA0<=360.0)) RLES=0.1524333f;
		if((AZA0>=0.0) && (AZA0 <15.0)) RLES=0.152388f;
		if((AZA0>=15.0) && (AZA0 <90.0)) RLES=0.15242f;
	}
	if(RLEA==0.0762) 
	{
		if((AZA0>=90.0) && (AZA0<140.0)) RLES=0.07623f;
		if((AZA0>=140.0) && (AZA0<180.0)) RLES=0.0762332f;
		if((AZA0>=180.0) && (AZA0<270.0)) RLES=0.0762331f;
		if((AZA0>=270.0) && (AZA0<=360.0)) RLES=0.0762332f;
		if((AZA0>=0.0) && (AZA0<15.0)) RLES=0.076188f;
		if((AZA0>=15.0) && (AZA0<90.0)) RLES=0.07623325f;
	}

	if(FLAG==1.0)  // FLAG =????
		goto label_22;
	FLAG=1.0;
	DEV1=DEVI;
	AZIM1=AZIM;   
	goto label_11;
label_22 :     	 
	DEV2=DEVI;
	AZIM2=AZIM;      
	if(FLSA==0.0) 
	{
		if((DEV2==DEV1) && (AZIM2==AZIM1))  goto label_100;
		if((DEV2==DEV1) && (AZIM2!=AZIM1)) goto label_300;
		if((DEV2!=DEV1) && (AZIM2==AZIM1))  goto label_200; 
		if ((AZIM1<=1.5*PI) && (AZIM1>=0.5*PI)) goto label_500; 
		if((AZIM2<=1.5*PI) && (AZIM2>=0.5*PI))  goto label_500; 
		if((AZIM1>1.5*PI) && (AZIM2>1.5*PI))  goto label_500; 
		if((AZIM1<0.5*PI) && (AZIM2<0.5*PI))   goto label_500; 
		if((AZIM1<0.5*PI) && (AZIM2>1.5*PI))  goto label_400; 
		DDA=(DEV2-DEV1)*(AZIM2+2*PI-AZIM1);
		DDV=(cos(DEV1)-cos(DEV2))*(cos(AZIM1)-cos(AZIM2));
		DDE=(cos(DEV1)-cos(DEV2))*(sin(AZIM2)-sin(AZIM1));
		DX=(RLES*DDV)/DDA;
		DY=(RLES*DDE)/DDA;
		goto label_510;
label_400 :   DDB=(DEV2-DEV1)*(AZIM2-2*PI-AZIM1);
		DDV=(cos(DEV1)-cos(DEV2))*(cos(AZIM1)-cos(AZIM2));
		DDE=(cos(DEV1)-cos(DEV2))*(sin(AZIM2)-sin(AZIM1));
		DX=(RLES*DDV)/DDB;
		DY=(RLES*DDE)/DDB;
		goto label_510;
label_500 :   DDB=(DEV2-DEV1)*(AZIM2-AZIM1);
		DDV=(cos(DEV1)-cos(DEV2))*(cos(AZIM1)-cos(AZIM2));
		DDE=(cos(DEV1)-cos(DEV2))*(sin(AZIM2)-sin(AZIM1));
		DX=(RLES*DDV)/DDB;
		DY=(RLES*DDE)/DDB;
label_510 :   TVD=(RLES*(sin(DEV2)-sin(DEV1)))/(DEV2-DEV1);
		goto label_666;
label_100 :   DX=RLES*sin(DEV2)*sin(AZIM2);
		DY=RLES*sin(DEV2)*cos(AZIM2);
		TVD=RLES*cos(DEV2);
		goto label_666;
label_200 :   DX=(RLES*(cos(DEV1)-cos(DEV2))*sin(AZIM2))/(DEV2-DEV1);
		DY=(RLES*(cos(DEV1)-cos(DEV2))*cos(AZIM2))/(DEV2-DEV1);
		TVD=(RLES*(sin(DEV2)-sin(DEV1)))/(DEV2-DEV1);
		goto label_666;
label_300 :  if ((AZIM1<=1.5*PI) && (AZIM1>=0.5*PI)) goto label_600; 
		if((AZIM2<=1.5*PI) && (AZIM2>=0.5*PI))  goto label_600; 
		if((AZIM1>1.5*PI) && (AZIM2>1.5*PI))  goto label_600; 
		if((AZIM1<0.5*PI) && (AZIM2<0.5*PI))   goto label_600; 
		if((AZIM1<0.5*PI) && (AZIM2>1.5*PI))  goto label_550;
		DX=(RLES*(cos(AZIM1)-cos(AZIM2))*sin(DEV2))/(2*PI+AZIM2-AZIM1);
		DY=(RLES*(sin(AZIM2)-sin(AZIM1))*sin(DEV2))/(2*PI+AZIM2-AZIM1);
		goto label_650;
label_550 :  DX=(RLES*(cos(AZIM1)-cos(AZIM2))*sin(DEV2))/(AZIM2-AZIM1-2*PI);
		DY=(RLES*(sin(AZIM2)-sin(AZIM1))*sin(DEV2))/(AZIM2-AZIM1-2*PI);
		goto label_650;
label_600 :  DX=(RLES*(cos(AZIM1)-cos(AZIM2))*sin(DEV2))/(AZIM2-AZIM1);
		DY=(RLES*(sin(AZIM2)-sin(AZIM1))*sin(DEV2))/(AZIM2-AZIM1);
label_650 :  TVD=RLES*cos(DEV2);
label_666 :  
		XE=XE+DX;    
		YN=YN+DY;
		DZDA=DZDA+TVD;	//DZDA=?
		ZD=TVDS+DZDA;	//TVDS=?		
	}
	if(FLSA==1.0) 
	{
		DX=RLES*sin(DEV2)*sin(AZIM2);
		DY=RLES*sin(DEV2)*cos(AZIM2);
		TVD=RLES*cos(DEV2);
		XE=XE+DX;
		YN=YN+DY;
		DZDA=DZDA+TVD;
		ZD=TVDS+DZDA;
	}	   
	DEV1=DEV2;
	AZIM1=AZIM2;

label_11 : ;
	HOFF=sqrt(XE*XE+YN*YN);     
	X=abs(XE);                                                                      
	Y=abs(YN) ;           
	if((XE>0.0) && (YN>0.0)) HOAZ=atan2(X,Y) ;                                     
	if((XE>0.0) && (YN<0.0)) HOAZ=PI-atan2(X,Y);                                   
	if((XE<0.0) && (YN<0.0)) HOAZ=PI+atan2(X,Y);                                   
	if((XE<0.0) && (YN>0.0)) HOAZ=2*PI-atan2(X,Y) ;                                 
	if((XE==0.0) && (YN>0.0)) HOAZ=0.;                                             
	if((YN==0.0) && (XE>0.0)) HOAZ=0.5*PI;                                          
	if((XE==0.0) && (YN<0.0)) HOAZ=PI  ;                                            
	if((YN==0.0) && (XE<0.0)) HOAZ=1.5*PI ;                                         
	HOAZ=(HOAZ*360.)/(2*PI) ;
	float GTD=0;
	GTD=sqrt(pow((DEVI-OLDDEVI)/RLEA,2)+
		pow((AZIM-OLDAZIM>180?AZIM-OLDAZIM-360:
		AZIM-OLDAZIM<-180?AZIM-OLDAZIM+360:AZIM-OLDAZIM)/RLEA,2)*
		pow(sin((DEVI+OLDDEVI)/2*PI/180),2))*RLEA;//30

	OLDDEVI=DEVI;
	OLDAZIM=AZIM;
*/
	XE0=YN;
	YN0=XE;
	ZD0=ZD;
	HOAZ0=HOAZ;
	HOFF0=HOFF;
	GTD0=GTD;
	AZIM0=AZIM;
	return 1;
}