/****************************************************************************
 * Copyright (C) 2009-2015 EPAM Systems
 * 
 * This file is part of Indigo toolkit.
 * 
 * This file may be distributed and/or modified under the terms of the
 * GNU General Public License version 3 as published by the Free Software
 * Foundation and appearing in the file LICENSE.GPL included in the
 * packaging of this file.
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 ***************************************************************************/

#include "base_cpp/red_black.h"
#include "molecule/molecule_3d_constraints.h"
#include "molecule/query_molecule.h"

using namespace indigo;

QueryMolecule & Molecule3dConstraints::_getMolecule ()
{
   char dummy[sizeof(QueryMolecule)];
   int offset = (int)((char *)(&((QueryMolecule *)dummy)->spatial_constraints) - dummy);

   return *(QueryMolecule *)((char *)this - offset);
}

IMPL_ERROR(Molecule3dConstraints, "molecule 3d constraints");

Molecule3dConstraints::Molecule3dConstraints ()
{
}

void Molecule3dConstraints::init ()
{
   QueryMolecule &mol = _getMolecule();

   int i;

   for (i = mol.vertexBegin(); i != mol.vertexEnd(); i = mol.vertexNext(i))
   {
      AutoPtr<PointByAtom> constr;

      constr.create();
      constr->atom_idx = i;
      _constraints.add(constr.release());
   }
}

int Molecule3dConstraints::begin () const
{
   return 0;
}

int Molecule3dConstraints::end () const
{
   return _constraints.size();
}

int Molecule3dConstraints::next (int idx) const
{
   return idx + 1;
}

const Molecule3dConstraints::Base & Molecule3dConstraints::at (int idx) const
{
   return *_constraints[idx];
}

Molecule3dConstraints::Base & Molecule3dConstraints::add (Molecule3dConstraints::Base *constraint)
{
   return _constraints.add(constraint);
}

bool Molecule3dConstraints::haveConstraints ()
{
   for (int i = 0; i < _constraints.size(); i++)
   {
      const Base & base = *_constraints.at(i);

      switch (base.type)
      {
         case ANGLE_2LINES:
         case ANGLE_DIHEDRAL:
         case ANGLE_2PLANES:
         case ANGLE_3POINTS:
         case DISTANCE_2POINTS:
         case DISTANCE_POINT_LINE:
         case DISTANCE_POINT_PLANE:
         case EXCLUSION_SPHERE:
         case LINE_BEST_FIT:
         case PLANE_BEST_FIT:
            return true;
      }
   }

   return false;
}

