/****************************************************************************
 * Copyright (C) from 2009 to Present EPAM Systems.
 *
 * This file is part of Indigo toolkit.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ***************************************************************************/

#include "molecule/molecule_3d_constraints.h"

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

using namespace indigo;

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

Molecule3dConstraints::Molecule3dConstraints(QueryMolecule& mol) : _mol(mol)
{
}

void Molecule3dConstraints::init()
{
    for (int i : _mol.vertices())
    {
        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();
}
