/*
    openDCM, dimensional constraint manager
    Copyright (C) 2012  Stefan Troeger <stefantroeger@gmx.net>

    This library is free software; you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation; either version 2.1 of the License, or
    (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License along
    with this library; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/

#ifndef GCM_CLUSTERMATH_H
#define GCM_CLUSTERMATH_H

#include <vector>
#include <cmath>

#include <Eigen/StdVector>
#include <boost/noncopyable.hpp>
#include <opendcm/core/logging.hpp>
#include "defines.hpp"

#define MAXFAKTOR 1.2   //the maximal distance allowd by a point normed to the cluster size
#define MINFAKTOR 0.8   //the minimal distance allowd by a point normed to the cluster size
#define SKALEFAKTOR 1.  //the faktor by which the biggest size is multiplied to get the scale value
#define NQFAKTOR 0.5    //the faktor by which the norm quaternion is multiplied with to get the RealScalar
//norm quaternion to generate the unit quaternion

namespace dcm {
namespace details {

enum Scalemode {
    one,
    two,
    three,
    multiple_inrange,
    multiple_outrange
};

template<typename Sys>
struct ClusterMath {

public:
    typedef typename system_traits<Sys>::Kernel Kernel;
    typedef typename system_traits<Sys>::Cluster Cluster;
    typedef typename system_traits<Sys>::template getModule<m3d>::type module3d;
    typedef typename module3d::Geometry3D Geometry3D;
    typedef boost::shared_ptr<Geometry3D> Geom;
    typedef typename module3d::math_prop math_prop;
    typedef typename module3d::fix_prop fix_prop;

    typedef typename Kernel::number_type Scalar;

    typename Kernel::Transform3D m_transform, m_ssrTransform, m_resetTransform;
    typename Kernel::DiffTransform3D m_diffTrans;
    typename Kernel::Vector3Map	 m_normQ;
    typename Kernel::Quaternion  m_resetQuaternion;

    int m_offset;
    bool init, fix;
    std::vector<Geom> m_geometry;

    typename Kernel::Vector3Map m_translation;
    //shift scale stuff
    typename Kernel::Vector3 midpoint, m_shift, scale_dir, maxm, minm, max, fixtrans;
    Scalemode mode;
    Scalar m_scale;

    typedef std::vector<typename Kernel::Vector3, Eigen::aligned_allocator<typename Kernel::Vector3> > Vec;
    Vec m_points, m_pseudo;

#ifdef USE_LOGGING
    src::logger log;
#endif

public:
    ClusterMath();

    void setParameterOffset(int offset);
    int getParameterOffset();

    typename Kernel::Vector3Map& getNormQuaternionMap();
    typename Kernel::Vector3Map& getTranslationMap();
    void initMaps();
    void initFixMaps();

    typename Kernel::Transform3D& getTransform();
    void mapsToTransform(typename Kernel::Transform3D& trans);
    void transformToMaps(typename Kernel::Transform3D& trans);

    void finishCalculation();
    void finishFixCalculation();

    void resetClusterRotation(typename Kernel::Transform3D& trans);

    void calcDiffTransform(typename Kernel::DiffTransform3D& trans);
    void recalculate();

    void addGeometry(Geom g);
    void clearGeometry();
    std::vector<Geom>& getGeometry();

    struct map_downstream {

        details::ClusterMath<Sys>& 	m_clusterMath;
        typename Kernel::Transform3D	m_transform;
        bool m_isFixed;

        map_downstream(details::ClusterMath<Sys>& cm, bool fix);

        void operator()(Geom g);
        void operator()(boost::shared_ptr<Cluster> c);
    };

    void mapClusterDownstreamGeometry(boost::shared_ptr<Cluster> cluster);

    //Calculate the scale of the cluster. Therefore the midpoint is calculated and the scale is
    // defined as the max distance between the midpoint and the points.
    Scalar calculateClusterScale();

    void applyClusterScale(Scalar scale, bool isFixed);

private:
    Scalar calcOnePoint(const typename Kernel::Vector3& p);

    Scalar calcTwoPoints(const typename Kernel::Vector3& p1, const typename Kernel::Vector3& p2);

    Scalar calcThreePoints(const typename Kernel::Vector3& p1,
                           const typename Kernel::Vector3& p2, const typename Kernel::Vector3& p3);
};


/*****************************************************************************************************************/
/*****************************************************************************************************************/
/*****************************************************************************************************************/
/*****************************************************************************************************************/