void Molecule3dConstraints::_buildSub (PtrArray<Base> &sub, const PtrArray<Base> &super,
                                       const int *mapping)
{
   QS_DEF(Array<int>, cmapping); // mapping of constraints from supermolecule to submolecule
   int i, j;

   cmapping.resize(super.size());
   for (i = 0; i < super.size(); i++)
      cmapping[i] = -1;

   sub.clear();

   do
   {
      for (i = 0; i < super.size(); i++)
      {
         int oldsize = sub.size();

         if (cmapping[i] >= 0)
            continue;

         const Base & base = *super.at(i);

         switch (base.type)
         {
            case POINT_ATOM:
            {
               int atom_idx = ((const Molecule3dConstraints::PointByAtom &)base).atom_idx;

               if (mapping[atom_idx] < 0)
                  continue;

               AutoPtr<PointByAtom> newconstr;

               newconstr.create();
               newconstr->atom_idx = mapping[atom_idx];

               sub.add(newconstr.release());
               break;
            }
            case POINT_DISTANCE:
            {
               const PointByDistance &constr = (const PointByDistance &)base;

               int beg_id = cmapping[constr.beg_id];
               int end_id = cmapping[constr.end_id];

               if (beg_id < 0 || end_id < 0)
                  continue;

               AutoPtr<PointByDistance> newconstr;

               newconstr.create();
               newconstr->beg_id = beg_id;
               newconstr->end_id = end_id;
               newconstr->distance = constr.distance;

               sub.add(newconstr.release());
               break;
            }
            case POINT_PERCENTAGE:
            {
               const PointByPercentage &constr = (const PointByPercentage &)base;

               int beg_id = cmapping[constr.beg_id];
               int end_id = cmapping[constr.end_id];

               if (beg_id < 0 || end_id < 0)
                  continue;

               AutoPtr<PointByPercentage> newconstr;

               newconstr.create();
               newconstr->beg_id = beg_id;
               newconstr->end_id = end_id;
               newconstr->percentage = constr.percentage;
               sub.add(newconstr.release());
               break;
            }
            case POINT_NORMALE:
            {
               const PointByNormale &constr = (const PointByNormale &)base;

               int org_id = cmapping[constr.org_id];
               int norm_id = cmapping[constr.norm_id];

               if (org_id < 0 || norm_id < 0)
                  continue;

               AutoPtr<PointByNormale> newconstr;

               newconstr.create();
               newconstr->norm_id = norm_id;
               newconstr->org_id = org_id;
               newconstr->distance = constr.distance;
               sub.add(newconstr.release());
               break;
            }
            case POINT_CENTROID:
            {
               const Centroid &constr = (const Centroid &)base;

               AutoPtr<Centroid> newconstr;
               newconstr.create();

               for (j = 0; j < constr.point_ids.size(); j++)
               {
                  int pt_idx = cmapping[constr.point_ids[j]];

                  if (pt_idx < 0)
                     break;

                  newconstr->point_ids.push(pt_idx);
               }

               if (newconstr->point_ids.size() < constr.point_ids.size())
                  continue;

               sub.add(newconstr.release());
               break;
            }
            case LINE_NORMALE:
            {
               const Normale &constr = (const Normale &)base;

               int plane_id = cmapping[constr.plane_id];
               int point_id = cmapping[constr.point_id];

               if (plane_id < 0 || point_id < 0)
                  continue;

               AutoPtr<Normale> newconstr;

               newconstr.create();
               newconstr->plane_id = plane_id;
               newconstr->point_id = point_id;

               sub.add(newconstr.release());
               break;
            }
            case LINE_BEST_FIT:
            {
               const BestFitLine &constr = (const BestFitLine &)base;

               AutoPtr<BestFitLine> newconstr;
               newconstr.create();

               for (j = 0; j < constr.point_ids.size(); j++)
               {
                  int pt_idx = cmapping[constr.point_ids[j]];

                  if (pt_idx < 0)
                     break;

                  newconstr->point_ids.push(pt_idx);
               }

               if (newconstr->point_ids.size() < constr.point_ids.size())
                  continue;

               newconstr->max_deviation = constr.max_deviation;
               sub.add(newconstr.release());
               break;
            }
            case PLANE_BEST_FIT:
            {
               const BestFitPlane &constr = (const BestFitPlane &)base;

               AutoPtr<BestFitPlane> newconstr;
               newconstr.create();

               for (j = 0; j < constr.point_ids.size(); j++)
               {
                  int pt_idx = cmapping[constr.point_ids[j]];

                  if (pt_idx < 0)
                     break;

                  newconstr->point_ids.push(pt_idx);
               }

               if (newconstr->point_ids.size() < constr.point_ids.size())
                  continue;

               newconstr->max_deviation = constr.max_deviation;
               sub.add(newconstr.release());
               break;
            }
            case PLANE_POINT_LINE:
            {
               const PlaneByPoint &constr = (const PlaneByPoint &)base;

               int point_id = cmapping[constr.point_id];
               int line_id = cmapping[constr.line_id];

               if (line_id < 0 || point_id < 0)
                  continue;

               AutoPtr<PlaneByPoint> newconstr;

               newconstr.create();
               newconstr->line_id = line_id;
               newconstr->point_id = point_id;
               sub.add(newconstr.release());
               break;
            }
            case ANGLE_3POINTS:
            {
               const AngleByPoints &constr = (const AngleByPoints &)base;

               int point1_id = cmapping[constr.point1_id];
               int point2_id = cmapping[constr.point2_id];
               int point3_id = cmapping[constr.point3_id];

               if (point1_id < 0 || point2_id < 0 || point3_id < 0)
                  continue;

               AutoPtr<AngleByPoints> newconstr;

               newconstr.create();
               newconstr->point1_id = point1_id;
               newconstr->point2_id = point2_id;
               newconstr->point3_id = point3_id;

               newconstr->bottom = constr.bottom;
               newconstr->top = constr.top;
               sub.add(newconstr.release());
               break;
            }
            case ANGLE_2LINES:
            {
               const AngleByLines &constr = (const AngleByLines &)base;

               int line1_id = cmapping[constr.line1_id];
               int line2_id = cmapping[constr.line2_id];

               if (line1_id < 0 || line2_id < 0)
                  continue;

               AutoPtr<AngleByLines> newconstr;

               newconstr.create();
               newconstr->line1_id = line1_id;
               newconstr->line2_id = line2_id;

               newconstr->bottom = constr.bottom;
               newconstr->top = constr.top;
               sub.add(newconstr.release());
               break;
            }
            case ANGLE_2PLANES:
            {
               const AngleByPlanes &constr = (const AngleByPlanes &)base;

               int plane1_id = cmapping[constr.plane1_id];
               int plane2_id = cmapping[constr.plane2_id];

               if (plane1_id < 0 || plane2_id < 0)
                  continue;

               AutoPtr<AngleByPlanes> newconstr;

               newconstr.create();
               newconstr->plane1_id = plane1_id;
               newconstr->plane2_id = plane2_id;

               newconstr->bottom = constr.bottom;
               newconstr->top = constr.top;
               sub.add(newconstr.release());
               break;
            }
            case ANGLE_DIHEDRAL:
            {
               const AngleDihedral &constr = (const AngleDihedral &)base;

               int point1_id = cmapping[constr.point1_id];
               int point2_id = cmapping[constr.point2_id];
               int point3_id = cmapping[constr.point3_id];
               int point4_id = cmapping[constr.point4_id];

               if (point1_id < 0 || point2_id < 0 || point3_id < 0 || point4_id < 0)
                  continue;

               AutoPtr<AngleDihedral> newconstr;

               newconstr.create();
               newconstr->point1_id = point1_id;
               newconstr->point2_id = point2_id;
               newconstr->point3_id = point3_id;
               newconstr->point4_id = point4_id;

               newconstr->bottom = constr.bottom;
               newconstr->top = constr.top;
               sub.add(newconstr.release());
               break;
            }
            case DISTANCE_2POINTS:
            {
               const DistanceByPoints &constr = (const DistanceByPoints &)base;

               int beg_id = cmapping[constr.beg_id];
               int end_id = cmapping[constr.end_id];

               if (beg_id < 0 || end_id < 0)
                  continue;

               AutoPtr<DistanceByPoints> newconstr;

               newconstr.create();
               newconstr->beg_id = beg_id;
               newconstr->end_id = end_id;
               newconstr->bottom = constr.bottom;
               newconstr->top = constr.top;
               sub.add(newconstr.release());
               break;
            }
            case DISTANCE_POINT_LINE:
            {
               const DistanceByLine &constr = (const DistanceByLine &)base;

               int point_id = cmapping[constr.point_id];
               int line_id = cmapping[constr.line_id];

               if (line_id < 0 || point_id < 0)
                  continue;

               AutoPtr<DistanceByLine> newconstr;

               newconstr.create();
               newconstr->line_id = line_id;
               newconstr->point_id = point_id;
               newconstr->top = constr.top;
               newconstr->bottom = constr.bottom;
               sub.add(newconstr.release());
               break;
            }
            case DISTANCE_POINT_PLANE:
            {
               const DistanceByPlane &constr = (const DistanceByPlane &)base;

               int plane_id = cmapping[constr.plane_id];
               int point_id = cmapping[constr.point_id];

               if (plane_id < 0 || point_id < 0)
                  continue;

               AutoPtr<DistanceByPlane> newconstr;

               newconstr.create();
               newconstr->plane_id = plane_id;
               newconstr->point_id = point_id;
               newconstr->bottom = constr.bottom;
               newconstr->top = constr.top;
               sub.add(newconstr.release());
               break;
            }
            case EXCLUSION_SPHERE:
            {
               const ExclusionSphere &constr = (const ExclusionSphere &)base;

               int center_id = cmapping[constr.center_id];

               if (center_id < 0)
                  continue;

               AutoPtr<ExclusionSphere> newconstr;

               newconstr.create();

               for (j = 0; j < constr.allowed_atoms.size(); j++)
               {
                  int atom_idx = mapping[constr.allowed_atoms[j]];

                  if (atom_idx >= 0)
                     newconstr->allowed_atoms.push(atom_idx);
               }

               newconstr->center_id = center_id;
               newconstr->allow_unconnected = constr.allow_unconnected;
               newconstr->radius = constr.radius;
               sub.add(newconstr.release());
               break;
            }
            default:
               throw Error("build on submolecule: unknown feature %d", base.type);
         }
         cmapping[i] = oldsize;
      }
      if (i == super.size())
         break;
   } while (1);
}

