/***************************** LICENSE START ***********************************

 Copyright 2012 ECMWF and INPE. This software is distributed under the terms
 of the Apache License version 2.0. In applying this license, ECMWF does not
 waive the privileges and immunities granted to it by virtue of its status as
 an Intergovernmental Organization or submit itself to any jurisdiction.

 ***************************** LICENSE END *************************************/

#include "SatelliteProjection.h"

// =============================================================
// class SatelliteProjectionFactory - builds Satellite projections

class SatelliteProjectionFactory : public PmProjectionFactory
{
	virtual PmProjection* Build( const MvRequest& reqst )
		{ return new Satellite ( reqst ); }

public:
	SatelliteProjectionFactory() : PmProjectionFactory("SPACEVIEW") {}
};


// Instance of which is loaded in the "factory"
static SatelliteProjectionFactory satelliteProjectionFactoryInstance;
 
//=====================================================================
//
// SATELLITE projection
//
//
Satellite::Satellite ( const MvRequest& request ):
	PmProjection ( request )
{
	// Original parameters used by INPE's satellite projection
	// These parameters come from the GRIB documentation

	// These values are scaled by 10**3 in the GRIB file
	SPlo0 = request ( "SUBPAGE_MAP_SUB_SAT_LONGITUDE" );
	SPla0 = request ( "SUBPAGE_MAP_SUB_SAT_LATITUDE"  );
	SPyaw = request ( "SUBPAGE_MAP_GRID_ORIENTATION"  );

	SPlo0 *= DEG_TO_RAD * SCALE_THSD;
	SPla0 *= DEG_TO_RAD * SCALE_THSD;
	SPyaw *= DEG_TO_RAD * SCALE_THSD;

	// This value is scaled by 10**6 in the GRIB file
	SPrs  = request ( "SUBPAGE_MAP_CAMERA_ALTITUDE"   );
	SPrs *= Prd * SCALE_MILLION;

	SPis  = request ( "SUBPAGE_MAP_SUB_SAT_Y" );
	SPjs  = request ( "SUBPAGE_MAP_SUB_SAT_X" );

	double diay = request ( "SUBPAGE_MAP_X_EARTH_DIAMETER");
	double diax = request ( "SUBPAGE_MAP_Y_EARTH_DIAMETER");

	SPri  = 2. * asin( Prd/SPrs ) / diay;
	SPrj  = 2. * asin( Prd/SPrs ) / diax;

	SPscn = request ( "MAP_SCANNING_MODE");

	// Additional parameters used at ECMWF
	SProw0   = request ( "SUBPAGE_MAP_INITIAL_ROW"         );
	SPcol0   = request ( "SUBPAGE_MAP_INITIAL_COLUMN"      );

	SPnrows  = request ( "INPUT_IMAGE_ROWS"        );
	SPncols  = request ( "INPUT_IMAGE_COLUMNS"     );

	SPglobal = request ( "IMAGE_SUBAREA_SELECTION" );

	if ( request ( "IMAGE_SUBAREA_SELECTION" ) == Cached ( "ON" ) )
	{
    		Point inGeo ( (double) request ( "SUBPAGE_MAP_CENTRE_LONGITUDE" ),
			      (double) request ( "SUBPAGE_MAP_CENTRE_LATITUDE"  ) );
		inGeo.x ( inGeo.x () * DEG_TO_RAD );
 		inGeo.y ( inGeo.y () * DEG_TO_RAD );
		Point centerInProj = this->LL2PC ( inGeo );

    		inGeo.x ( (double) request ( "SUBPAGE_MAP_CORNER_LONGITUDE" ) );
		inGeo.y ( (double) request ( "SUBPAGE_MAP_CORNER_LATITUDE"  ) );
		inGeo.x ( inGeo.x () * DEG_TO_RAD );
 		inGeo.y ( inGeo.y () * DEG_TO_RAD );
		Point topRightInProj = this->LL2PC ( inGeo );
		
		Point botLeftInProj ( 2.0 * centerInProj.x () - topRightInProj.x (),
				      2.0 * centerInProj.y () - topRightInProj.y () );

      		Location projCoord ( topRightInProj.y (),
				     botLeftInProj.x  (),
				     botLeftInProj.y (),
				     topRightInProj.x () );

		// Store the projection coordinates for later use
		projCoord_ = projCoord;

		geodCoord_ = this->LatLongTransf ( projCoord );
	}
	else
		this->CalculateProjectionCoord ();
}