template<typename Sys>
ClusterMath<Sys>::ClusterMath() : m_normQ(NULL), m_translation(NULL), init(false) {

    m_resetTransform = typename Kernel::Quaternion(1,1,1,1);
    m_shift.setZero();

#ifdef USE_LOGGING
    log.add_attribute("Tag", attrs::constant< std::string >("Clustermath3D"));
#endif
};

template<typename Sys>
void ClusterMath<Sys>::setParameterOffset(int offset) {
    m_offset = offset;
};

template<typename Sys>
int ClusterMath<Sys>::getParameterOffset() {
    return m_offset;
};

template<typename Sys>
typename ClusterMath<Sys>::Kernel::Vector3Map& ClusterMath<Sys>::getNormQuaternionMap() {
    return m_normQ;
};

template<typename Sys>
typename ClusterMath<Sys>::Kernel::Vector3Map& ClusterMath<Sys>::getTranslationMap() {
    return m_translation;
};

template<typename Sys>
void ClusterMath<Sys>::initMaps() {

    transformToMaps(m_transform);
    init = true;
    midpoint.setZero();
    m_shift.setZero();
    m_ssrTransform.setIdentity();
    m_diffTrans = m_transform;
    fix=false;
#ifdef USE_LOGGING
    BOOST_LOG(log) << "Init transform: "<<m_transform;
#endif
};

template<typename Sys>
void ClusterMath<Sys>::initFixMaps() {
    //when fixed no maps exist
    new(&m_translation) typename Kernel::Vector3Map(&fixtrans(0));
    m_translation = m_transform.translation().vector();
    init = true;
    midpoint.setZero();
    m_shift.setZero();
    m_ssrTransform.setIdentity();
    m_diffTrans = m_transform;
    fix=true;
#ifdef USE_LOGGING
    BOOST_LOG(log) << "Init fix transform: "<<m_transform;
#endif
};

template<typename Sys>
typename ClusterMath<Sys>::Kernel::Transform3D& ClusterMath<Sys>::getTransform() {
    return m_transform;
};

template<typename Sys>
void ClusterMath<Sys>::mapsToTransform(typename ClusterMath<Sys>::Kernel::Transform3D& trans) {
    //add scale only after possible reset
    typename Kernel::Transform3D::Scaling scale(m_transform.scaling());
    trans = m_diffTrans;
    trans *= scale;
};

template<typename Sys>
void ClusterMath<Sys>::transformToMaps(typename ClusterMath<Sys>::Kernel::Transform3D& trans) {

    const typename Kernel::Quaternion& m_quaternion = trans.rotation();
    if(m_quaternion.w() < 1.) {
        Scalar s = std::acos(m_quaternion.w())/std::sin(std::acos(m_quaternion.w()));
        m_normQ = m_quaternion.vec()*s;
        m_normQ /= NQFAKTOR;
    } else {
        m_normQ.setZero();
    }
    m_translation = trans.translation().vector();
};

template<typename Sys>
void ClusterMath<Sys>::finishCalculation() {

    mapsToTransform(m_transform);
    init=false;

#ifdef USE_LOGGING
    BOOST_LOG(log) << "Finish calculation";
#endif

    m_transform = m_ssrTransform*m_transform;

    //scale all geometries back to the original size
    m_diffTrans *= typename Kernel::Transform3D::Scaling(1./m_ssrTransform.scaling().factor());
    typedef typename std::vector<Geom>::iterator iter;
    for(iter it = m_geometry.begin(); it != m_geometry.end(); it++)
        (*it)->recalculate(m_diffTrans);
#ifdef USE_LOGGING
    BOOST_LOG(log) << "Finish transform:"<<std::endl<<m_transform;
#endif
};