void Molecule3dConstraints::buildOnSubmolecule (const Molecule3dConstraints &super, const int *mapping)
{
   _buildSub(_constraints, super._constraints, mapping);
}

void Molecule3dConstraints::removeAtoms (const int *mapping)
{
   PtrArray<Base> new_constraints;
   int i;

   _buildSub(new_constraints, _constraints, mapping);

   _constraints.clear();

   for (i = 0; i < new_constraints.size(); i++)
   {
      _constraints.add(new_constraints.at(i));
      new_constraints.release(i);
   }
}

IMPL_ERROR(Molecule3dConstraintsChecker, "molecule 3d constraints checker");

CP_DEF(Molecule3dConstraintsChecker);

Molecule3dConstraintsChecker::Molecule3dConstraintsChecker (const Molecule3dConstraints &constraints) :
_constraints(constraints),
CP_INIT,
TL_CP_GET(_cache_v),
TL_CP_GET(_cache_l),
TL_CP_GET(_cache_p),
TL_CP_GET(_cache_mark)
{
}

bool Molecule3dConstraintsChecker::check (BaseMolecule &target, const int *mapping)
{
   _cache_l.clear();
   _cache_p.clear();
   _cache_v.clear();
   _cache_mark.clear();

   _target = &target;
   _mapping = mapping;

   for (int i = _constraints.begin(); i != _constraints.end(); i = _constraints.next(i))
   {
      const MC::Base & base = _constraints.at(i);

      switch (base.type)
      {
         case MC::ANGLE_DIHEDRAL:
         case MC::ANGLE_3POINTS:
         {
            float value = _getAngle(i);
            const MC::AngleBase &constr = (const MC::AngleBase &)base;

            if (value < constr.bottom || value > constr.top)
               return false;
            break;
         }
         case MC::ANGLE_2LINES:
         case MC::ANGLE_2PLANES:
         {
            float value = _getAngle(i);
            const MC::AngleBase &constr = (const MC::AngleBase &)base;

            if ((value < constr.bottom || value > constr.top) &&
                (M_PI - value < constr.bottom || M_PI - value > constr.top))
               return false;
            break;
         }
         case MC::DISTANCE_2POINTS:
         case MC::DISTANCE_POINT_LINE:
         case MC::DISTANCE_POINT_PLANE:
         {
            float value = _getDistance(i);
            const MC::DistanceBase &constr = (const MC::DistanceBase &)base;

            if (value < constr.bottom || value > constr.top)
               return false;
            break;
         }
         case MC::EXCLUSION_SPHERE:
         {
            const MC::ExclusionSphere &constr = (const MC::ExclusionSphere &)base;

            _cache(constr.center_id);

            const Vec3f &center = _cache_v.at(constr.center_id);
            QS_DEF(Array<int>, allowed);
            int i;

            allowed.clear_resize(_target->vertexCount());
            allowed.zerofill();

            for (i = 0; i < constr.allowed_atoms.size(); i++)
               allowed[_mapping[constr.allowed_atoms[i]]]  = 1;

            for (i = _target->vertexBegin(); i < _target->vertexEnd();
                 i = _target->vertexNext(i))
            {
               if (allowed[i])
                  continue;

               if (constr.allow_unconnected && _target->getVertex(i).degree() < 1)
                  continue;

               const Vec3f &pos = _target->getAtomXyz(i);

               if (Vec3f::dist(pos, center) < constr.radius - EPSILON)
                  return false;
            }

            break;
         }
         case MC::LINE_BEST_FIT:
         {
            _cache(i);

            const Line3f &bfl = _cache_l.at(i);
            const MC::BestFitLine &constr = (const MC::BestFitLine &)base;
            float rms = .0f, dist = .0f;

            for (int i = 0; i < constr.point_ids.size(); i++)
            {
               dist = bfl.distFromPoint(_cache_v.at(constr.point_ids[i]));
               rms += dist * dist;
            }
            if (rms > constr.max_deviation + 1e-6)
               return false;

            break;
         }
         case MC::PLANE_BEST_FIT:
         {
            _cache(i);

            const Plane3f &bfp = _cache_p.at(i);
            const MC::BestFitPlane &constr = (const MC::BestFitPlane &)base;
            float rms = .0f, dist = .0f;

            for (int i = 0; i < constr.point_ids.size(); i++)
            {
               dist = bfp.distFromPoint(_cache_v.at(constr.point_ids[i]));
               rms += dist * dist;
            }
            if (rms > constr.max_deviation + 1e-6)
               return false;

            break;
         }
      }
   }
   return true;
}

