// Traj3DShow.cpp: implementation of the CTraj3DShow class.
//
//////////////////////////////////////////////////////////////////////


#include "matlab.hpp"
#include "libmwsglm.hpp"
#include "stdafx.h"
#include "RADIO.h"
#include "Traj3DShow.h"
#include "APPStatic.h"


#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif


const double CTraj3DShow::PI=3.141593;
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CTraj3DShow::CTraj3DShow()
{
	m_dPlaneScale=200;
}

CTraj3DShow::~CTraj3DShow()
{

}

void CTraj3DShow::RefreshTraj()
{ 
	TRAJ traj=APPStatic::GetMainView()->m_fileAccess.GetCurrentPackageData();
    mwArray x=get(APPStatic::GetMainView()->m_mwhLineTraj,"xdata");
	mwArray y=get(APPStatic::GetMainView()->m_mwhLineTraj,"ydata");
	mwArray z=get(APPStatic::GetMainView()->m_mwhLineTraj,"zdata");

    double newx=C2Rad(traj.lon-m_ptOrigin.x)/m_C2;
	double newy=C2Rad(traj.lat-m_ptOrigin.y)/m_C1;
    double newz=traj.heg;

    x=horzcat(x,newx);
	y=horzcat(y,newy);
	z=horzcat(z,newz);
	set(APPStatic::GetMainView()->m_mwhLineTraj,"xdata",x,"ydata",y,"zdata",z);
	
	//更新Plane	
	m_plane.ScaleOriginal(100,100,100);
	m_plane.Rotate(horzcat(0,0,1),90,horzcat(0,0,0));
	m_plane.Rotate(horzcat(0,0,1),-traj.yaw,horzcat(0,0,0));
	m_plane.Rotate(horzcat(180-traj.yaw,0),-traj.pitch,horzcat(0,0,0));
	m_plane.Rotate(horzcat(90-traj.yaw,traj.pitch),traj.roll,horzcat(0,0,0));
	m_plane.OffSet(newx,newy,newz);
	double dScale=APPStatic::GetMainView()->m_dlgSheet2.GetPlaneScale();
	if(APPStatic::GetMainView()->m_bTracePlane){
		Vset(APPStatic::GetMainView()->m_mwhAxis,"xlim",horzcat(newx-2*dScale*m_dPlaneScale,newx+2*dScale*m_dPlaneScale),
			"ylim",horzcat(newy-dScale*m_dPlaneScale,newy+dScale*m_dPlaneScale),"zlim",horzcat(newz-dScale*m_dPlaneScale,newz+dScale*m_dPlaneScale));
        }
}


void CTraj3DShow::Rotate(mwArray h, mwArray azel, mwArray alpha, mwArray origin)
{
//把h沿指定方向和原點(diǎn)轉(zhuǎn)
// ROTATE(H,[THETA PHI],ALPHA,[x0,y0,z0]) 
//或ROTATE(H,[X Y Z],ALPHA,[x0,y0,z0]) 
//逆時(shí)針轉(zhuǎn)為正。角度單位是度

	mwArray theta,phi,u;
	mwArray alph,cosa,sina,vera;
	mwArray x,y,z;
	mwArray rot;
	mwArray t;
	mwArray skip;
	mwArray m,n;
	mwArray newxyz,newx,newy,newz;
	mwArray p;
	
    // 得到轉(zhuǎn)軸的單位矢量
	if (tobool(prod(size(azel)) == mwArray(2)))//theta, phi
	{ 
		theta = 3.141593*azel(1)/mwArray(180.0);
		phi = 3.141593*azel(2)/mwArray(180.0);
		u = vertcat(cos(phi)*cos(theta), cos(phi)*sin(theta),sin(phi));
	}
	else if (tobool(prod(size(azel)) == mwArray(3)))// 方向向量
	{   
		u = azel(colon())/norm(azel);
	}
	//
	alph = alpha*3.141593/mwArray(180.0);
	cosa = cos(alph);
	sina = sin(alph);
	vera = mwArray(1) - cosa;
	x = u(1);
	y = u(2);
	z = u(3);
	rot =transpose(vertcat(horzcat(cosa+x*x*vera ,x*y*vera-z*sina ,x*z*vera+y*sina),
	                       horzcat(x*y*vera+z*sina ,cosa+y*y*vera, y*z*vera-x*sina),
                           horzcat(x*z*vera-y*sina ,y*z*vera+x*sina ,cosa+z*z*vera)));
	//
	for (int i=1;i<=(prod(size(h)).ExtractScalar(1));i++)
	{
		t = get(h(i),"type");
		skip = mwArray(0);
		if (tobool(strcmp(t,"surface")) || tobool(strcmp(t,"patch")) || tobool(strcmp(t,"line")))
		{
			x = get(h(i),"xdata");
			y = get(h(i),"ydata");
			z = get(h(i),"zdata");
			if (tobool(isempty(z)))
			{
				z = -origin(3)*ones(size(y));
			}
			m= size(z,1);
			n= size(z,2);
			if (tobool(prod(size(x)) < m*n))
			{   
				x = meshgrid(&y,x,y);
			}
		}
		else if (tobool(strcmp(t,"text")))
			{
				p = get(h(i),"position");
				x = p(1); y = p(2); z = p(3);
			}
		else if (tobool(strcmp(t,"image")))
			{
				x = get(h(i),"xdata");
				y = get(h(i),"ydata");
				z = zeros(size(x));
			}
		else
			{
				skip = mwArray(1);
			}
		
		
		if (!tobool(skip))
		{
			m=size(x,1);
			n=size(x,2);
			newxyz = horzcat(x(colon())-origin(1), y(colon())-origin(2), z(colon())-origin(3));
			newxyz = newxyz*rot;
			newx = origin(1) + reshape(newxyz(colon(),1),m,n);
			newy = origin(2) + reshape(newxyz(colon(),2),m,n);
			newz = origin(3) + reshape(newxyz(colon(),3),m,n);
			
			if (tobool(strcmp(t,"surface")) || tobool(strcmp(t,"patch")) || tobool(strcmp(t,"line")))
			{
				Vset(h(i),"xdata",newx,"ydata",newy,"zdata",newz);
			}
			else if (tobool(strcmp(t,"text")))
			{
				Vset(h(i),"position",horzcat(newx,newy,newz));
			}
			else if (tobool(strcmp(t,"image")))
			{
				Vset(h(i),"xdata",newx,"ydata",newy);
			}
		}
	}	
}

