#include <math.h>
#include "vector3d.h"
#include "utilgeom.h"

Vector3d::Vector3d() 
{
  Setxyz(1.,0.,0.);
}

Vector3d::Vector3d(double x, double y, double z) 
{
  _x=x;
  _y=y;
  _z=z;
  xyz2ThetaPhi();
}

Vector3d::Vector3d(double theta, double phi) 
{
  _theta=mod(theta,M_PI); // dans [0;pi]
  _phi=mod(phi,pi2); // dans [0;2pi]
  ThetaPhi2xyz();
}

Vector3d::Vector3d(const LongLat& ll)
{
  _theta=ll.Theta(); // dans [0;pi]
  _phi=ll.Phi(); // dans [0;2pi]
  ThetaPhi2xyz();
}

Vector3d::Vector3d(const Vector3d& v) 
{
  _x=v._x;
  _y=v._y;
  _z=v._z;
  _theta=v._theta;
  _phi=v._phi;
}

void Vector3d::SetThetaPhi(double theta, double phi) 
{
  _theta=mod(theta,M_PI);
  _phi=mod(phi,pi2);
  ThetaPhi2xyz();
}

void Vector3d::Setxyz(double x, double y, double z) 
{
  _x=x;
  _y=y;
  _z=z;
  xyz2ThetaPhi();
}

void Vector3d::ThetaPhi2xyz()
{
  _x=sin(_theta)*cos(_phi);
  _y=sin(_theta)*sin(_phi);
  _z=cos(_theta);
}

void Vector3d::xyz2ThetaPhi()
{
  double norm=this->Norm();
  if( norm != 0. ) 
    {
      _theta=acos(_z/norm); // dans [0,Pi]
      if( mod(_theta,M_PI) == 0. ) _phi=0.; // on est sur +-Oz, le vecteur z est en phi=0
      //      else _phi=acos(_x/sin(_theta)/norm)+M_PI*(_y<0);
      else _phi=scangle(_y/sin(_theta)/norm,_x/sin(_theta)/norm);
    }
  else // vecteur nul
    {
      _theta=0.;
      _phi=0.;
    }
}

Vector3d& Vector3d::Normalize() 
{
  double norm=this->Norm();
  if( norm != 0. )  (*this)/=norm;
  else cerr << "Division par zero" << endl;
  return *this;
} 

double Vector3d::Norm() const 
{
  return sqrt(_x*_x+_y*_y+_z*_z);
}

double Vector3d::Psc(const Vector3d& v) const 
{
  return _x*v._x+_y*v._y+_z*v._z;
}

double Vector3d::SepAngle(const Vector3d& v) const 
{
  double n1=this->Norm();
  double n2=v.Norm();
  double ret;
  if( n1!=0. && n2!=0. ) ret=acos((this->Psc(v))/n1/n2);
  else 
    {
      cerr << "Division par zero" << endl;
      ret=0.;
    }
  return ret;
}

Vector3d Vector3d::Vect(const Vector3d& v) const 
{
  double xo=_y*v._z-_z*v._y;
  double yo=_z*v._x-_x*v._z;
  double zo=_x*v._y-_y*v._x;
  return Vector3d(xo,yo,zo);
}

Vector3d Vector3d::VperpPhi() const // vecteur perpendiculaire de meme phi
{
  double theta;
  if( _theta != pi_over_2 ) theta=_theta+(0.5-(_theta>pi_over_2))*M_PI; // on tourne theta de +-pi/2
  else theta=0.;
  return Vector3d(theta,_phi);
}

Vector3d Vector3d::VperpTheta() const // vecteur perpendiculaire de meme theta
{
  double phi=mod(_phi+pi_over_2,pi2); // on tourne phi de pi/2
  return Vector3d(_theta,phi);
}

Vector3d Vector3d::EPhi() const 
{
  Vector3d temp;
  if ( fabs(_z) == 1. ) // si on est en +- Oz
    {
      temp=Vector3d(1.,0.,0.);
    }
  else 
    {
      Vector3d k(0,0,-1);
      temp=this->Vect(k);
      temp.Normalize();
    }
  return temp;
}

Vector3d Vector3d::ETheta() const 
{
  Vector3d temp=this->Vect(EPhi());
  temp.Normalize();
  return temp;
}


