/*********************************************************************
// created:    2007/04/12 - 16:30

//
// author:     Elie Al Alam & Gerald Dherbomez
// 
// version:    $Id: DbtPlyGgaManager.cpp 1203 2012-08-02 11:58:15Z morasjul $
//
// purpose:    Dbite Player GGA Manager implementation
*********************************************************************/

#include "DbtPlyGgaManager.h"

#include <cmath>
#include <qapplication.h>

//#include "kernel/ComponentManager.h"

namespace pacpus {

//////////////////////////////////////////////////////////////////////////
// Construction de la fabrique de composant DbtPlyGgaManager
//////////////////////////////////////////////////////////////////////////
static ComponentFactory<DbtPlyGgaManager> sFactory("DbtPlyGgaManager"); 

// double dist1[2];
// double dist2[2];

double atanh(double  z)
{
	return (0.5 * log((z+1)/(1-z)));
}

double  abs_me(double x)
{
	if(x<0) x=-x;
	return x;
}




//////////////////////////////////////////////////////////////////////////
//transformation des longitudes tatitudes en Lambert93
//////////////////////////////////////////////////////////////////////////
void lonlattolam(double lon, double lat, double & lam93x, double & lam93y)
{
  double GRS_a = 6378137;
  double GRS_f = 1/298.257222101;
  
  double GRS_b = GRS_a*(1-GRS_f);
  double GRS_bb= GRS_b*GRS_b;
  double GRS_aa= 40680631590769.0;
  double GRS_e = sqrt((GRS_aa - GRS_bb) / (GRS_aa));
  
  double n = 0.725607765053267;
  double C = 11754255.4261;
  double XS = 700000;
  double YS = 12655612.0499;
  
  double latiso;
  latiso = atanh(sin(lat)) - GRS_e*atanh(GRS_e*sin(lat));
  double gamma;
  gamma = (lon - 0.0523598775598299)*n;
  double R;
  R = C * exp(-n*latiso);
  
  lam93x = R *sin(gamma)+XS;
  lam93y = -R *cos(gamma)+YS;
}





//////////////////////////////////////////////////////////////////////////
//Transformation des coordonnees de Lambert93 en Longitude lattitude
//////////////////////////////////////////////////////////////////////////
void lamtolonlat(double lamx, double lamy, double & lon, double & lat)
{
	double GRS_a = 6378137;
	double GRS_f = 1/298.257222101;
	double GRS_b = GRS_a*(1-GRS_f);
	double GRS_bb= GRS_b*GRS_b;
	double GRS_aa= 40680631590769.0;
	double GRS_e = sqrt((GRS_aa - GRS_bb) / (GRS_aa));

	//double n = 0.725607765053267;
	//double C = 11754255.4261;
	//double XS = 700000;
	//double YS = 12655612.0499;

	
	//lamx = lamx-700000;
	//lamy = lamy-12655612.0499;

	double gamma;
	gamma = atan(-(lamx-700000)/(lamy-12655612.0499));


	lon = gamma/0.725607765053267 + 0.0523598775598299;
	double R;
	R = sqrt((lamx-700000) * (lamx-700000) + (lamy-12655612.0499) * (lamy-12655612.0499));

	double latiso;
	latiso = log((11754255.4261)/R)/(0.725607765053267);

	double phiNew, phiOld;
	phiOld =1;
	phiNew= asin (tanh ( latiso + GRS_e * atanh(GRS_e * sin(phiOld)))); 
	//printf("\nphiNew: %.20lf",phiNew);
	while (abs_me(phiOld-phiNew) > 1e-10)
	{
		
		if(abs_me(phiOld-phiNew) > 1e-10)
		{
	
			phiOld = phiNew;
			phiNew = asin(tanh(latiso+GRS_e*atanh(GRS_e*sin(phiOld))));
		}
		else
			phiOld = phiNew;
	}
	
	lat = phiNew;
}





//////////////////////////////////////////////////////////////////////////
/// Constructor
DbtPlyGgaManager::DbtPlyGgaManager(QString name)
    : DbtPlyFileManager(name)
{

}

//////////////////////////////////////////////////////////////////////////
/// Destructor 
DbtPlyGgaManager::~DbtPlyGgaManager()
{
	
}

//////////////////////////////////////////////////////////////////////////
// displayData 
//////////////////////////////////////////////////////////////////////////
void DbtPlyGgaManager::processData(road_time_t t, road_timerange_t /*tr*/ , void * buf)
{
    // no data available
    if (buf == NULL) {
        printf("NULL");
        emit displayLat(0);
        emit displayLon(0);
    }
  
  val = (trame_gga_dbl*)(buf);


  emit displayLat(val->lat);
  emit displayLon(val->lon);

}

ComponentBase::COMPONENT_CONFIGURATION DbtPlyGgaManager::configureComponent(XmlComponentConfig config)
{
  DbtPlyFileManager::configureComponent(config);

  return ComponentBase::CONFIGURED_OK;
}




void DbtPlyGgaManager::startActivity()
{ 
  DbtPlyFileManager::startActivity();
  // user interface 
}




void DbtPlyGgaManager::stopActivity()
{
  DbtPlyFileManager::stopActivity();
}


void DbtPlyGgaManager::displayUI()
{
  // todo
	// afficher lat lon
	// voir dans DbtPlyUserInterface§.cpp
	w = new QWidget();
	w->setWindowTitle(name());
	w->show();
	w->setFixedSize (170,80);
	
	prop = new QGroupBox ("GGA Properties",w);
	prop->show();
	prop->setFixedSize(170,78);
  
	lat = new QLabel("Latitude ", prop);
	lat->move( 5, 15);
	lat->show();
	lat->setFixedSize (80,20);
	latVal = new QLCDNumber (prop);
	latVal->move( 5, 35);
	latVal->setFixedSize(50,20);
	latVal->show();
	connect(this, SIGNAL(displayLat(double)) , latVal , SLOT(display(double)));
  
	lon = new QLabel("Longitude ", prop);
	lon->move( 90, 15);
	lon->show();
	lon->setFixedSize (80,27);
  
	lonVal = new QLCDNumber (prop);
	lonVal->move( 90, 35);
	lonVal->setFixedSize(50,20);
	lonVal->show();
	connect (this, SIGNAL(displayLon(double)) , lonVal , SLOT(display(double)));
}

} // namespace pacpus