//
// C++ Implementation: viewing_geometry
//
// Description: Compute the position of viewed locations
//
// Author: Nicolas PASCAL, (C) 2012
//
// Copyright: See COPYING file that comes with this distribution
//
//

#include "viewing_geometry.h"

void Viewing::earth_to_sat_from_range(  const Carthesian::Point3D & p_v,
                                        const double & theta_v, const double & phi_v, const double & slant_range,
                                        Carthesian::Point3D & p_s) {
    earth_to_sat_from_range(p_v.to_geodetic(), theta_v, phi_v, slant_range, p_s);
}


void Viewing::earth_to_sat_from_range(  const Geodetic::Point3D & p_v,
                                        const double & theta_v, const double & phi_v, const double & slant_range,
                                        Carthesian::Point3D & p_s) {
    // all angles needed in radians
    double _lat_v   = radians (p_v.lat);
    double _lon_v   = radians (p_v.lon);
    double _theta_v = radians (theta_v);
    double _phi_v   = radians (phi_v);
    // precomputations for speed
    double clat_v = cos(_lat_v);
    double slat_v = sin(_lat_v);
    double clon_v = cos(_lon_v);
    double slon_v = sin(_lon_v);
    // --- computes directionnal unit vectors
    // normal to surface vector
    Carthesian::Vector3D v_unit_norm(clat_v * clon_v, clat_v * slon_v, slat_v);
//     std::cout << "v_unit_norm " << v_unit_norm << std::endl;
    // at observer point, east unit vector
    Carthesian::Vector3D v_unit_east(-slon_v, clon_v, 0);
//     std::cout << "v_unit_east " << v_unit_east << std::endl;
    // at view point, north unit vector = v_unit_n x v_unit_east
    Carthesian::Vector3D v_unit_north = Carthesian::crossproduct(v_unit_norm, v_unit_east);
//     std::cout << "v_unit_north " << v_unit_north << std::endl;
    // rotate v_unit_norm of viewing azimuth around North axis -> inclination of normal
    Carthesian::Vector3D vs_inc = Carthesian::rotate(v_unit_norm, v_unit_east, -_theta_v);
//     std::cout << "vs_inc " << vs_inc << std::endl;
    // rotate vs inclinated of -phi_v around normal
    Carthesian::Vector3D vs = Carthesian::rotate(vs_inc, v_unit_norm, -_phi_v);
//     std::cout << "vs " << vs << std::endl;
    // translate view point to satellite position using the viewing vector and the slant range
//     p_s = p_v.to_carthesian(Earth::WGS84_MODEL);
    p_s = p_v.to_carthesian();
    p_s.translate(vs * slant_range);
//     std::cout << "p_s " << p_s << "\t" << p_s.to_spherical() << "\þ" << p_s.to_geocentric() << std::endl;
}

double Viewing::alt_to_range(const Carthesian::Point3D & p_v, const double & theta_v, const double & alt_sat) {
    return alt_to_range (p_v.to_geodetic(), theta_v, alt_sat);
}

double Viewing::alt_to_range(const Geodetic::Point3D & p_v, const double & theta_v, const double & alt_sat) {
    double Rearth = Earth::get_radius();
    double Rp = Rearth + p_v.alt;
    double Rs = Rearth + alt_sat;
    double _theta_v = radians (theta_v);
    double stheta_v = sin(_theta_v);
    return sqrt (Rs * Rs - (Rp * Rp) * (stheta_v * stheta_v)) - Rp * cos(_theta_v);
}

void Viewing::earth_to_sat_from_alt(const Carthesian::Point3D & p_v,
                                    const double & theta_v, const double & phi_v, const double & alt_sat,
                                    Carthesian::Point3D & p_s) {
    double slant_range = Viewing::alt_to_range (p_v, theta_v, alt_sat);
//     std::cout << "slant_range " << slant_range << std::endl;
    Viewing::earth_to_sat_from_range(p_v, theta_v, phi_v, slant_range, p_s);
}

void Viewing::earth_to_sat_from_alt(const Geodetic::Point3D & p_v,
                                    const double & theta_v, const double & phi_v, const double & alt_sat,
                                    Carthesian::Point3D & p_s) {
    Viewing::earth_to_sat_from_alt(p_v.to_carthesian(), theta_v, phi_v, alt_sat, p_s);
}