#include "Mass_center.h"

#include "Atom.h"
#include "Atom_group.h"
#include "Atom_kit.h"
#include "Point_3D_impl.h"
#include "Vector_3D_impl.h"

namespace MM
{

void  Mass_center::
evaluate (Point_3D  & mass_center, 
          double    * mass, 
          Vector_3D * velocity) const
{
    Point_3D_impl   zero   (0,0,0);
    Vector_3D_impl  vector (0,0,0), sum (0,0,0);
    double          sum_mass = 0.;
    int             count = group_.atom_count();

    if (velocity == 0)
        for (int i=0;  i<count;  ++i)
        {
            Atom     const & atom  = group_.atom(i);
            double   atom_mass = atom.mass();
            Point_3D const & point = atom.position();
            point.difference (zero, &vector);
            sum.add_scaled (atom_mass, vector);
            sum_mass += atom_mass;
        }
    else
    {
        velocity->clear();

        for (int i=0;  i<count;  ++i)
        {
            Atom     const & atom  = group_.atom(i);
            double   atom_mass = atom.mass();
            Point_3D const & point = atom.position();
            point.difference (zero, &vector);
            sum.add_scaled (atom_mass, vector);
            sum_mass += atom_mass;

            Vector_3D_impl atom_impulse (atom.kit().velocity());
            atom_impulse *= atom_mass;
            *velocity += atom_impulse;
        }
    }

    sum *= 1. / sum_mass;
    mass_center.set_zero();
    mass_center += sum;

    if (mass != 0)
        *mass = sum_mass;

    if (velocity != 0)
        *velocity *= 1./ sum_mass;
}

void Mass_center::
set (Point_3D const& new_center)
{
    int             count = group_.atom_count();
    Point_3D_impl   old_center (0,0,0);
    Vector_3D_impl  delta      (0,0,0);

    evaluate (old_center);
    new_center.difference (old_center, &delta);

    for (int i=0;  i<count;  ++i)
        group_.atom(i).move (delta);
}

}//MM