void Molecule3dConstraintsChecker::_cache (int idx)
{
   if (_cache_v.find(idx) || _cache_l.find(idx) || _cache_p.find(idx))
      return;

   const MC::Base &base = _constraints.at(idx);

   switch (base.type)
   {
      case MC::POINT_ATOM:
      {
         int atom_idx = ((const Molecule3dConstraints::PointByAtom &)base).atom_idx;

         _cache_v.insert(idx, _target->getAtomXyz(_mapping[atom_idx]));
         break;
      }
      case MC::POINT_DISTANCE:
      {
         const MC::PointByDistance &constr = (const MC::PointByDistance &)base;

         _cache(constr.beg_id);
         _cache(constr.end_id);

         const Vec3f &beg = _cache_v.at(constr.beg_id);
         const Vec3f &end = _cache_v.at(constr.end_id);
         Vec3f dir;

         dir.diff(end, beg);

         if (!dir.normalize())
            throw Error("point-by-distance: degenerate case");

         Vec3f res;

         res.lineCombin(beg, dir, constr.distance);

         _cache_v.insert(idx, res);
         break;
      }
      case MC::POINT_PERCENTAGE:
      {
         const MC::PointByPercentage &constr = (const MC::PointByPercentage &)base;

         _cache(constr.beg_id);
         _cache(constr.end_id);

         const Vec3f &beg = _cache_v.at(constr.beg_id);
         const Vec3f &end = _cache_v.at(constr.end_id);
         Vec3f dir;

         dir.diff(end, beg);

         if (!dir.normalize())
            throw Error("point-by-percentage: degenerate case");

         Vec3f res;

         res.lineCombin2(beg, 1.f - constr.percentage, end, constr.percentage);

         _cache_v.insert(idx, res);
         break;
      }
      case MC::POINT_NORMALE:
      {
         const MC::PointByNormale &constr = (const MC::PointByNormale &)base;

         _cache(constr.org_id);
         _cache(constr.norm_id);

         const Vec3f &org = _cache_v.at(constr.org_id);
         const Line3f &norm = _cache_l.at(constr.norm_id);

         Vec3f res;

         res.lineCombin(org, norm.dir, constr.distance);
         _cache_v.insert(idx, res);
         break;
      }
      case MC::POINT_CENTROID:
      {
         const MC::Centroid &constr = (const MC::Centroid &)base;

         Vec3f res;

         if (constr.point_ids.size() < 1)
            throw Error("centroid: have %d points", constr.point_ids.size());

         for (int i = 0; i < constr.point_ids.size(); i++)
         {
            _cache(constr.point_ids[i]);

            const Vec3f &pt = _cache_v.at(constr.point_ids[i]);

            res.add(pt);
         }
         
         res.scale(1.f / constr.point_ids.size());
         _cache_v.insert(idx, res);
         break;
      }
      case MC::LINE_NORMALE:
      {
         const MC::Normale &constr = (const MC::Normale &)base;

         _cache(constr.plane_id);
         _cache(constr.point_id);

         const Plane3f &plane = _cache_p.at(constr.plane_id);
         const Vec3f &point = _cache_v.at(constr.point_id);

         Vec3f projection;
         Line3f res;

         plane.projection(point, projection);

         res.dir.copy(plane.getNorm());
         res.org.copy(projection);

         _cache_l.insert(idx, res);
         break;
      }
      case MC::LINE_BEST_FIT:
      {
         const MC::BestFitLine &constr = (const MC::BestFitLine &)base;

         if (constr.point_ids.size() < 2)
            throw Error("best fit line: only %d points", constr.point_ids.size());

         QS_DEF(Array<Vec3f>, points);

         points.clear();
         for (int i = 0; i < constr.point_ids.size(); i++)
         {
            _cache(constr.point_ids[i]);
            points.push(_cache_v.at(constr.point_ids[i]));
         }

         Line3f res;

         res.bestFit(points.size(), points.ptr(), 0);

         _cache_l.insert(idx, res);
         break;
      }
      case MC::PLANE_BEST_FIT:
      {
         const MC::BestFitPlane &constr = (const MC::BestFitPlane &)base;

         if (constr.point_ids.size() < 3)
            throw Error("best fit line: only %d points", constr.point_ids.size());

         QS_DEF(Array<Vec3f>, points);

         points.clear();
         for (int i = 0; i < constr.point_ids.size(); i++)
         {
            _cache(constr.point_ids[i]);
            points.push(_cache_v.at(constr.point_ids[i]));
         }

         Plane3f res;

         res.bestFit(points.size(), points.ptr(), 0);

         _cache_p.insert(idx, res);
         break;
      }
      case MC::PLANE_POINT_LINE:
      {
         const MC::PlaneByPoint &constr = (const MC::PlaneByPoint &)base;

         _cache(constr.point_id);
         _cache(constr.line_id);

         const Vec3f &point = _cache_v.at(constr.point_id);
         const Line3f &line = _cache_l.at(constr.line_id);

         Plane3f res;

         res.byPointAndLine(point, line);

         _cache_p.insert(idx, res);
         break;
      }
      default:
         throw Error("unknown constraint type %d", base.type);
   }
}

