#ifndef GEOMETRY_H
#define GEOMETRY_H

#ifndef USER_H
#include "User.h"
#endif

#ifndef  MACH_EPS_H
#include "Mach_eps.h" 
#endif

#ifndef VECT_H
#include "vect.h" 
#endif

#ifndef ATOM_CACHE_H
#include "Atom_cache.h" 
#endif

//#ifndef INCLUDED_MATH_H
//#include <math.h> 
//#define INCLUDED_MATH_H
//#endif
#include <cmath>
#include <limits>

namespace MM
{

inline double
distance (Atom_cache const point_1, Atom_cache const point_2)
{
    double dx = point_1.x___ - point_2.x___;
    double dy = point_1.y___ - point_2.y___;
    double dz = point_1.z___ - point_2.z___;

    return sqrt (dx * dx + dy * dy + dz * dz);
}

inline double
distance_2 (Atom_cache const point_1, Atom_cache const point_2)
{
    double dx = point_1.x___ - point_2.x___;
    double dy = point_1.y___ - point_2.y___;
    double dz = point_1.z___ - point_2.z___;

    return dx * dx + dy * dy + dz * dz;
}

template <typename T> inline double 

length (const T & point_1,
        const T & point_2)  //fix to vector operations //fix speed
{
    double dx = point_1.x() - point_2.x();
    double dy = point_1.y() - point_2.y();
    double dz = point_1.z() - point_2.z();

    return sqrt (dx * dx + dy * dy + dz * dz);
}

template <typename T> inline double 
length_2 (const T & point_1,
        const T & point_2)  //fix to vector operations //fix speed
{
    double dx = point_1.x() - point_2.x();
    double dy = point_1.y() - point_2.y();
    double dz = point_1.z() - point_2.z();

    return dx * dx + dy * dy + dz * dz;
}

template <typename T> inline double 
angle (const T & point_1, 
       const T & point_2, 
       const T & point_3)  //fix to vector operations //fix speed
{
    double vector_21_x = point_1.x() - point_2.x();
    double vector_21_y = point_1.y() - point_2.y();
    double vector_21_z = point_1.z() - point_2.z();

    double vector_23_x = point_3.x() - point_2.x();
    double vector_23_y = point_3.y() - point_2.y();
    double vector_23_z = point_3.z() - point_2.z();

    double module_21 = sqrt (vector_21_x * vector_21_x
                           + vector_21_y * vector_21_y
                           + vector_21_z * vector_21_z);

    double module_23 = sqrt (vector_23_x * vector_23_x
                           + vector_23_y * vector_23_y
                           + vector_23_z * vector_23_z);

    //fix to k=1/mod
    vector_21_x /= module_21;
    vector_21_y /= module_21;
    vector_21_z /= module_21;

    vector_23_x /= module_23;
    vector_23_y /= module_23;
    vector_23_z /= module_23;

    double dot_product = vector_21_x * vector_23_x
                       + vector_21_y * vector_23_y
                       + vector_21_z * vector_23_z;

    return acos (dot_product);
}

template <typename T> inline double 
torsion (const T & a, const T & b, const T & c, const T & d)
{
    double ab_x = b.x() -  a.x();//-0.50163800000000
    double ab_y = b.y() -  a.y();//-0.70981900000000
    double ab_z = b.z() -  a.z();// 0.86888200000000

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

    double cd_x = d.x() -  c.x();//-0.055202000000000
    double cd_y = d.y() -  c.y();// 1.1949100000000
    double cd_z = d.z() -  c.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 );
}
//template <typename T> inline double 
//torsion (const T & point_a,
//         const T & point_b,
//         const T & point_c,
//         const T & point_d)
//{
//    //fix Point_3D ?
//    Vector a; a.x_ = point_a.x(); a.y_ = point_a.y(); a.z_ = point_a.z();  
//    Vector b; b.x_ = point_b.x(); b.y_ = point_b.y(); b.z_ = point_b.z();  
//    Vector c; c.x_ = point_c.x(); c.y_ = point_c.y(); c.z_ = point_c.z();  
//    Vector d; d.x_ = point_d.x(); d.y_ = point_d.y(); d.z_ = point_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);
//    double  angle = 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

#endif//GEOMETRY_H