template<typename Sys>
void ClusterMath<Sys>::finishFixCalculation() {
#ifdef USE_LOGGING
    BOOST_LOG(log) << "Finish fix calculation";
#endif
    typedef typename std::vector<Geom>::iterator iter;
    m_transform *= m_ssrTransform.inverse();
    typename Kernel::DiffTransform3D diff(m_transform);
    for(iter it = m_geometry.begin(); it != m_geometry.end(); it++)
        (*it)->recalculate(diff);
#ifdef USE_LOGGING
    BOOST_LOG(log) << "Finish fix transform:"<<std::endl<<m_transform;
#endif
};

template<typename Sys>
void ClusterMath<Sys>::resetClusterRotation(typename ClusterMath<Sys>::Kernel::Transform3D& trans) {

#ifdef USE_LOGGING
    BOOST_LOG(log) << "Reset cluster rotation:"<<std::endl<<m_diffTrans;
#endif

    trans  = m_resetTransform.inverse()*trans;
    m_ssrTransform *= m_resetTransform;
    m_transform = m_resetTransform.inverse()*m_transform;

    //apply the needed transformation to all geometries local values
    typedef typename std::vector<Geom>::iterator iter;
    for(iter it = m_geometry.begin(); it != m_geometry.end(); it++) {
        (*it)->transform(m_resetTransform);
    };
};

template<typename Sys>
void ClusterMath<Sys>::calcDiffTransform(typename ClusterMath<Sys>::Kernel::DiffTransform3D& trans) {

    Scalar norm = m_normQ.norm();
    trans.setIdentity();

    if(norm < 0.1) {
        if(norm == 0) {
            trans *= typename Kernel::Transform3D::Translation(m_translation);
            resetClusterRotation(trans);
        } else {
            const Scalar fac = std::sin(NQFAKTOR*norm)/norm;
            trans =  typename Kernel::Quaternion(std::cos(NQFAKTOR*norm), m_normQ(0)*fac, m_normQ(1)*fac,m_normQ(2)*fac);
            trans *= typename Kernel::Transform3D::Translation(m_translation);
            resetClusterRotation(trans);
        }
        transformToMaps(trans);
        return;
    }

    const Scalar fac = std::sin(NQFAKTOR*norm)/norm;
    trans = typename Kernel::Quaternion(std::cos(NQFAKTOR*norm), m_normQ(0)*fac, m_normQ(1)*fac, m_normQ(2)*fac);
    trans *= typename Kernel::Transform3D::Translation(m_translation);
};