float Molecule3dConstraintsChecker::_getAngle (int idx)
{
   const MC::Base &base = _constraints.at(idx);

   switch (base.type)
   {
      case MC::ANGLE_3POINTS:
      {
         const MC::AngleByPoints &constr = (const MC::AngleByPoints &)base;

         _cache(constr.point1_id);
         _cache(constr.point2_id);
         _cache(constr.point3_id);

         const Vec3f &v1 = _cache_v.at(constr.point1_id);
         const Vec3f &v2 = _cache_v.at(constr.point2_id);
         const Vec3f &v3 = _cache_v.at(constr.point3_id);

         Vec3f dir1, dir3;

         dir1.diff(v1, v2);
         dir3.diff(v3, v2);

         float ang;

         if (!Vec3f::angle(dir1, dir3, ang))
            throw Error("angle by points: degerenate");

         return ang;
      }
      case MC::ANGLE_2LINES:
      {
         const MC::AngleByLines &constr = (const MC::AngleByLines &)base;

         _cache(constr.line1_id);
         _cache(constr.line2_id);

         const Line3f &line1 = _cache_l.at(constr.line1_id);
         const Line3f &line2 = _cache_l.at(constr.line2_id);

         float ang;

         if (!Vec3f::angle(line1.dir, line2.dir, ang))
            throw Error("angle by lines: degerenate");

         return ang;
      }
      case MC::ANGLE_2PLANES:
      {
         const MC::AngleByPlanes &constr = (const MC::AngleByPlanes &)base;

         _cache(constr.plane1_id);
         _cache(constr.plane2_id);

         const Plane3f &plane1 = _cache_p.at(constr.plane1_id);
         const Plane3f &plane2 = _cache_p.at(constr.plane2_id);

         float ang;

         if (!Vec3f::angle(plane1.getNorm(), plane2.getNorm(), ang))
            throw Error("angle by planes: degerenate");

         return ang;
      }
      case MC::ANGLE_DIHEDRAL:
      {
         const MC::AngleDihedral &constr = (const MC::AngleDihedral &)base;

         _cache(constr.point1_id);
         _cache(constr.point2_id);
         _cache(constr.point3_id);
         _cache(constr.point4_id);

         const Vec3f &v1 = _cache_v.at(constr.point1_id);
         const Vec3f &v2 = _cache_v.at(constr.point2_id);
         const Vec3f &v3 = _cache_v.at(constr.point3_id);
         const Vec3f &v4 = _cache_v.at(constr.point4_id);

         Vec3f d1, d2, axis;

         d1.diff(v2, v1);
         d2.diff(v3, v4);
         axis.diff(v2, v3);

         if (!axis.normalize())
            throw Error("dihedral angle: degenerate axis");

         d1.addScaled(axis, -Vec3f::dot(d1, axis));
         d2.addScaled(axis, -Vec3f::dot(d2, axis));

         float ang;

         if (!Vec3f::angle(d1, d2, ang))
            throw Error("dihedral angle: degenerate");

         return ang;
      }
      default:
         throw Error("get angle: bad constraint type %d", base.type);
   }
}

