#include "Dihedral_angle.h"

#include <cmath>
#include <limits>

#include "Point_3D.h"
#include "Defs.h"

namespace MM
{

Dihedral_angle::
Dihedral_angle (Point_3D const & vector1,
                Point_3D const & vector2,
                Point_3D const & vector3,
                Point_3D const & vector4 )
:
    vector1_( vector1 ),
    vector2_( vector2 ),
    vector3_( vector3 ),
    vector4_( vector4 )
{
}

double Dihedral_angle::
value () const
{                                              // CONH-0.hin
    double ab_x = vector2_.x() -  vector1_.x();//-0.50163800000000
    double ab_y = vector2_.y() -  vector1_.y();//-0.70981900000000
    double ab_z = vector2_.z() -  vector1_.z();// 0.86888200000000

    double bc_x = vector3_.x() -  vector2_.x();//-0.60631800000000
    double bc_y = vector3_.y() -  vector2_.y();// 0.55829700000000
    double bc_z = vector3_.z() -  vector2_.z();// 1.0501960000000

    double cd_x = vector4_.x() -  vector3_.x();//-0.055202000000000
    double cd_y = vector4_.y() -  vector3_.y();// 1.1949100000000
    double cd_z = vector4_.z() -  vector3_.z();// 0.095616000000000

    // n1 = ab*bc
    // n2 = bc*cd
    double n1_x = ab_y * bc_z - ab_z * bc_y;//-1.2305432884780
    double n1_y = ab_z * bc_x - ab_x * bc_z;//-5.7542799991595e-007
    double n1_z = ab_x * bc_y - ab_y * bc_x;//-0.71043902692800

    double n2_x = bc_y * cd_z - bc_z * cd_y;//-1.2015075764080
    double n2_y = bc_z * cd_x - bc_x * cd_z;// 7.8229600012497e-007
    double n2_z = bc_x * cd_y - bc_y * cd_x;//-0.69367633038600

    double norm = sqrt( ( n1_x*n1_x + n1_y*n1_y + n1_z*n1_z )*
                        ( n2_x*n2_x + n2_y*n2_y + n2_z*n2_z ) );//1.9713218213674

    //fix
    if (norm <= std::numeric_limits<float>::epsilon())
    {
        #ifndef NDEBUG
        //FLAW ("Devide by zero.");
        #endif

        return 0.;
    }

    // cos_of_angle = n1^n2 / norm
    double cos_of_angle = ( n1_x*n2_x + n1_y*n2_y + n1_z*n2_z ) / norm; //0.99999999999943
    
    if (cos_of_angle <= -1.)
        return __Pi__;
    if (cos_of_angle >= 1.)
        return 0;

    // ( n1 * n2 ^ bc ) > 0. ? 1. : -1.
    double sign = 1.;
    double cross_product_x = n1_y * n2_z - n1_z * n2_y;
    double cross_product_y = n1_z * n2_x - n1_x * n2_z;
    double cross_product_z = n1_x * n2_y - n1_y * n2_x;
    double dot_product = cross_product_x * bc_x
                       + cross_product_y * bc_y
                       + cross_product_z * bc_z;
    if( dot_product < 0. )
        sign = -1.;

    return sign * acos( cos_of_angle );
}

/*// ChemCon
inline double 
torsion (const Atom &atom_a, const Atom &atom_b, 
         const Atom &atom_c, const Atom &atom_d )
{
    //fix Point_3D ?
    Vector a; a.x_ = atom_a.x(); a.y_ = atom_a.y(); a.z_ = atom_a.z();  
    Vector b; b.x_ = atom_b.x(); b.y_ = atom_b.y(); b.z_ = atom_b.z();  
    Vector c; c.x_ = atom_c.x(); c.y_ = atom_c.y(); c.z_ = atom_c.z();  
    Vector d; d.x_ = atom_d.x(); d.y_ = atom_d.y(); d.z_ = atom_d.z();  

    Vector ab = b - a;
    Vector bc = c - b;
    Vector cd = d - c;
    Vector n1 = norm( ab * bc );
    Vector n2 = norm( bc * cd );

    real  angle = (real)( n1 ^ n2 );
    if( angle < -1.0 )
    {
        if( angle + 1.0 < -d_mach_eps_2 )
          MM::to_user().error( "real Model::get_torsion\n angle + 1.0 < -d_mach_eps_2" );
        return  -1;
    }

    return  ( ( ( n1 * n2 ^ bc ) > 0.0 ) ? 1 : -1 ) * acos( angle );
}//*/

}//MM