template<typename Sys>
void ClusterMath<Sys>::recalculate() {


    calcDiffTransform(m_diffTrans);

    const typename Kernel::Quaternion Q = m_diffTrans.rotation();

    // now calculate the gradient quaternions and calculate the diff rotation matrices
    // m_normQ = (a,b,c)
    // n = ||m_normQ||
    //
    // Q = (a/n sin(n), b/n sin(n), c/n sin(n), cos(n))
    //

    //n=||m_normQ||, sn = sin(n)/n, sn3 = sin(n)/n^3, cn = cos(n)/n, divn = 1/n;
    const Scalar n    = m_normQ.norm();
    const Scalar sn   = std::sin(NQFAKTOR*n)/n;
    const Scalar mul  = (NQFAKTOR*std::cos(NQFAKTOR*n)-sn)/std::pow(n,2);

    //dxa = dQx/da
    const Scalar dxa = sn + std::pow(m_normQ(0),2)*mul;
    const Scalar dxb = m_normQ(0)*m_normQ(1)*mul;
    const Scalar dxc = m_normQ(0)*m_normQ(2)*mul;

    const Scalar dya = m_normQ(1)*m_normQ(0)*mul;
    const Scalar dyb = sn + std::pow(m_normQ(1),2)*mul;
    const Scalar dyc = m_normQ(1)*m_normQ(2)*mul;

    const Scalar dza = m_normQ(2)*m_normQ(0)*mul;
    const Scalar dzb = m_normQ(2)*m_normQ(1)*mul;
    const Scalar dzc = sn + std::pow(m_normQ(2),2)*mul;

    const Scalar dwa = -sn*NQFAKTOR*m_normQ(0);
    const Scalar dwb = -sn*NQFAKTOR*m_normQ(1);
    const Scalar dwc = -sn*NQFAKTOR*m_normQ(2);

    //write in the diffrot matrix, starting with dQ/dx
    m_diffTrans.at(0,0) = -4.0*(Q.y()*dya+Q.z()*dza);
    m_diffTrans.at(0,1) = -2.0*(Q.w()*dza+dwa*Q.z())+2.0*(Q.x()*dya+dxa*Q.y());
    m_diffTrans.at(0,2) = 2.0*(dwa*Q.y()+Q.w()*dya)+2.0*(dxa*Q.z()+Q.x()*dza);
    m_diffTrans.at(1,0) = 2.0*(Q.w()*dza+dwa*Q.z())+2.0*(Q.x()*dya+dxa*Q.y());
    m_diffTrans.at(1,1) = -4.0*(Q.x()*dxa+Q.z()*dza);
    m_diffTrans.at(1,2) = -2.0*(dwa*Q.x()+Q.w()*dxa)+2.0*(dya*Q.z()+Q.y()*dza);
    m_diffTrans.at(2,0) = -2.0*(dwa*Q.y()+Q.w()*dya)+2.0*(dxa*Q.z()+Q.x()*dza);
    m_diffTrans.at(2,1) = 2.0*(dwa*Q.x()+Q.w()*dxa)+2.0*(dya*Q.z()+Q.y()*dza);
    m_diffTrans.at(2,2) = -4.0*(Q.x()*dxa+Q.y()*dya);

    //dQ/dy
    m_diffTrans.at(0,3) = -4.0*(Q.y()*dyb+Q.z()*dzb);
    m_diffTrans.at(0,4) = -2.0*(Q.w()*dzb+dwb*Q.z())+2.0*(Q.x()*dyb+dxb*Q.y());
    m_diffTrans.at(0,5) = 2.0*(dwb*Q.y()+Q.w()*dyb)+2.0*(dxb*Q.z()+Q.x()*dzb);
    m_diffTrans.at(1,3) = 2.0*(Q.w()*dzb+dwb*Q.z())+2.0*(Q.x()*dyb+dxb*Q.y());
    m_diffTrans.at(1,4) = -4.0*(Q.x()*dxb+Q.z()*dzb);
    m_diffTrans.at(1,5) = -2.0*(dwb*Q.x()+Q.w()*dxb)+2.0*(dyb*Q.z()+Q.y()*dzb);
    m_diffTrans.at(2,3) = -2.0*(dwb*Q.y()+Q.w()*dyb)+2.0*(dxb*Q.z()+Q.x()*dzb);
    m_diffTrans.at(2,4) = 2.0*(dwb*Q.x()+Q.w()*dxb)+2.0*(dyb*Q.z()+Q.y()*dzb);
    m_diffTrans.at(2,5) = -4.0*(Q.x()*dxb+Q.y()*dyb);

    //dQ/dz
    m_diffTrans.at(0,6) = -4.0*(Q.y()*dyc+Q.z()*dzc);
    m_diffTrans.at(0,7) = -2.0*(Q.w()*dzc+dwc*Q.z())+2.0*(Q.x()*dyc+dxc*Q.y());
    m_diffTrans.at(0,8) = 2.0*(dwc*Q.y()+Q.w()*dyc)+2.0*(dxc*Q.z()+Q.x()*dzc);
    m_diffTrans.at(1,6) = 2.0*(Q.w()*dzc+dwc*Q.z())+2.0*(Q.x()*dyc+dxc*Q.y());
    m_diffTrans.at(1,7) = -4.0*(Q.x()*dxc+Q.z()*dzc);
    m_diffTrans.at(1,8) = -2.0*(dwc*Q.x()+Q.w()*dxc)+2.0*(dyc*Q.z()+Q.y()*dzc);
    m_diffTrans.at(2,6) = -2.0*(dwc*Q.y()+Q.w()*dyc)+2.0*(dxc*Q.z()+Q.x()*dzc);
    m_diffTrans.at(2,7) = 2.0*(dwc*Q.x()+Q.w()*dxc)+2.0*(dyc*Q.z()+Q.y()*dzc);
    m_diffTrans.at(2,8) = -4.0*(Q.x()*dxc+Q.y()*dyc);

    //recalculate all geometries
    typedef typename std::vector<Geom>::iterator iter;
    for(iter it = m_geometry.begin(); it != m_geometry.end(); it++)
        (*it)->recalculate(m_diffTrans);
};

