/**********************************************************************
 
	Copyright (C) 2003 Hirohisa MORI <joshua@nichibun.ac.jp>
 
	This program is free software; you can redistribute it 
	and/or modify it under the terms of the GLOBALBASE 
	Library General Public License (G-LGPL) as published by 

	http://www.globalbase.org/
 
	This program is distributed in the hope that it will be 
	useful, but WITHOUT ANY WARRANTY; without even the 
	implied warranty of MERCHANTABILITY or FITNESS FOR A 
	PARTICULAR PURPOSE.

**********************************************************************/

#include <math.h>
#include "coordinate_conv.h"

static LONG_FLOAT get_latitude_r(const VPF_COORDINATE *itrf94, LONG_FLOAT l, LONG_FLOAT n){

	LONG_FLOAT p,e;
	LONG_FLOAT a=6378137;
	LONG_FLOAT f=1/298.2572236;
	LONG_FLOAT l_,n_;
	LONG_FLOAT diff;

	p = sqrt(itrf94->x*itrf94->x + itrf94->y*itrf94->x);
	e = sqrt( (2*f) - (f*f) );

	l_ = atan(itrf94->z / (p-(e*e)*n*cos(l)));
	
	diff = l_-l;
	if(diff<0){
		diff = -diff;
	}
	if(diff < 1/1000000000000.0){
		return l_;
	}
	else{
		n_ = a/sqrt(1-(e*e*sin(l)*sin(l)));
		return get_latitude_r(itrf94, l_, n_);
	}
}


void get_wgs84_from_itrf94(VPF_WGS84_COORDINATE *wgs84, const VPF_COORDINATE *itrf94)
{
	VPF_COORDINATE itrf94_;
	LONG_FLOAT p,n,n0,e,l0;
	LONG_FLOAT a=6378137;
	LONG_FLOAT f=1/298.2572236;
	
	// move origin
	itrf94_.x = itrf94->x + 0.078;
	itrf94_.y = itrf94->y - 0.505;
	itrf94_.z = itrf94->z - 0.253;

	// calclate longitude
	wgs84->longitude = atan(itrf94_.y/itrf94_.x) ;
	
	// calclate latitude
	p = sqrt(itrf94_.x*itrf94_.x + itrf94_.y*itrf94_.x);
	l0 = atan(itrf94_.z/p);
	e = sqrt( (2*f) - (f*f) );
	n0 = a/sqrt(1-(e*e*sin(l0)*sin(l0)));
	wgs84->latitude = get_latitude_r(&itrf94_, l0, n0);
	
	// calclate height
	n = a/sqrt(1-(e*e*sin(wgs84->latitude)*sin(wgs84->latitude)));
	wgs84->height = p/cos(wgs84->latitude) - n;
}