/************************************************************************
		GEODETIC COORDINATES TO SATELLITE PROJECTION
*************************************************************************/
Point
Satellite :: LL2PC(Point& ptll)
{
	double equad, n, x, y, z, sn, dp, dl,xlo,yla,lin,col,resx,resy;
	double x1, x2, a, b, c,  Rd2, Rm, Rm2, Rs2, v;

	xlo = (double)ptll.X();
	yla = (double)ptll.Y();

	// Cartesian geocentric coordinates
	xlo = xlo - SPlo0;
	yla = yla - SPla0;
	equad = 2.*Pflt - pow(Pflt,(double)2);
	n = Prd/sqrt((double)1-equad*pow(sin(yla),(double)2));
	x = n*cos(yla)*cos(xlo);
	y = n*cos(yla)*sin(xlo);
	z = (n*(1-equad))*sin(yla);
 
	// Field of view angles
	dp = atan(y / (SPrs - x));
	dl = atan(z * cos(dp) /(SPrs - x));

	// Visibility test
	if ( x < 0.0 )
	{
		col = MAXFLOAT;
		lin = MAXFLOAT;
		return (Point(col,lin));
	}
	else
	{
		Rd2 = Prd * Prd ;
		Rm  = Prd * (1 - Pflt);
		Rm2 = Rm * Rm ;
		Rs2 = SPrs * SPrs;

		v = (tan(dp)*tan(dp)*cos(dp)*cos(dp)*Rm2)+(tan(dl)*tan(dl)*Rd2);
		a = cos(dp)*cos(dp)*Rm2 + v;
		b = -2.*SPrs*v;
		c = Rs2*v-Rd2*Rm2*cos(dp)*cos(dp);

		// Equation Resolution a*x**2+b*x+c=0 
		v = (b*b) - (4.*a*c);
		if (v < 0.0) v=0.0;
   		x1 = (-b + sqrt(v)) / (2.*a);
		x2 = (-b - sqrt(v)) / (2.*a);
		if (x1 < x2)  x1 = x2;

		if ( fabs(x-x1) > 1.0 )
		{
			col = MAXFLOAT;
			lin = MAXFLOAT;
			return (Point(col,lin));
		}
	}

	// Line and column of image
	if (SPscn == 0.) sn = 1.0;  else sn = -1.0;
	col = (sn * dp / SPrj + SPjs );
	lin = (-sn * dl / SPri + SPis);

	// Axis rotation correction due to yaw angle
//	col = col*cos(SPyaw) - lin*sin(SPyaw);
//	lin = lin*cos(SPyaw) + col*sin(SPyaw);
	resx = tan(SPrj) * (SPrs - Prd);
	resy = tan(SPri) * (SPrs - Prd);

//	return (Point(col*resx,-(lin*resy)));
	return ( Point ( col*resx, (SPnrows-lin) * resy ) );
}

/************************************************************************
		SATELLITE PROJECTION TO GEODETIC COORDINATES
************************************************************************/
Point
Satellite :: PC2LL(Point& ptpc)
{
	double	dl, dp, x, y, z, x1, x2, equad, n, d, sn,
		a, b, c,  Rd2, Rm,Rm2, Rs2,v,ptpcx,ptpcy,
		yla,xlo,resx,resy;

	resx = tan(SPrj) * (SPrs - Prd);
	resy = tan(SPri) * (SPrs - Prd);
//	ptpcx = (double)ptpc.X()*SPncols;
//	ptpcy = (double)ptpc.Y()*SPnrows;
	ptpcx = (double)ptpc.X() / resx;
	ptpcy = SPnrows - (double)ptpc.Y() / resy;

	// Axis rotation correction due yaw angle
//	ptpcx = ptpcx*cos(SPyaw) + ptpcy*sin(SPyaw);
//	ptpcy = ptpcy*cos(SPyaw) - ptpcx*sin(SPyaw);

	// Field of view angles
	if (SPscn == 0.) sn =  1.0;
	else 		 sn = -1.0;
	dl = - sn * ((ptpcy - SPis) * SPri);
	dp =   sn * ((ptpcx - SPjs) * SPrj);

	// Cartesian coordinates
	Rd2 = Prd * Prd ;
	Rm  = Prd * (1 - Pflt);
	Rm2 = Rm * Rm ;
	Rs2 = SPrs * SPrs;

	v = (tan(dp)*tan(dp)*cos(dp)*cos(dp)*Rm2)+(tan(dl)*tan(dl)*Rd2);
	a = cos(dp)*cos(dp)*Rm2 + v;
	b = -2.*SPrs*v;
	c = Rs2*v-Rd2*Rm2*cos(dp)*cos(dp);

	// Equation Resolution a*x**2+b*x+c=0 
	v = (b*b) - (4.*a*c);
	if (v < 0)
	{
//		xlo = MAXFLOAT;
//		yla = MAXFLOAT;
		xlo = 90  * DEG_TO_RAD;
		yla = 180 * DEG_TO_RAD;
		return (Point(xlo,yla));
	}

   	x1 = (-b + sqrt(v)) / (2.*a);
	x2 = (-b - sqrt(v)) / (2.*a);

	if ( x1 >= x2)  x = x1;
	else		x = x2;

	z = (SPrs - x) * tan(dl) / cos(dp);
	y = (SPrs - x) * tan(dp);

	// Geodetic coordinates
	equad 	= 2. * Pflt - Pflt * Pflt;

	double aux = SPrs * dl / Prd;
	if (aux > 1.0) aux = 1.0;
	if (aux < -1.0) aux = -1.0;

	yla = asin(aux);
	
	do
	{
		n= Prd/sqrt((double)1-equad*pow(sin(yla),(double)2));
		yla = atan((z + n*equad*sin(yla))/sqrt(x*x + y*y));
		d = Prd/sqrt((double)1-equad*pow(sin(yla),(double)2)) - n;
	}
	while (fabs(d) > 0.001);

	xlo = atan(y/x) + SPlo0;
	yla = yla + SPla0;

	return (Point(xlo,yla));
}

void
Satellite::CalculateProjectionCoord ()
{
	double resx = tan(SPrj) * (SPrs - Prd);
	double resy = tan(SPri) * (SPrs - Prd);

	Location projCoord ( SPnrows * resy,	//projUpperRight.Y(),
			     0.0,		//projLowerLeft.X(),
			     0.0,		//projLowerLeft.Y(),
			     SPncols * resx );  	//projUpperRight.X() );

	// Store the projection coordinates for later use
	projCoord_ = projCoord;
}

bool
Satellite::CheckGeodeticCoordinates (Location& area)
{
	area = geodCoord_;
	return true;
}