template<typename Sys>
void ClusterMath<Sys>::addGeometry(Geom g) {
    m_geometry.push_back(g);
};

template<typename Sys>
void ClusterMath<Sys>::clearGeometry() {
    m_geometry.clear();
};

template<typename Sys>
std::vector<typename ClusterMath<Sys>::Geom>& ClusterMath<Sys>::getGeometry() {
    return m_geometry;
};

template<typename Sys>
ClusterMath<Sys>::map_downstream::map_downstream(details::ClusterMath<Sys>& cm, bool fix)
    : m_clusterMath(cm), m_isFixed(fix) {
    m_transform = m_clusterMath.getTransform();
};

template<typename Sys>
void ClusterMath<Sys>::map_downstream::operator()(Geom g) {
    //allow iteration over all maped geometries
    m_clusterMath.addGeometry(g);
    //set the offsets so that geometry knows where it is in the parameter map
    g->m_offset = m_clusterMath.getParameterOffset();
    //position and offset of the parameters must be set to the clusters values
    g->setClusterMode(true, m_isFixed);
    //calculate the appropriate local values
    typename Kernel::Transform3D trans = m_transform.inverse();
    g->transform(trans);
};

template<typename Sys>
void ClusterMath<Sys>::map_downstream::operator()(boost::shared_ptr<Cluster> c) {
    m_transform *= c->template getClusterProperty<math_prop>().getTransform();
};


template<typename Sys>
void ClusterMath<Sys>::mapClusterDownstreamGeometry(boost::shared_ptr<Cluster> cluster) {

#ifdef USE_LOGGING
    BOOST_LOG(log) << "Map downstream geometry";
#endif

    map_downstream down(cluster->template getClusterProperty<math_prop>(),
                        cluster->template getClusterProperty<fix_prop>());
    cluster->template for_each<Geometry3D>(down, true);
    //TODO: if one subcluster is fixed the hole cluster should be too, as there are no
    //	dof's remaining between parts and so nothing can be moved when one part is fixed.
};