float Molecule3dConstraintsChecker::_getDistance (int idx)
{
   const MC::Base &base = _constraints.at(idx);

   switch (base.type)
   {
      case MC::DISTANCE_2POINTS:
      {
         const MC::DistanceByPoints &constr = (const MC::DistanceByPoints &)base;

         _cache(constr.beg_id);
         _cache(constr.end_id);
         
         const Vec3f &beg = _cache_v.at(constr.beg_id);
         const Vec3f &end = _cache_v.at(constr.end_id);

         return Vec3f::dist(beg, end);
      }
      case MC::DISTANCE_POINT_LINE:
      {
         const MC::DistanceByLine &constr = (const MC::DistanceByLine &)base;

         _cache(constr.line_id);
         _cache(constr.point_id);

         const Vec3f &point = _cache_v.at(constr.point_id);
         const Line3f &line = _cache_l.at(constr.line_id);

         return line.distFromPoint(point);
      }
      case MC::DISTANCE_POINT_PLANE:
      {
         const MC::DistanceByPlane &constr = (const MC::DistanceByPlane &)base;

         _cache(constr.plane_id);
         _cache(constr.point_id);

         const Vec3f &point = _cache_v.at(constr.point_id);
         const Plane3f &plane = _cache_p.at(constr.plane_id);

         return plane.distFromPoint(point);
      }
      default:
         throw Error("get distance: bad constraint type %d", base.type);
   }
}

