#include "Joint.h"

#include "Vector_3D_impl.h"
#include "Atom.h"
#include "Residue.h"
#include "Molecular_width_iterator.h"
#include "Arbitrary_atom_group.h"
#include "Point_3D.h"
#include "Mach_eps.h"
#include "Defs.h"
#include "Create.h"


#include <math.h>

namespace MM
{
Joint::
Joint (Prototype &)
:
    first_point_     (0),
    second_point_    (0),
    third_point_     (0),
    stub_atom_       (0),
    terminal_atom_   (0),
    directional_atom_(0),
    in_residue_      (&Residue::none())
{}

Joint::
Joint (Point_3D const & first_point,
       Point_3D const & second_point,
       Point_3D const & third_point,
       Atom           & stub_atom,
       Atom           & terminal_atom,
       Atom           & directional_atom,
       Atom_group *adoptee)
:   
    atoms_           (adoptee),
    first_point_     (&first_point),
    second_point_    (&second_point),
    third_point_     (&third_point),
    stub_atom_       (&stub_atom),
    terminal_atom_   (&terminal_atom),
    directional_atom_(&directional_atom),
    in_residue_      (&Residue::none())
{
}

Joint::
Joint (Atom & stub_atom, Atom & terminal_atom, Atom & directional_atom)
:   
    first_point_     (&stub_atom.        position()),
    second_point_    (&terminal_atom.    position()),
    third_point_     (&directional_atom. position()),
    stub_atom_       (&stub_atom),
    terminal_atom_   (&terminal_atom),
    directional_atom_(&directional_atom),
    in_residue_      (&Residue::none())
{}

void Joint::
set_stub_atom (Atom & stub_atom)
{
    stub_atom_   = &stub_atom;
    first_point_ = &stub_atom.position();
}

void Joint::
set_directional_atom (Atom & directional_atom)
{
    directional_atom_ = &directional_atom;
    third_point_      = &directional_atom.position();
}

/*Atom_group & Joint::
atom_group ()
{
    if (atoms_.is_valid())
        return atoms_();

    Arbitrary_atom_group * group = new Arbitrary_atom_group;
    atoms_.adopt (group);

    for (Molecular_width_iterator it (terminal_atom ());
         it.is_valid();
         it.next())
    {
        group->add (it.current());
    }

    return atoms_();
}//*/

void Joint::
get_atom_group (Expanded_atom_group & group)
{
    int i;

    if (atoms_.is_valid())
    {
        for (i=0;  i<atoms_().atom_count();  ++i)
            group.add (atoms_().atom(i));

        return;
    }

    for (Molecular_width_iterator it (terminal_atom ());
         it.is_valid();
         it.next())
    {
        group.add (it.current());
    }
}

void Joint::
chain (Joint & second_joint, double distance)
{
    //fix preconditions

    // f3
    //  |
    // f2 - f1
    // s1 - s2
    //       |
    //      s3

    int i;
    Arbitrary_atom_group group;
    second_joint.get_atom_group (group);
    int          atom_count = group.atom_count();

    Point_3D const& f1 = *first_point_;
    Point_3D const& f2 = *second_point_;
    Point_3D const& f3 = *third_point_;
    Point_3D const& s1 = second_joint.first_point ();//fix const
    Point_3D const& s2 = second_joint.second_point ();
    Point_3D const& s3 = second_joint.third_point ();

    // Put in the same plane
    Vector_3D_impl  f2_f3 (0,0,0);  f3.difference (f2, &f2_f3);
    Vector_3D_impl  f2_f1 (0,0,0);  f1.difference (f2, &f2_f1);
    Vector_3D_impl  s2_s3 (0,0,0);  s3.difference (s2, &s2_s3);
    Vector_3D_impl  s2_s1 (0,0,0);  s1.difference (s2, &s2_s1);
    Vector_3D_impl  fn    (0,0,0);  f2_f1.cross (f2_f3, &fn);
    Vector_3D_impl  sn    (0,0,0);  s2_s1.cross (s2_s3, &sn);
    Vector_3D_impl  n     (0,0,0);  sn.norm().cross (fn.norm(), &n);

    if (n.length() > d_mach_eps_2)
    {
        double angle = asin (n.length());

        if (sn.dot(fn) < 0.)
            angle = __Pi__ - angle;

        for (i=0;  i<atom_count;  ++i)
            group.atom(i).rotate (n, s2, angle);
    }
    else 
    {
        Vector_3D_impl tmp (fn);
        tmp += sn;
        
        // fn and sn have the opposite direction
        if (tmp.length() < d_mach_eps_2)
        {
            for (i=0;  i<atom_count;  ++i)
                group.atom(i).rotate (s2_s3, s2, __Pi__);
        }
    }


    // Setup orientation in plane
    Vector_3D_impl  f1_f2 (f2_f1);  f1_f2.negate().norm();
    s1.difference (s2, &s2_s1);
    s2_s1.norm().cross (f1_f2, &n);

    if (n.length() > d_mach_eps_2)
    {
        double angle = asin (n.length());

        if (s2_s1.dot(f1_f2) < 0.)
            angle = __Pi__ - angle;

        for (i=0;  i<atom_count;  ++i)
            group.atom(i).rotate (n, s2, angle);
    }
    else 
    {
        Vector_3D_impl tmp (s2_s1);
        tmp += f1_f2;
        
        // f2_f1 and s2_s1 have the same direction
        if (tmp.length() < d_mach_eps_2)
        {
            s3.difference (s2, &s2_s3);
            s1.difference (s2, &s2_s1);
            s2_s1.cross (s2_s3, &sn);

            for (i=0;  i<atom_count;  ++i)
                group.atom(i).rotate (sn, s2, __Pi__);
        }
    }

    // Set up distance
    Vector_3D_impl  s2_f2 (0,0,0);
    f2.difference (s2, &s2_f2);
    
    Vector_3D_impl movement (f2_f1);
    movement.norm ();
    movement *= distance;
    movement += s2_f2;

    for (i=0;  i<atom_count;  ++i)
        group.atom(i).move (movement);


    return;
/*
    ///////////////// Move

    //fix to Point_3D
    //Vector_3D_impl movement = *stub_atom_;  // v1
    Vector_3D_impl movement (first_point_.x(),
                             first_point_.y(), 
                             first_point_.z());   // v1

    Vector_3D_impl terminal (second_point_.x(),
                             second_point_.y(),
                             second_point_.z()); //v2

    Vector_3D_impl terminal_stroke (second_joint.second_point().x(),
                                    second_joint.second_point().y(),
                                    second_joint.second_point().z());

    movement -= terminal;            // v21

    Vector_3D_impl v21 = movement;

    // target_point = norm(v21) * distance + v2
    movement.norm();
    movement *= distance;
    movement += terminal;

    Vector_3D_impl o = movement;

    // v2stroke_target
    movement -= terminal_stroke;

    for (i=0;  i<second_joint.atom_group().atom_count();  ++i)
        //second_joint.atom_group().atom (i) += movement;
        second_joint.atom_group().atom (i).move (movement);


    ///////////////// Rotate
    // 
    // r' = nrn + cos theta (r - nrn) + sin theta (n x r)

    Vector_3D_impl v12_stroke (second_joint.first_point().x(),
                               second_joint.first_point().y(),
                               second_joint.first_point().z());

    v12_stroke -= terminal_stroke;
    v12_stroke.norm();
    v21.norm();

    Vector_3D_impl n = v12_stroke;
    n.cross (v21);

    double sin_theta = n.length ();
    double cos_theta = sqrt (1 - sin_theta*sin_theta);
    n.norm();
    
    for (i=0;  i<second_joint.atom_group().atom_count();  ++i)
    {
        Vector_3D_impl r (second_joint.atom_group().atom (i).x(),
                          second_joint.atom_group().atom (i).y(),
                          second_joint.atom_group().atom (i).z());

        r -= o;

        //double nr = n.dot_product (r);
        double nr = r.dot (n);
        Vector_3D_impl nrn = n;
        nrn *= nr;

        Vector_3D_impl cos_theta_nrn = r;
        cos_theta_nrn -= nrn;
        cos_theta_nrn *= cos_theta;

        Vector_3D_impl sin_theta_nxr = n;
        sin_theta_nxr.cross (r);
        sin_theta_nxr *= sin_theta;
    
        Vector_3D_impl r_stroke = nrn;
        r_stroke += cos_theta_nrn;
        r_stroke += sin_theta_nxr;

        r_stroke += o;

        //fix second_joint.atom_group ().atom (i) set_position (r_stroke); 
        second_joint.atom_group ().atom (i). set_x (r_stroke.x()); 
        second_joint.atom_group ().atom (i). set_y (r_stroke.y()); 
        second_joint.atom_group ().atom (i). set_z (r_stroke.z()); 
    }//*/
}

void Joint::
//overlay (Joint & second_joint) 
overlay (Joint & ) 
{
    FIX;
}

}//MM