template<typename Sys>
typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calculateClusterScale() {

#ifdef USE_LOGGING
    BOOST_LOG(log) << "Calculate cluster scale with transform scale: "<<m_transform.scaling().factor();
#endif
    //ensure the usage of the right transformation
    if(!fix)
        mapsToTransform(m_transform);
#ifdef USE_LOGGING
    BOOST_LOG(log) << "Calculate cluster scale sec transform scale: "<<m_transform.scaling().factor();
#endif

    //collect all points together
    m_points.clear();
    typename Kernel::Transform3D trans(m_transform.rotation(), m_transform.translation());
    trans.invert();
    typedef typename Vec::iterator iter;
    for(iter it = m_pseudo.begin(); it != m_pseudo.end(); it++) {
        m_points.push_back(trans*(*it));
    }
    typedef typename std::vector<Geom>::iterator g_iter;
    for(g_iter it = m_geometry.begin(); it != m_geometry.end(); it++)
        m_points.push_back((*it)->getPoint());

    //start scale calculation
    if(m_points.empty()) assert(false); //TODO: Throw
    else if(m_points.size() == 1) {
        const typename Kernel::Vector3 p = m_points[0];
        return calcOnePoint(p);
    } else if(m_points.size() == 2) {
        const typename Kernel::Vector3 p1 = m_points[0];
        const typename Kernel::Vector3 p2 = m_points[1];

        if(Kernel::isSame((p1-p2).norm(), 0.))
            return calcOnePoint(p1);

        return calcTwoPoints(p1, p2);
    } else if(m_points.size() == 3) {

        const typename Kernel::Vector3 p1 = m_points[0];
        const typename Kernel::Vector3 p2 = m_points[1];
        const typename Kernel::Vector3 p3 = m_points[2];

        const typename Kernel::Vector3 d = p2-p1;
        const typename Kernel::Vector3 e = p3-p1;

        if(Kernel::isSame(d.norm(), 0.)) {

            if(Kernel::isSame(e.norm(), 0.))
                return calcOnePoint(p1);

            return calcTwoPoints(p1, p3);
        } else if(Kernel::isSame(e.norm(), 0.)) {
            return calcTwoPoints(p1, p2);
        } else if(!Kernel::isSame((d/d.norm() - e/e.norm()).norm(), 0.) &&
                  !Kernel::isSame((d/d.norm() + e/e.norm()).norm(), 0.)) {
            return calcThreePoints(p1, p2, p3);
        }
        //three points on a line need to be treaded as multiple points
    }

    //more than 3 points dont have a exakt solution. we search for a midpoint from which all points
    //are at least MAXFAKTOR*scale away, but not closer than MINFAKTOR*scale

    //get the bonding box to get the center of points
    Scalar xmin=1e10, xmax=1e-10, ymin=1e10, ymax=1e-10, zmin=1e10, zmax=1e-10;
    for(iter it = m_points.begin(); it != m_points.end(); it++) {
        typename Kernel::Vector3 v = (*it);
        xmin = (v(0)<xmin) ? v(0) : xmin;
        xmax = (v(0)<xmax) ? xmax : v(0);
        ymin = (v(1)<ymin) ? v(1) : ymin;
        ymax = (v(1)<ymax) ? ymax : v(1);
        zmin = (v(2)<zmin) ? v(2) : zmin;
        zmax = (v(2)<zmax) ? zmax : v(2);
    };
    //now calculate the midpoint
    midpoint << xmin+xmax, ymin+ymax, zmin+zmax;
    midpoint /= 2.;


    //get the scale direction an the resulting nearest point indexes
    double xh = xmax-xmin;
    double yh = ymax-ymin;
    double zh = zmax-zmin;
    int i1, i2, i3;
    if((xh<=yh) && (xh<=zh)) {
        i1=1;
        i2=2;
        i3=0;
    } else if((yh<xh) && (yh<zh)) {
        i1=0;
        i2=2;
        i3=1;
    } else {
        i1=0;
        i2=1;
        i3=2;
    }
    scale_dir.setZero();
    scale_dir(i3) = 1;
    max = Eigen::Vector3d(xmin,ymin,zmin);
    m_scale = (midpoint-max).norm()/MAXFAKTOR;
    mode = multiple_inrange;

    maxm = max-midpoint;
    Scalar minscale = 1e10;

    //get the closest point
    for(iter it = m_points.begin(); it != m_points.end(); it++) {

        const Eigen::Vector3d point = (*it)-midpoint;
        if(point.norm()<MINFAKTOR*m_scale) {

            const double h = std::abs(point(i3)-maxm(i3));
            const double k = std::pow(MINFAKTOR/MAXFAKTOR,2);
            double q = std::pow(point(i1),2) + std::pow(point(i2),2);
            q -= (std::pow(maxm(i1),2) + std::pow(maxm(i2),2) + std::pow(h,2))*k;
            q /= 1.-k;

            const double p = h*k/(1.-k);

            if(std::pow(p,2)<q) assert(false);

            midpoint(i3) += p + std::sqrt(std::pow(p,2)-q);
            maxm = max-midpoint;
            m_scale = maxm.norm()/MAXFAKTOR;

            mode = multiple_outrange;
            minm = (*it)-midpoint;

            it = m_points.begin();
        } else if(point.norm()<minscale) {
            minscale = point.norm();
        }
    }

    if(mode==multiple_inrange) {
        //we are in the range, let's get the perfect balanced scale value
        m_scale = (minscale+maxm.norm())/2.;
    }
    return m_scale;
};