void Molecule3dConstraintsChecker::markUsedAtoms (int *arr, int value)
{
   int i;

   _to_mark = arr;
   _mark_value = value;

   for (i = _constraints.begin(); i != _constraints.end(); i = _constraints.next(i))
   {
      const MC::Base & base = _constraints.at(i);

      switch (base.type)
      {
         case MC::ANGLE_2LINES:
         case MC::ANGLE_DIHEDRAL:
         case MC::ANGLE_2PLANES:
         case MC::ANGLE_3POINTS:
         case MC::DISTANCE_2POINTS:
         case MC::DISTANCE_POINT_LINE:
         case MC::DISTANCE_POINT_PLANE:
         case MC::EXCLUSION_SPHERE:
         {
            _mark(i);
            break;
         }
      }
   }
}

void Molecule3dConstraintsChecker::_mark (int idx)
{
   if (_cache_mark.find(idx))
      return;

   _cache_mark.insert(idx);

   const MC::Base &base = _constraints.at(idx);

   switch (base.type)
   {
      case MC::POINT_ATOM:
      {
         int atom_idx = ((const Molecule3dConstraints::PointByAtom &)base).atom_idx;

         _to_mark[atom_idx] = _mark_value;
         break;
      }
      case MC::POINT_DISTANCE:
      {
         const MC::PointByDistance &constr = (const MC::PointByDistance &)base;

         _mark(constr.beg_id);
         _mark(constr.end_id);
         break;
      }
      case MC::POINT_PERCENTAGE:
      {
         const MC::PointByPercentage &constr = (const MC::PointByPercentage &)base;

         _mark(constr.beg_id);
         _mark(constr.end_id);
         break;
      }
      case MC::POINT_NORMALE:
      {
         const MC::PointByNormale &constr = (const MC::PointByNormale &)base;

         _mark(constr.org_id);
         _mark(constr.norm_id);
         break;
      }
      case MC::POINT_CENTROID:
      {
         const MC::Centroid &constr = (const MC::Centroid &)base;

         for (int i = 0; i < constr.point_ids.size(); i++)
            _mark(constr.point_ids[i]);

         break;
      }
      case MC::LINE_NORMALE:
      {
         const MC::Normale &constr = (const MC::Normale &)base;

         _mark(constr.plane_id);
         _mark(constr.point_id);
         break;
      }
      case MC::LINE_BEST_FIT:
      {
         const MC::BestFitLine &constr = (const MC::BestFitLine &)base;

         for (int i = 0; i < constr.point_ids.size(); i++)
            _mark(constr.point_ids[i]);

         break;
      }
      case MC::PLANE_BEST_FIT:
      {
         const MC::BestFitPlane &constr = (const MC::BestFitPlane &)base;

         for (int i = 0; i < constr.point_ids.size(); i++)
            _mark(constr.point_ids[i]);

         break;
      }
      case MC::PLANE_POINT_LINE:
      {
         const MC::PlaneByPoint &constr = (const MC::PlaneByPoint &)base;

         _mark(constr.point_id);
         _mark(constr.line_id);

         break;
      }
      case MC::ANGLE_3POINTS:
      {
         const MC::AngleByPoints &constr = (const MC::AngleByPoints &)base;

         _mark(constr.point1_id);
         _mark(constr.point2_id);
         _mark(constr.point3_id);
         break;
      }
      case MC::ANGLE_2LINES:
      {
         const MC::AngleByLines &constr = (const MC::AngleByLines &)base;

         _mark(constr.line1_id);
         _mark(constr.line2_id);
         break;
      }
      case MC::ANGLE_2PLANES:
      {
         const MC::AngleByPlanes &constr = (const MC::AngleByPlanes &)base;

         _mark(constr.plane1_id);
         _mark(constr.plane2_id);
         break;
      }
      case MC::ANGLE_DIHEDRAL:
      {
         const MC::AngleDihedral &constr = (const MC::AngleDihedral &)base;

         _mark(constr.point1_id);
         _mark(constr.point2_id);
         _mark(constr.point3_id);
         _mark(constr.point4_id);
         break;
      }
      case MC::DISTANCE_2POINTS:
      {
         const MC::DistanceByPoints &constr = (const MC::DistanceByPoints &)base;

         _mark(constr.beg_id);
         _mark(constr.end_id);
         break;
      }
      case MC::DISTANCE_POINT_LINE:
      {
         const MC::DistanceByLine &constr = (const MC::DistanceByLine &)base;

         _mark(constr.line_id);
         _mark(constr.point_id);
         break;
      }
      case MC::DISTANCE_POINT_PLANE:
      {
         const MC::DistanceByPlane &constr = (const MC::DistanceByPlane &)base;

         _mark(constr.plane_id);
         _mark(constr.point_id);
         break;
      }
      case MC::EXCLUSION_SPHERE:
      {
         const MC::ExclusionSphere &constr = (const MC::ExclusionSphere &)base;

         _mark(constr.center_id);
         break;
      }
   }
}

void Molecule3dConstraints::clear ()
{
   _constraints.clear();
}