/*
 * Utilities relating to reactor anti-neutrinos allowing user to calculate
 * baseline distances and convert between different co-ordinate systems.
 *
 * Author: N. Barros <nfbarros@hep.upenn.edu>
 *         I. Coulter <icoulter@hep.upenn.edu>
 *
 *  Created on: Feb 10, 2015
 *      Author: I Coulter
 *          Moved from the ReactorGen to be shared with ReactorOsc processor
 */

#include <RAT/LLAUtil.hh>
#include <RAT/Log.hh>
#include <cmath>
#include <TMath.h>

namespace RAT {

double CalculateDistance(const G4ThreeVector &point1, const G4ThreeVector& point2) {
  return (point2 - point1).mag();
}

double GetReactorDistanceECEF(const G4ThreeVector &reactor_ecef_coords) {
  return CalculateDistance(SNO_ECEF_coord_,reactor_ecef_coords);
}

double GetReactorDistanceLLA(const G4ThreeVector &reactor_lla_coords) {
  return CalculateDistance(SNO_ECEF_coord_,LLAtoECEF(reactor_lla_coords.x(),reactor_lla_coords.y(),reactor_lla_coords.z()));
}

double GetReactorDistanceLLA(const double &longitude, const double&latitude, const double &altitude) {
  return CalculateDistance(SNO_ECEF_coord_,LLAtoECEF(longitude, latitude,altitude));
}

G4ThreeVector LLAtoECEF(double longitude, double latitude, double altitude) {
  // reference http://www.mathworks.co.uk/help/aeroblks/llatoecefposition.html
  static double toRad = TMath::Pi()/180.;
  static double Earthradius = 6378137.0; //Radius of the Earth (in meters)
  static double f = 1./298.257223563; //Flattening factor WGS84 Model
  static double L, rs, x, y, z;
  L = atan( pow((1. - f),2)*tan(latitude*toRad))*180./TMath::Pi();
  rs = sqrt( pow(Earthradius,2)/(1. + (1./pow((1. - f),2) - 1.)*pow(sin(L*toRad),2)));
  x = (rs*cos(L*toRad)*cos(longitude*toRad) + altitude*cos(latitude*toRad)*cos(longitude*toRad))/1000; // in km
  y = (rs*cos(L*toRad)*sin(longitude*toRad) + altitude*cos(latitude*toRad)*sin(longitude*toRad))/1000; // in km
  z = (rs*sin(L*toRad) + altitude*sin(latitude*toRad))/1000; // in km

  G4ThreeVector ECEF(x,y,z);

  return ECEF;
}

G4ThreeVector Direction(const double longitude,const  double latitude,const  double altitude)
{

  G4ThreeVector reactorxyz = LLAtoECEF(longitude, latitude, altitude);
  G4ThreeVector diff = SNO_ECEF_coord_ - reactorxyz;
  G4ThreeVector snoZ = SNO_ECEF_coord_.unit();

  static const double radian = TMath::Pi()/180.;

  G4ThreeVector z(0,0,1.0);
  double theta = snoZ.angle(z);

  G4ThreeVector snoX = z - snoZ*TMath::Cos(theta); //axis perpendicular so snoZ axis, in plane of z pointing (true) North
  G4ThreeVector snoY = snoZ.cross(snoX);

  snoX = snoX.unit(); //Normalise new vectors
  snoY = snoY.unit();

  theta = -40.4*radian; //angle to rotate axes from north to SNO coordinates
  snoX.rotate(theta,snoZ); //rotate to SNO coordinates
  snoY.rotate(theta,snoZ);


  G4ThreeVector direction(diff*snoX, diff*snoY, diff*snoZ);
  direction = direction.unit();
  return direction;
}

}