template<typename Sys>
void ClusterMath<Sys>::applyClusterScale(Scalar scale, bool isFixed) {

#ifdef USE_LOGGING
    BOOST_LOG(log) << "Apply cluster scale: "<<scale;
    BOOST_LOG(log) << "initial transform scale: "<<m_transform.scaling().factor();
#endif
    //ensure the usage of the right transform
    if(!fix)
        mapsToTransform(m_transform);



    //when fixed, the geometries never get recalculated. therefore we have to do a calculate now
    //to alow the adoption of the scale. and no shift should been set.
    if(isFixed) {
        m_transform*=typename Kernel::Transform3D::Scaling(1./(scale*SKALEFAKTOR));
        m_ssrTransform*=typename Kernel::Transform3D::Scaling(1./(scale*SKALEFAKTOR));
        typename Kernel::DiffTransform3D diff(m_transform);
        //now calculate the scaled geometrys
        typedef typename std::vector<Geom>::iterator iter;
        for(iter it = m_geometry.begin(); it != m_geometry.end(); it++) {
            (*it)->recalculate(diff);
#ifdef USE_LOGGING
            BOOST_LOG(log) << "Fixed cluster geometry value:" << (*it)->m_rotated.transpose();
#endif
        };
        return;
    }

    //if this is our scale then just applie the midpoint as shift
    if(Kernel::isSame(scale, m_scale)) {

    }
    //if only one point exists we extend the origin-point-line to match the scale
    else if(mode==details::one) {
        if(Kernel::isSame(midpoint.norm(),0))
            midpoint << scale, 0, 0;
        else midpoint += scale*scale_dir;
    }
    //two and three points form a rectangular triangle, so same procedure
    else if(mode==details::two || mode==details::three) {

        midpoint+= scale_dir*std::sqrt(std::pow(scale,2) - std::pow(m_scale,2));
    }
    //multiple points
    else if(mode==details::multiple_outrange) {

        if(scale_dir(0)) {
            Scalar d = std::pow(maxm(1),2) + std::pow(maxm(2),2);
            Scalar h = std::sqrt(std::pow(MAXFAKTOR*scale,2)-d);
            midpoint(0) += maxm(0) + h;
        } else if(scale_dir(1)) {
            Scalar d = std::pow(maxm(0),2) + std::pow(maxm(2),2);
            Scalar h = std::sqrt(std::pow(MAXFAKTOR*scale,2)-d);
            midpoint(1) += maxm(1) + h;
        } else {
            Scalar d = std::pow(maxm(0),2) + std::pow(maxm(1),2);
            Scalar h = std::sqrt(std::pow(MAXFAKTOR*scale,2)-d);
            midpoint(2) += maxm(2) + h;
        }
    } else {

        //TODO: it's possible that for this case we get too far away from the outer points.
        //	    The m_scale for "midpoint outside the bounding box" may be bigger than the
        //      scale to applie, so it results in an error.
        //get the closest point
        typedef typename Vec::iterator iter;
        for(iter it = m_points.begin(); it != m_points.end(); it++) {

            const Eigen::Vector3d point = (*it)-midpoint;
            if(point.norm()<MINFAKTOR*scale) {

                if(scale_dir(0)) {
                    Scalar d = std::pow(point(1),2) + std::pow(point(2),2);
                    Scalar h = std::sqrt(std::pow(MINFAKTOR*scale,2)-d);
                    midpoint(0) += point(0) + h;
                } else if(scale_dir(1)) {
                    Scalar d = std::pow(point(0),2) + std::pow(point(2),2);
                    Scalar h = std::sqrt(std::pow(MINFAKTOR*scale,2)-d);
                    midpoint(1) += point(1) + h;
                } else {
                    Scalar d = std::pow(point(0),2) + std::pow(point(1),2);
                    Scalar h = std::sqrt(std::pow(MINFAKTOR*scale,2)-d);
                    midpoint(2) += point(2) + h;
                }
            }
        }
    }

    typename Kernel::Transform3D ssTrans;
    ssTrans = typename Kernel::Transform3D::Translation(-midpoint);
    ssTrans *= typename Kernel::Transform3D::Scaling(1./(scale*SKALEFAKTOR));

    //recalculate all geometries
    typedef typename std::vector<Geom>::iterator iter;
    for(iter it = m_geometry.begin(); it != m_geometry.end(); it++)
        (*it)->transform(ssTrans);

    //set the new rotation and translation
    m_transform = ssTrans.inverse()*m_transform;
    m_ssrTransform *= ssTrans;

    transformToMaps(m_transform);

#ifdef USE_LOGGING
    BOOST_LOG(log) << "sstrans scale: "<<ssTrans.scaling().factor();
    BOOST_LOG(log) << "finish transform scale: "<<m_transform.scaling().factor();
#endif
};