Vector3d Vector3d::Euler(double phi, double theta, double psi) const 
{
  double cpsi=cos(psi);
  double ctheta=cos(theta);
  double cphi=cos(phi);
  double spsi=sin(psi);
  double stheta=sin(theta);
  double sphi=sin(phi);
  double xnew=(cpsi*cphi-ctheta*sphi*spsi)*_x
    +(cpsi*sphi+ctheta*cphi*spsi)*_y
    +spsi*stheta*_z;
  double ynew=(-spsi*cphi-ctheta*sphi*cpsi)*_x
    +(-spsi*sphi+ctheta*cphi*cpsi)*_y
    +cpsi*stheta*_z;
  double znew=stheta*sphi*_x-stheta*cphi*_y+ctheta*_z;
  return Vector3d(xnew,ynew,znew);
}

Vector3d Vector3d::InvEuler(double phi, double theta, double psi) const 
{
  double cpsi=cos(psi);
  double ctheta=cos(theta);
  double cphi=cos(phi);
  double spsi=sin(psi);
  double stheta=sin(theta);
  double sphi=sin(phi);
  double xnew=(cpsi*cphi-ctheta*sphi*spsi)*_x
    -(spsi*cphi+ctheta*sphi*cpsi)*_y
    +sphi*stheta*_z;
  double ynew=(cpsi*sphi+ctheta*cphi*spsi)*_x
    +(-spsi*sphi+ctheta*cphi*cpsi)*_y
    -cphi*stheta*_z;
  double znew=stheta*spsi*_x+stheta*cpsi*_y+ctheta*_z;
  return Vector3d(xnew,ynew,znew);
}

Vector3d Vector3d::Rotate(const Vector3d& omega, double phi)
{
  Vector3d rotationaxis=omega;
  rotationaxis.Normalize();
  double n=this->Psc(rotationaxis);
  Vector3d myself=*this;
  Vector3d rotate=n*rotationaxis+(myself-n*rotationaxis)*cos(phi)-(myself^rotationaxis)*sin(phi);
  return rotate;
}   

Vector3d& Vector3d::operator+=(const Vector3d& v) 
{
  *this=*this+v;
  return *this;
}

Vector3d& Vector3d::operator-=(const Vector3d& v) 
{
  *this=*this-v;
  return *this;
}

Vector3d& Vector3d::operator+=(double d) 
{
  Setxyz(_x+d,_y+d,_z+d);
  return *this;
}

Vector3d& Vector3d::operator/=(double d) 
{
  if( d != 0. ) Setxyz(_x/d,_y/d,_z/d);
  else cerr << "Division par zero." << endl;
  return *this;
}

Vector3d& Vector3d::operator*=(double d) 
{
  Setxyz(_x*d,_y*d,_z*d);
  return *this;
}

Vector3d Vector3d::operator^(const Vector3d& v) const 
{
  return this->Vect(v);
}

Vector3d Vector3d::operator+(double d) const 
{
  return Vector3d(_x+d,_y+d,_z+d);
}

Vector3d Vector3d::operator+(const Vector3d& v) const 
{
  return Vector3d(_x+v._x,_y+v._y,_z+v._z);
}

Vector3d Vector3d::operator-(double d) const 
{
  return *this+(-d);
}

Vector3d Vector3d::operator-(const Vector3d& v) const 
{
  return *this+(v*(-1.));
}

Vector3d Vector3d::operator*(double d) const 
{
  return Vector3d(d*_x,d*_y,d*_z);
}

double Vector3d::operator*(const Vector3d& v) const 
{
  return this->Psc(v);
}

Vector3d Vector3d::operator/(double d) const 
{
  Vector3d ret=*this;
  if( d != 0. ) ret/=d;
  else  cerr << "Division par zero." << endl;
  return ret;
}

Vector3d& Vector3d::operator=(const Vector3d& v) 
{
  if( this != &v ) 
    {
      _x=v._x; 
      _y=v._y; 
      _z=v._z; 
      _theta=v._theta; 
      _phi=v._phi;
    }
  return *this;
}

bool Vector3d::operator==(const Vector3d& v) 
{
  return (this==&v);
}

void Vector3d::Print(ostream& os) const 
{
  os << "Vector3: (X,Y,Z)= (" << _x << ", " << _y << ", " << _z 
     << ") --- Theta,Phi= " << _theta << ", " << _phi << "\n"
     << "Norme = " << this->Norm() << endl;
}