double CTraj3DShow::C2Rad(double d, double m, double s)
{
//度分秒轉(zhuǎn)換為弧度。
//例：C2Rad(30,0,0);
return ((s/60.0+m)/60.0+d)*PI/180.0;
}

void CTraj3DShow::Init3DShow(double lon,double lat,double h)
{
	//輸入是起始點(diǎn)位置，單位是度,（經(jīng)度，緯度，高度）
	m_ptOrigin.x=lon;
	m_ptOrigin.y=lat;
	m_ptOrigin.z=h;
    //畫(huà)起始經(jīng)緯度
	CString strOriLa,strOriPhi;
	strOriLa.Format("%6.3lf",m_ptOrigin.x);
	strOriPhi.Format("%6.3lf",m_ptOrigin.y);
	APPStatic::GetMainView()->m_dlgSheet2.m_strOriLa=strOriLa;
	APPStatic::GetMainView()->m_dlgSheet2.m_strOriPhi=strOriPhi;
    APPStatic::GetMainView()->m_dlgSheet2.m_staticOriLa.SetPaintInfo(strOriLa);
	APPStatic::GetMainView()->m_dlgSheet2.m_staticOriPhi.SetPaintInfo(strOriPhi);
	APPStatic::GetMainView()->m_dlgSheet2.UpdateData(FALSE);
	//
	double phi_=C2Rad(m_ptOrigin.y,0,0);
	double e=0.081819191,Re=6378137.0;
    mwArray Rm=mwArray(Re)*mwArray(1-e*e)/power(sqrt((1-e*e*sin(phi_)*sin(phi_))),mwArray(3));
    mwArray C1=1/(Rm+h);//緯度比
    mwArray Rn=Re/sqrt((1-e*e*sin(phi_)*sin(phi_)));
    mwArray C2=sec(phi_)/(Rn+h);//經(jīng)度比
	m_C1=C1.ExtractScalar(1);
    m_C2=C2.ExtractScalar(1);
	Vset(APPStatic::GetMainView()->m_mwhLineTraj,"xdata",mwArray(0),"ydata",mwArray(0),"zdata",mwArray(m_ptOrigin.z));
    m_plane.ScaleOriginal(m_dPlaneScale,m_dPlaneScale,m_dPlaneScale);
	m_plane.OffSet(0,0,m_ptOrigin.z);
	m_plane.Show();
	//view(APPStatic::GetMainView()->m_mwhAxis,-36,16);
    Vaxis(APPStatic::GetMainView()->m_mwhAxis,"equal");
	Vaxis(APPStatic::GetMainView()->m_mwhAxis,"auto");
    RefreshTraj();
}