template<typename Sys>
typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calcOnePoint(const typename ClusterMath<Sys>::Kernel::Vector3& p) {

    //one point can have every scale when moving the midpoint on the origin - point vector
    midpoint  = p;
    scale_dir = -midpoint;
    scale_dir.normalize();
    mode = details::one;
    m_scale = 0.;
    return 0.;
};

template<typename Sys>
typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calcTwoPoints(const typename ClusterMath<Sys>::Kernel::Vector3& p1,
																  const typename ClusterMath<Sys>::Kernel::Vector3& p2) {

    //two points have their minimal scale at the mid position. Scaling perpendicular to this
    //line allows arbitrary scale values. Best is to have the scale dir move towards the origin
    //as good as possible.
    midpoint  = p1+(p2-p1)/2.;
    scale_dir = (p2-p1).cross(midpoint);
    scale_dir = scale_dir.cross(p2-p1);
    if(!Kernel::isSame(scale_dir.norm(),0)) scale_dir.normalize();
    else scale_dir(0) = 1;
    mode = details::two;
    m_scale = (p2-p1).norm()/2.;
    return m_scale;
};

template<typename Sys>
typename ClusterMath<Sys>::Scalar ClusterMath<Sys>::calcThreePoints(const typename ClusterMath<Sys>::Kernel::Vector3& p1,
        const typename ClusterMath<Sys>::Kernel::Vector3& p2, const typename ClusterMath<Sys>::Kernel::Vector3& p3) {

    //Three points form a triangle with it's minimal scale at the center of it's outer circle.
    //Arbitrary scale values can be achieved by moving perpendicular to the triangle plane.
    typename Kernel::Vector3 d = p2-p1;
    typename Kernel::Vector3 e = p3-p1;

    typename Kernel::Vector3 f = p1+0.5*d;
    typename Kernel::Vector3 g = p1+0.5*e;
    scale_dir = d.cross(e);

    typename Kernel::Matrix3 m;
    m.row(0) = d.transpose();
    m.row(1) = e.transpose();
    m.row(2) = scale_dir.transpose();

    typename Kernel::Vector3 res(d.transpose()*f, e.transpose()*g, scale_dir.transpose()*p1);

    midpoint =  m.colPivHouseholderQr().solve(res);
    scale_dir.normalize();
    mode = details::three;
    m_scale = (midpoint-p1).norm();

    return m_scale;

};

}//details
}//dcm


#endif //GCM_CLUSTERMATH_H