#include "Angle.h"

#include "Point_3D.h"
#include <cmath>
#include <limits>

namespace MM
{

Angle::
Angle (Point_3D const & vector1,
       Point_3D const & vector2,
       Point_3D const & vector3 )
:
    vector1_(vector1),
    vector2_(vector2),
    vector3_(vector3)
{
    //double test1 = vector1_.x();
    //double test2 = vector2_.x();
    //double test3 = vector3_.x();
}

/** acos( norm( v1 - v2 ) ^ norm( v3 - v2 ) ); */   
double Angle::
value () const //fix to vector operations        //fix speed
{
    double vector_21_x = vector1_.x() - vector2_.x();
    double vector_21_y = vector1_.y() - vector2_.y();
    double vector_21_z = vector1_.z() - vector2_.z();

    double vector_23_x = vector3_.x() - vector2_.x();
    double vector_23_y = vector3_.y() - vector2_.y();
    double vector_23_z = vector3_.z() - vector2_.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
    if (module_21 <= std::numeric_limits<float>::epsilon() ||
        module_23 <= std::numeric_limits<float>::epsilon())
    {
        #ifndef NDEBUG
        //FLAW ("Devide by zero.");
        #endif

        return 0.;
    }

    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 );
}

}//MM
