/******************************************************************************
**
** Copyright (C) 2009-2011 Kyle Lutz <kyle.r.lutz@gmail.com>
** All rights reserved.
**
** This file is a part of the chemkit project. For more information
** see <http://www.chemkit.org>.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions
** are met:
**
**   * Redistributions of source code must retain the above copyright
**     notice, this list of conditions and the following disclaimer.
**   * Redistributions in binary form must reproduce the above copyright
**     notice, this list of conditions and the following disclaimer in the
**     documentation and/or other materials provided with the distribution.
**   * Neither the name of the chemkit project nor the names of its
**     contributors may be used to endorse or promote products derived
**     from this software without specific prior written permission.
**
** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**
******************************************************************************/

#include "mmffcalculation.h"

#include <boost/lexical_cast.hpp>

#include <chemkit/topology.h>
#include <chemkit/constants.h>
#include <chemkit/forcefield.h>
#include <chemkit/cartesiancoordinates.h>

#include "mmffparameters.h"

// === MmffCalculation ===================================================== //
MmffCalculation::MmffCalculation(int type, int atomCount, int parameterCount)
    : ForceFieldCalculation(type, atomCount, parameterCount)
{
}

// === MmffBondStrechCalculation =========================================== //
MmffBondStrechCalculation::MmffBondStrechCalculation(size_t a, size_t b)
    : MmffCalculation(BondStrech, 2, 2)
{
    setAtom(0, a);
    setAtom(1, b);
}

bool MmffBondStrechCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));
    int bondType = topology->bondedInteractionType(a, b);

    const MmffBondStrechParameters *bondStrechParameters = parameters->bondStrechParameters(bondType, typeA, typeB);
    if(bondStrechParameters){
        setParameter(0, bondStrechParameters->kb);
        setParameter(1, bondStrechParameters->r0);
        return true;
    }

    return false;
}

chemkit::Real MmffBondStrechCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real kb = parameter(0);
    chemkit::Real r0 = parameter(1);

    chemkit::Real r = coordinates->distance(a, b);
    chemkit::Real dr = r - r0;
    chemkit::Real cs = -2.0; // cubic strech constant

    // equation 2
    return 143.9325 * (kb / 2) * (dr*dr) * (1 + cs * dr + ((7.0/12.0)*(cs*cs)) * (dr*dr));
}

std::vector<chemkit::Vector3> MmffBondStrechCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real kb = parameter(0);
    chemkit::Real r0 = parameter(1);

    chemkit::Real r = coordinates->distance(a, b);
    chemkit::Real dr = r - r0;
    chemkit::Real cs = -2.0; // cubic strech constant

    // dE/dr
    chemkit::Real de_dr = 143.9325 * kb * dr * (1 + cs * dr + (7.0/12.0 * (cs*cs) * (dr*dr)) + 0.5 * dr * (cs + (14.0/12.0 * (cs*cs) * dr)));

    boost::array<chemkit::Vector3, 2> gradient = coordinates->distanceGradient(a, b);

    gradient[0] *= de_dr;
    gradient[1] *= de_dr;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === MmffAngleBendCalculation ============================================ //
MmffAngleBendCalculation::MmffAngleBendCalculation(size_t a, size_t b, size_t c)
    : MmffCalculation(AngleBend, 3, 2)
{
    setAtom(0, a);
    setAtom(1, b);
    setAtom(2, c);
}

bool MmffAngleBendCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));
    int typeC = boost::lexical_cast<int>(topology->type(c));
    int angleType = topology->angleInteractionType(a, b, c);

    const MmffAngleBendParameters *angleBendParameters =
        parameters->angleBendParameters(angleType, typeA, typeB, typeC);

    if(angleBendParameters){
        setParameter(0, angleBendParameters->ka);
        setParameter(1, angleBendParameters->theta0);
        return true;
    }

    return false;
}

chemkit::Real MmffAngleBendCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    chemkit::Real ka = parameter(0);
    chemkit::Real t0 = parameter(1);

    chemkit::Real cb = -0.007; // cubic bend constant
    chemkit::Real t = coordinates->angle(a, b, c);
    chemkit::Real dt = t - t0;

    // equation 3
    return 0.043844 * (ka / 2.0) * pow(dt, 2) * (1 + cb * dt);
}

std::vector<chemkit::Vector3> MmffAngleBendCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    chemkit::Real ka = parameter(0);
    chemkit::Real t0 = parameter(1);

    chemkit::Real cb = -0.007; // cubic bend constant
    chemkit::Real t = coordinates->angle(a, b, c);
    chemkit::Real dt = t - t0;

    // dE/dt
    chemkit::Real de_dt = 0.043844 * ka * dt * (1 + cb * dt + 0.5 * cb * dt);

    boost::array<chemkit::Vector3, 3> gradient = coordinates->angleGradient(a, b, c);

    gradient[0] *= de_dt;
    gradient[1] *= de_dt;
    gradient[2] *= de_dt;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === MmffStrechBendCalculation =========================================== //
MmffStrechBendCalculation::MmffStrechBendCalculation(size_t a, size_t b, size_t c)
    : MmffCalculation(BondStrech | AngleBend, 3, 5)
{
    setAtom(0, a);
    setAtom(1, b);
    setAtom(2, c);
}

bool MmffStrechBendCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));
    int typeC = boost::lexical_cast<int>(topology->type(c));
    int bondTypeAB = topology->bondedInteractionType(a, b);
    int bondTypeBC = topology->bondedInteractionType(b, c);
    int angleType = topology->angleInteractionType(a, b, c);

    int strechBendType =
        parameters->calculateStrechBendType(bondTypeAB, bondTypeBC, angleType);

    bool parametersSwapped = false;
    const MmffStrechBendParameters *strechBendParameters =
        parameters->strechBendParameters(strechBendType, typeA, typeB, typeC);
    if(!strechBendParameters){
        strechBendType =
            parameters->calculateStrechBendType(bondTypeBC, bondTypeAB, angleType);
        strechBendParameters =
            parameters->strechBendParameters(strechBendType, typeC, typeB, typeA);

        if(strechBendParameters){
            parametersSwapped = true;
        }
        else{
            strechBendParameters = parameters->defaultStrechBendParameters(typeA, typeB, typeC);

            if(!strechBendParameters){
                strechBendParameters = parameters->defaultStrechBendParameters(typeC, typeB, typeA);
                parametersSwapped = true;
            }
        }
    }

    const MmffBondStrechParameters *bondStrechParameters_ab =
        parameters->bondStrechParameters(bondTypeAB, typeA, typeB);
    const MmffBondStrechParameters *bondStrechParameters_bc =
        parameters->bondStrechParameters(bondTypeBC, typeB, typeC);
    const MmffAngleBendParameters *angleBendParameters =
        parameters->angleBendParameters(angleType, typeA, typeB, typeC);
    if(strechBendParameters && bondStrechParameters_ab && bondStrechParameters_bc && angleBendParameters){
        if(parametersSwapped){
            setParameter(1, strechBendParameters->kba_ijk);
            setParameter(0, strechBendParameters->kba_kji);
        }
        else{
            setParameter(0, strechBendParameters->kba_ijk);
            setParameter(1, strechBendParameters->kba_kji);
        }

        setParameter(2, bondStrechParameters_ab->r0);
        setParameter(3, bondStrechParameters_bc->r0);
        setParameter(4, angleBendParameters->theta0);
        return true;
    }

    return false;
}

chemkit::Real MmffStrechBendCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    chemkit::Real kba_ijk = parameter(0);
    chemkit::Real kba_kji = parameter(1);
    chemkit::Real r0_ab = parameter(2);
    chemkit::Real r0_bc = parameter(3);
    chemkit::Real t0 = parameter(4);

    chemkit::Real r_ab = coordinates->distance(a, b);
    chemkit::Real r_bc = coordinates->distance(b, c);
    chemkit::Real dr_ab = r_ab - r0_ab;
    chemkit::Real dr_bc = r_bc - r0_bc;
    chemkit::Real t = coordinates->angle(a, b, c);
    chemkit::Real dt = t - t0;

    // equation 5
    return 2.51210 * (kba_ijk * dr_ab + kba_kji * dr_bc) * dt;
}

std::vector<chemkit::Vector3> MmffStrechBendCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    chemkit::Real kba_ijk = parameter(0);
    chemkit::Real kba_kji = parameter(1);
    chemkit::Real r0_ab = parameter(2);
    chemkit::Real r0_bc = parameter(3);
    chemkit::Real t0 = parameter(4);

    chemkit::Real r_ab = coordinates->distance(a, b);
    chemkit::Real r_bc = coordinates->distance(b, c);
    chemkit::Real dr_ab = r_ab - r0_ab;
    chemkit::Real dr_bc = r_bc - r0_bc;
    chemkit::Real t = coordinates->angle(a, b, c);
    chemkit::Real dt = t - t0;

    std::vector<chemkit::Vector3> gradient(3);

    boost::array<chemkit::Vector3, 2> distanceGradientAB = coordinates->distanceGradient(a, b);
    boost::array<chemkit::Vector3, 2> distanceGradientBC = coordinates->distanceGradient(b, c);
    boost::array<chemkit::Vector3, 3> angleGradientABC = coordinates->angleGradient(a, b, c);

    gradient[0] = (distanceGradientAB[0] * kba_ijk * dt + angleGradientABC[0] * (kba_ijk * dr_ab + kba_kji * dr_bc)) * 2.51210;
    gradient[1] = ((distanceGradientAB[1] * kba_ijk + distanceGradientBC[0] * kba_kji) * dt + angleGradientABC[1] * (kba_ijk * dr_ab + kba_kji * dr_bc)) * 2.51210;
    gradient[2] = ((distanceGradientBC[1] * kba_kji) * dt + angleGradientABC[2] * (kba_ijk * dr_ab + kba_kji * dr_bc)) * 2.51210;

    return gradient;
}

// === MmffOutOfPlaneBendingCalculation ==================================== //
MmffOutOfPlaneBendingCalculation::MmffOutOfPlaneBendingCalculation(size_t a, size_t b, size_t c, size_t d)
    : MmffCalculation(Inversion, 4, 1)
{
    setAtom(0, a);
    setAtom(1, b);
    setAtom(2, c);
    setAtom(3, d);
}

bool MmffOutOfPlaneBendingCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));
    int typeC = boost::lexical_cast<int>(topology->type(c));
    int typeD = boost::lexical_cast<int>(topology->type(d));

    const MmffOutOfPlaneBendingParameters *outOfPlaneBendingParameters =
        parameters->outOfPlaneBendingParameters(typeA, typeB, typeC, typeD);
    if(!outOfPlaneBendingParameters){
        return false;
    }

    setParameter(0, outOfPlaneBendingParameters->koop);
    return true;
}

chemkit::Real MmffOutOfPlaneBendingCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    chemkit::Real angle = coordinates->wilsonAngle(a, b, c, d);
    chemkit::Real koop = parameter(0);

    // equation 6
    return 0.043844 * (koop / 2.0) * (angle*angle);
}

std::vector<chemkit::Vector3> MmffOutOfPlaneBendingCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    chemkit::Real angle = coordinates->wilsonAngle(a, b, c, d);
    chemkit::Real koop = parameter(0);

    // dE/dw
    chemkit::Real de_dw = 0.043844 * koop * angle;

    boost::array<chemkit::Vector3, 4> gradient = coordinates->wilsonAngleGradient(a, b, c, d);

    gradient[0] *= de_dw;
    gradient[1] *= de_dw;
    gradient[2] *= de_dw;
    gradient[3] *= de_dw;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === MmffTorsionCalculation ============================================== //
MmffTorsionCalculation::MmffTorsionCalculation(size_t a, size_t b, size_t c, size_t d)
    : MmffCalculation(Torsion, 4, 3)
{
    setAtom(0, a);
    setAtom(1, b);
    setAtom(2, c);
    setAtom(3, d);
}

bool MmffTorsionCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));
    int typeC = boost::lexical_cast<int>(topology->type(c));
    int typeD = boost::lexical_cast<int>(topology->type(d));
    int torsionType = topology->torsionInteractionType(a, b, c, d);

    const MmffTorsionParameters *torsionParameters =
        parameters->torsionParameters(torsionType, typeA, typeB, typeC, typeD);
    if(!torsionParameters){
        return false;
    }

    setParameter(0, torsionParameters->V1);
    setParameter(1, torsionParameters->V2);
    setParameter(2, torsionParameters->V3);

    return true;
}

chemkit::Real MmffTorsionCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    chemkit::Real angle = coordinates->torsionAngleRadians(a, b, c, d);
    chemkit::Real V1 = parameter(0);
    chemkit::Real V2 = parameter(1);
    chemkit::Real V3 = parameter(2);

    // equation 7
    return 0.5 * (V1 * (1.0 + cos(angle)) + V2 * (1.0 - cos(2.0 * angle)) + V3 * (1.0 + cos(3.0 * angle)));
}

std::vector<chemkit::Vector3> MmffTorsionCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    chemkit::Real phi = coordinates->torsionAngleRadians(a, b, c, d);
    chemkit::Real V1 = parameter(0);
    chemkit::Real V2 = parameter(1);
    chemkit::Real V3 = parameter(2);

    // dE/dphi
    chemkit::Real de_dphi = 0.5 * (-V1 * sin(phi) + 2 * V2 * sin(2 * phi) - 3 * V3 * sin(3 * phi));

    boost::array<chemkit::Vector3, 4> gradient = coordinates->torsionAngleGradientRadians(a, b, c, d);

    gradient[0] *= de_dphi;
    gradient[1] *= de_dphi;
    gradient[2] *= de_dphi;
    gradient[3] *= de_dphi;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === MmffVanDerWaalsCalculation ========================================== //
MmffVanDerWaalsCalculation::MmffVanDerWaalsCalculation(size_t a, size_t b)
    : MmffCalculation(VanDerWaals, 2, 2)
{
    setAtom(0, a);
    setAtom(1, b);
}

bool MmffVanDerWaalsCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));

    const MmffVanDerWaalsParameters *parametersA = parameters->vanDerWaalsParameters(typeA);
    const MmffVanDerWaalsParameters *parametersB = parameters->vanDerWaalsParameters(typeB);
    if(!parametersA || !parametersB){
        return false;
    }

    chemkit::Real N_a = parametersA->N;
    chemkit::Real N_b = parametersB->N;
    chemkit::Real A_a = parametersA->A;
    chemkit::Real A_b = parametersB->A;
    chemkit::Real G_a = parametersA->G;
    chemkit::Real G_b = parametersB->G;
    chemkit::Real alpha_a = parametersA->alpha;
    chemkit::Real alpha_b = parametersB->alpha;
    char DA_a = parametersA->DA;
    char DA_b = parametersB->DA;

    // equation 9
    chemkit::Real rs_aa = A_a * pow(alpha_a, (1.0/4.0));
    chemkit::Real rs_bb = A_b * pow(alpha_b, (1.0/4.0));

    // equation 11
    chemkit::Real gamma = (rs_aa - rs_bb) / (rs_aa + rs_bb);

    // equation 10
    chemkit::Real rs;

    if(DA_a == 'D' || (DA_b == 'D')){
        rs = 0.5 * (rs_aa + rs_bb);
    }
    else{
        rs = 0.5 * (rs_aa + rs_bb) * (1.0 + 0.2 * (1.0 - exp(-12.0 * gamma * gamma)));
    }

    // equation 12
    chemkit::Real eps = ((181.16 * G_a * G_b * alpha_a * alpha_b) / (sqrt(alpha_a / N_a) + sqrt(alpha_b / N_b))) * pow(rs, -6.0);

    if((DA_a == 'D' && DA_b == 'A') || (DA_a == 'A' && DA_b == 'D')){
        rs *= 0.8;
        eps *= 0.5;
    }

    setParameter(0, rs);
    setParameter(1, eps);

    return true;
}

chemkit::Real MmffVanDerWaalsCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real rs = parameter(0);
    chemkit::Real eps = parameter(1);
    chemkit::Real r = coordinates->distance(a, b);

    // equation 8
    return eps * pow(((1.07 * rs) / (r + 0.07 * rs)), 7) * (((1.12 * pow(rs, 7)) / (pow(r, 7) + 0.12 * pow(rs, 7))) - 2);
}

std::vector<chemkit::Vector3> MmffVanDerWaalsCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real rs = parameter(0);
    chemkit::Real eps = parameter(1);
    chemkit::Real r = coordinates->distance(a, b);

    // dE/dr
    chemkit::Real de_dr = 7 * eps * pow(1.07 * rs / (r + 0.07 * rs), 6) *
                           ((-1.07 * rs / pow(r + 0.07 * rs, 2)) * (1.12 * pow(rs, 7) / (pow(r, 7) + 0.12 * pow(rs, 7)) - 2) +
                           (-1.12 * pow(rs, 7) * pow(r, 6) / pow(pow(r, 7) + 0.12 * pow(rs, 7), 2)) * (1.07 * rs / (r + 0.07 * rs)));

    boost::array<chemkit::Vector3, 2> gradient = coordinates->distanceGradient(a, b);

    gradient[0] *= de_dr;
    gradient[1] *= de_dr;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === MmffElectrostaticCalculation ======================================== //
MmffElectrostaticCalculation::MmffElectrostaticCalculation(size_t a, size_t b)
    : MmffCalculation(Electrostatic, 2, 3)
{
    setAtom(0, a);
    setAtom(1, b);
}

bool MmffElectrostaticCalculation::setup(const MmffParameters *parameters)
{
    CHEMKIT_UNUSED(parameters);

    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real oneFourScaling;

    if(topology->isOneFour(a, b)){
        oneFourScaling = 0.75;
    }
    else{
        oneFourScaling = 1.0;
    }

    setParameter(0, topology->charge(a));
    setParameter(1, topology->charge(b));
    setParameter(2, oneFourScaling);

    return true;
}

chemkit::Real MmffElectrostaticCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real qa = parameter(0);
    chemkit::Real qb = parameter(1);
    chemkit::Real oneFourScaling = parameter(2);

    chemkit::Real r = coordinates->distance(a, b);
    chemkit::Real e = 1.0; // dielectric constant
    chemkit::Real d = 0.05; // electrostatic buffering constant

    // equation 13
    return ((332.0716 * qa * qb) / (e * (r + d))) * oneFourScaling;
}

std::vector<chemkit::Vector3> MmffElectrostaticCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real qa = parameter(0);
    chemkit::Real qb = parameter(1);
    chemkit::Real oneFourScaling = parameter(2);

    chemkit::Real r = coordinates->distance(a, b);
    chemkit::Real e = 1.0; // dielectric constant
    chemkit::Real d = 0.05; // electrostatic buffering constant

    chemkit::Real de_dr = 332.0716 * qa * qb * oneFourScaling * (-1.0 / (e * pow(r + d, 2)));

    boost::array<chemkit::Vector3, 2> gradient = coordinates->distanceGradient(a, b);

    gradient[0] *= de_dr;
    gradient[1] *= de_dr;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_BA_BAS_HPP__
#define __OPTIMISATION2_BA_BAS_HPP__

#include "container.hpp"
#include <boost/mpl/transform.hpp>
#include <boost/fusion/include/as_map.hpp>
#include "create_hessian.hpp"

namespace lma
{

  template<class Bundle, class MatrixTag> struct ListJ
  {
    typedef typename Bundle::ListFunction L;

    template<class F> struct CreateBlocks
    {
      typedef Function<F> Fun;

      template<class Arg> struct CreateOneBlock
      {
        typedef  boost::fusion::pair<Arg*,typename lma::ContainerOption<
                                                          MatrixTag,
                                                          Size<typename Fun::ErreurType>::value,
                                                          lma::Size<Arg>::value
                                                         >::Matrix> type;
      };

      typedef typename br::as_map<typename mpl::transform<typename Fun::ParametersType,CreateOneBlock<mpl::_1>>::type>::type associative_tuple;
      typedef std::vector<associative_tuple,Eigen::aligned_allocator<associative_tuple>> type;
    };
    typedef typename mpl::transform<L,bf::pair<mpl::_1,CreateBlocks<mpl::_1>>>::type type;
  };

  template<class Bundle, class flt> struct Bas
  {
    typedef typename Bundle::ListFunction ListFunction;
    typedef typename Bundle::ParamFonctor ParamFonctor;
    typedef typename Bundle::ListeParam keys;

    typedef keys Keys;
    typedef flt MatrixTag;

    typedef typename mpl::transform< Keys, pair_>::type ListeDiag;
    typedef typename mpl::transform< Keys, VectorToPairStruct<mpl::_1,MatrixTag> >::type ListeVector;
    typedef typename ListH<Bundle,MatrixTag>::type ListeHessien;
    typedef typename br::as_map< ListeVector >::type Vectors;
    typedef typename br::as_map< ListeHessien >::type Hessian;

    typedef typename ListJ<Bundle,MatrixTag>::type ListeJacobian;
    typedef typename br::as_map< ListeJacobian>::type Jacobian;

    Hessian h;
    Vectors jte;
    Vectors delta;
    Jacobian jacob;

    Bas()
    {
//       std::cout << " hessien : " << ttt::name<ListeHessien>() << std::endl;
//       std::cout << ttt::name<ListeHessien2>() << std::endl;
//       std::cout << ttt::name<Jacobian>() << std::endl;
//       ListH<Bundle,MatrixTag>::disp();
    }
  };
}// end of bas

namespace ttt
{
}
#endif


/* decision_tree_xor_test.cc
   Jeremy Barnes, 25 February 2008
   Copyright (c) 2008 Jeremy Barnes.  All rights reserved.

   Test of the decision tree class.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <boost/thread.hpp>
#include <boost/thread/barrier.hpp>
#include <boost/bind.hpp>
#include <vector>
#include <stdint.h>
#include <iostream>

#include "jml/boosting/decision_tree_generator.h"
#include "jml/boosting/training_data.h"
#include "jml/boosting/dense_features.h"
#include "jml/boosting/feature_info.h"
#include "jml/utils/smart_ptr_utils.h"
#include "jml/utils/vector_utils.h"

using namespace ML;
using namespace std;

using boost::unit_test::test_suite;

/* Test that we are capable of learning the XOR function in a decision tree.
   It's tricky as the first split point looks like it hasn't learnt anything,
   but is necessary in order to continue.
*/

static const char * xor_dataset = "\
LABEL X Y\n\
1 0 0\n\
0 1 0\n\
0 0 1\n\
1 1 1\n\
";

static const char * config_options = "\
trace=1\n\
";

BOOST_AUTO_TEST_CASE( test_xor_function )
{
    /* Create the dataset */

    Dense_Feature_Space fs;

    Dense_Training_Data data;
    data.init(xor_dataset, xor_dataset + strlen(xor_dataset),
              make_unowned_sp(fs));
    guess_all_info(data, fs, true);

    cerr << "dataset has " << fs.features().size() << " features"
         << endl;

    cerr << fs.features() << endl;

    /* Create the decision tree generator */
    Configuration config;
    config.parse_string(config_options, "inbuilt config file");

    Decision_Tree_Generator generator;
    generator.configure(config);
    generator.init(data.feature_space(),
                   fs.features()[0]);

    boost::multi_array<float, 2> weights(boost::extents[data.example_count()][1]);
    std::fill(weights.data(), weights.data() + data.example_count(), 1.0 / data.example_count() / 2.0);

    Thread_Context context;

    Decision_Tree tree
        = generator.train_weighted(context, data, weights, data.all_features(), 3);

    cerr << tree.print();

    // Get the accuracy
    float accuracy JML_UNUSED = tree.accuracy(data).first;

    // Should be 100% accurate if we were able to learn properly
    //BOOST_CHECK_EQUAL(accuracy, 1.0);
}

/* We need to test the following situations:
   - Positive and negative infinity values in the dataset
   - Explicit NaN values versus simply missing features
   - Numbers so close together that there is no number between them
   - That the predict function has the same idea about the split as the split
     function
*/


/**
 * Copyright (c) 2016-present, Facebook, Inc.
 * All rights reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree. An additional grant
 * of patent rights can be found in the PATENTS file in the same directory.
 */

#include "LocalConstProp.h"

#include <boost/optional.hpp>
#include <cmath>
#include <stack>
#include <vector>

#include "ControlFlow.h"
#include "DexClass.h"
#include "DexUtil.h"
#include "IRInstruction.h"
#include "Transform.h"
#include "Walkers.h"

/** Local (basic block level) constant propagation.
 *
 * This analysis goes instruction by instruction at the basic block boundary
 * and gathers facts about constants and propagates them inside the constant
 * value lattice model defined in GlobalConstProp.h
 *
 * This alone can be used to drive a simple constant propagation analysis that
 * will reset itself after traversing a basic block and moving on to another.
 *
 * The idea is for this analysis to be composed with GlobalConstantPropagation
 * which properly combines different facts about constants at the cross basic
 * block level.
 */

constexpr const char* METRIC_BRANCH_PROPAGATED = "num_branch_propagated";

namespace {

template <typename Integral>
bool get_constant_value(const ConstPropEnvironment& env,
                        int16_t reg,
                        Integral& result);

template <>
bool get_constant_value(const ConstPropEnvironment& env,
                        int16_t reg,
                        int64_t& result) {
  if (ConstPropEnvUtil::is_wide_constant(env, reg)) {
    result = ConstPropEnvUtil::get_wide(env, reg);
    return true;
  } else {
    return false;
  }
}

template <>
bool get_constant_value(const ConstPropEnvironment& env,
                        int16_t reg,
                        int32_t& result) {
  if (ConstPropEnvUtil::is_narrow_constant(env, reg)) {
    result = ConstPropEnvUtil::get_narrow(env, reg);
    return true;
  } else {
    return false;
  }
}

template <typename Out, typename In>
// reinterpret the long's bits as a double
static Out reinterpret_bits(In in) {
  if (std::is_same<In, Out>::value) {
    return in;
  }
  static_assert(sizeof(In) == sizeof(Out), "types must be same size");
  return *reinterpret_cast<Out*>(&in);
}

bool is_compare_floating(DexOpcode op) {
  return op == OPCODE_CMPG_DOUBLE || op == OPCODE_CMPL_DOUBLE ||
         op == OPCODE_CMPG_FLOAT || op == OPCODE_CMPL_FLOAT;
}

bool is_less_than_bias(DexOpcode op) {
  return op == OPCODE_CMPL_DOUBLE || op == OPCODE_CMPL_FLOAT;
}

// Propagate the result of a compare if the operands are known constants.
// If we know enough, put -1, 0, or 1 into the destination register.
//
// Stored is how the data is stored in the register (the size).
//   Should be int32_t or int64_t
// Operand is how the data is used.
//   Should be float, double, or int64_t
template <typename Operand, typename Stored>
void analyze_compare(const IRInstruction* inst,
                     ConstPropEnvironment* current_state) {
  DexOpcode op = inst->opcode();
  Stored left_value;
  Stored right_value;
  auto left = get_constant_value(*current_state, inst->src(0), left_value);
  auto right = get_constant_value(*current_state, inst->src(1), right_value);

  if (left && right) {
    int32_t result;
    auto l_val = reinterpret_bits<Operand, Stored>(left_value);
    auto r_val = reinterpret_bits<Operand, Stored>(right_value);
    if (is_compare_floating(op) && (std::isnan(l_val) || std::isnan(r_val))) {
      if (is_less_than_bias(op)) {
        result = -1;
      } else {
        result = 1;
      }
    } else if (l_val > r_val) {
      result = 1;
    } else if (l_val == r_val) {
      result = 0;
    } else { // l_val < r_val
      result = -1;
    }
    TRACE(CONSTP,
          5,
          "Propagated constant in branch instruction %s, "
          "Operands [%d] [%d] -> Result: [%d]\n",
          SHOW(inst),
          l_val,
          r_val,
          result);
    ConstPropEnvUtil::set_narrow(*current_state, inst->dest(), result);
  } else {
    ConstPropEnvUtil::set_top(*current_state, inst->dest());
  }
}

} // namespace anonymous

void LocalConstantPropagation::analyze_instruction(
    IRInstruction* const& inst, ConstPropEnvironment* current_state) {
  switch (inst->opcode()) {
  case OPCODE_CONST:
  case OPCODE_CONST_HIGH16:
  case OPCODE_CONST_4:
  case OPCODE_CONST_16: {
    TRACE(CONSTP,
          5,
          "Discovered new narrow constant for reg: %d, value: %d\n",
          inst->dest(),
          inst->literal());
    ConstPropEnvUtil::set_narrow(*current_state, inst->dest(), inst->literal());
    break;
  }
  case OPCODE_CONST_WIDE_16:
  case OPCODE_CONST_WIDE_32:
  case OPCODE_CONST_WIDE:
  case OPCODE_CONST_WIDE_HIGH16: {
    TRACE(CONSTP,
          5,
          "Discovered new wide constant for regs: %d, %d, value: %ld\n",
          inst->dest(),
          inst->dest() + 1,
          inst->literal());
    ConstPropEnvUtil::set_wide(*current_state, inst->dest(), inst->literal());
    break;
  }
  case OPCODE_MOVE_FROM16:
  case OPCODE_MOVE_16:
  case OPCODE_MOVE_OBJECT_FROM16:
  case OPCODE_MOVE_OBJECT_16:
  case OPCODE_MOVE:
  case OPCODE_MOVE_OBJECT: {
    analyze_move(inst, current_state, false /* is_wide */);
    break;
  }
  case OPCODE_MOVE_WIDE:
  case OPCODE_MOVE_WIDE_FROM16:
  case OPCODE_MOVE_WIDE_16: {
    analyze_move(inst, current_state, true /* is_wide */);
    break;
  }

  case OPCODE_CMPL_FLOAT:
  case OPCODE_CMPG_FLOAT: {
    analyze_compare<float, int32_t>(inst, current_state);
    break;
  }

  case OPCODE_CMPL_DOUBLE:
  case OPCODE_CMPG_DOUBLE: {
    analyze_compare<double, int64_t>(inst, current_state);
    break;
  }

  case OPCODE_CMP_LONG: {
    analyze_compare<int64_t, int64_t>(inst, current_state);
    break;
  }

  default: {
    if (inst->dests_size()) {
      TRACE(CONSTP,
            5,
            "Marking value unknown [Reg: %d, Is wide: %d]\n",
            inst->dest(),
            inst->dest_is_wide());
      ConstPropEnvUtil::set_top(
          *current_state, inst->dest(), inst->dest_is_wide());
    }
  }
  }
}

void LocalConstantPropagation::analyze_move(IRInstruction* const& inst,
                                            ConstPropEnvironment* current_state,
                                            bool is_wide) {
  auto src = inst->src(0);
  auto dst = inst->dest();
  if (!is_wide && ConstPropEnvUtil::is_narrow_constant(*current_state, src)) {
    auto value = ConstPropEnvUtil::get_narrow(*current_state, src);
    TRACE(CONSTP,
          5,
          "Propagating narrow constant [Reg: %d, Value: %X] -> "
          "[Reg: %d]\n",
          src,
          value,
          dst);
    ConstPropEnvUtil::set_narrow(*current_state, dst, value);
  } else if (is_wide &&
             ConstPropEnvUtil::is_wide_constant(*current_state, src)) {
    auto value = ConstPropEnvUtil::get_wide(*current_state, src);
    TRACE(CONSTP,
          5,
          "Propagating wide constant [Reg: %d, Value: %lX] -> "
          "[Reg: %d]\n",
          src,
          value,
          dst);
    ConstPropEnvUtil::set_wide(*current_state, dst, value);
  } else {
    TRACE(CONSTP,
          5,
          "Marking value unknown [Reg: %d, Is wide: %d]\n",
          dst,
          is_wide);
    ConstPropEnvUtil::set_top(*current_state, dst, is_wide);
  }
}

// Evaluate the guard expression of an if opcode.
// pass 0 as the r_val for if-*Z opcodes
bool eval_if(DexOpcode op, int32_t l_val, int32_t r_val) {
  switch (op) {
  case OPCODE_IF_EQ:
  case OPCODE_IF_EQZ:
    return l_val == r_val;
  case OPCODE_IF_NE:
  case OPCODE_IF_NEZ:
    return l_val != r_val;
  case OPCODE_IF_LT:
  case OPCODE_IF_LTZ:
    return l_val < r_val;
  case OPCODE_IF_GE:
  case OPCODE_IF_GEZ:
    return l_val >= r_val;
  case OPCODE_IF_GT:
  case OPCODE_IF_GTZ:
    return l_val > r_val;
  case OPCODE_IF_LE:
  case OPCODE_IF_LEZ:
    return l_val <= r_val;
  default:
    always_assert_log(false, "opcode %s must be an if", SHOW(op));
  }
}

// If we can prove the operands of a branch instruction are constant values
// replace the conditional branch with an unconditional one.
void LocalConstantPropagation::simplify_branch(
    IRInstruction*& inst, const ConstPropEnvironment& current_state) {
  int32_t left_value;
  int32_t right_value;

  auto left = get_constant_value(current_state, inst->src(0), left_value);
  if (!left) {
    return;
  }

  if (inst->srcs_size() == 2) {
    auto right = get_constant_value(current_state, inst->src(1), right_value);
    if (!right) {
      return;
    }
  } else {
    // if-*Z vA        is the same as
    // if-*  vA, 0
    right_value = 0;
  }

  bool branch_result = eval_if(inst->opcode(), left_value, right_value);
  TRACE(CONSTP, 2, "Changed conditional branch %s\n", SHOW(inst));

  IRInstruction* replacement = nullptr;
  if (branch_result) {
    // Transform keeps track of the target and selects the right size
    // instruction based on the offset
    replacement = new IRInstruction(OPCODE_GOTO);
  } else {
    replacement = new IRInstruction(OPCODE_NOP);
  }

  m_branch_replacements.emplace_back(inst, replacement);
}

void LocalConstantPropagation::simplify_instruction(
    IRInstruction*& inst, const ConstPropEnvironment& current_state) {
  switch (inst->opcode()) {
  case OPCODE_IF_EQ:
  case OPCODE_IF_NE:
  case OPCODE_IF_LT:
  case OPCODE_IF_GE:
  case OPCODE_IF_GT:
  case OPCODE_IF_LE:
  case OPCODE_IF_LTZ:
  case OPCODE_IF_GEZ:
  case OPCODE_IF_GTZ:
  case OPCODE_IF_LEZ:
  case OPCODE_IF_EQZ:
  case OPCODE_IF_NEZ: {
    simplify_branch(inst, current_state);
    break;
  }

  default: {}
  }
}

void LocalConstantPropagation::propagate(DexMethod* method) {
  TRACE(CONSTP, 5, "Class: %s\n", SHOW(method->get_class()));
  TRACE(CONSTP, 5, "Method: %s\n", SHOW(method->get_name()));

  auto code = method->get_code();
  code->build_cfg();
  auto& cfg = code->cfg();
  TRACE(CONSTP, 5, "CFG: %s\n", SHOW(cfg));
  propagate_constants_in_method(method, cfg);
}

// This is temporary and should go away once we combine
// Intraprocedural constant propagation with this pass.
void LocalConstantPropagation::propagate_constants_in_method(
    DexMethod* method, ControlFlowGraph& cfg) {
  for (auto current_block : cfg.blocks()) {
    ConstPropEnvironment env;
    TRACE(CONSTP, 5, "Processing block %d\n", current_block->id());
    for (auto& mie : InstructionIterable(current_block)) {
      auto inst = mie.insn;
      TRACE(CONSTP, 5, "Analyzing constants in instruction: %s\n", SHOW(inst));
      analyze_instruction(inst, &env);
      simplify_instruction(inst, env);
    }
  }

  auto code = method->get_code();
  for (auto const& p : m_branch_replacements) {
    auto const& old_op = p.first;
    auto const& new_op = p.second;
    if (new_op->opcode() == OPCODE_NOP) {
      TRACE(CONSTP, 4, "Removing instruction %s\n", SHOW(old_op));
      code->remove_opcode(old_op);
      delete new_op;
    } else {
      TRACE(CONSTP,
            4,
            "Replacing instruction %s -> %s\n",
            SHOW(old_op),
            SHOW(new_op));
      code->replace_branch(old_op, new_op);
    }
    m_branch_propagated++;
  }
  m_branch_replacements.clear();
}

void LocalConstantPropagation::run() {
  walk_methods(m_scope, [&](DexMethod* m) {
    if (!m->get_code()) {
      return;
    }
    // Skipping blacklisted classes
    if (m_blacklist.count(m->get_class()) > 0) {
      TRACE(
          CONSTP, 2, "Skipping %s.%s\n", show(m->get_class()).c_str(), SHOW(m));
      return;
    }
    propagate(m);
  });

  TRACE(CONSTP, 1, "Branch condition removed: %lu\n", m_branch_propagated);
}

void LocalConstantPropagationPass::run_pass(DexStoresVector& stores,
                                            ConfigFiles& /* cfg */,
                                            PassManager& mgr) {
  auto scope = build_class_scope(stores);
  LocalConstantPropagation constant_prop(scope, m_blacklist);
  constant_prop.run();
  mgr.incr_metric(METRIC_BRANCH_PROPAGATED,
                  constant_prop.num_branch_propagated());
}

void LocalConstantPropagationPass::configure_pass(const PassConfig& pc) {
  std::vector<std::string> blacklist_names;
  pc.get("blacklist", {}, blacklist_names);

  for (auto const& name : blacklist_names) {
    DexType* entry = DexType::get_type(name.c_str());
    if (entry) {
      TRACE(CONSTP, 2, "Blacklisted class: %s\n", SHOW(entry));
      m_blacklist.insert(entry);
    }
  }
}

static LocalConstantPropagationPass s_pass;


#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/io/pcd_io.h>
#include <boost/filesystem.hpp>
#include <flann/flann.h>
#include <flann/io/hdf5.h>
#include <fstream>

typedef std::pair<std::string, std::vector<float> > vfh_model;

/** \brief Loads an n-D histogram file as a VFH signature
  * \param path the input file name
  * \param vfh the resultant VFH model
  */
bool
loadHist (const boost::filesystem::path &path, vfh_model &vfh)
{
  int vfh_idx;
  // Load the file as a PCD
  try
  {
    pcl::PCLPointCloud2 cloud;
    int version;
    Eigen::Vector4f origin;
    Eigen::Quaternionf orientation;
    pcl::PCDReader r;
    int type; unsigned int idx;
    r.readHeader (path.string (), cloud, origin, orientation, version, type, idx);

    vfh_idx = pcl::getFieldIndex (cloud, "vfh");
    if (vfh_idx == -1)
      return (false);
    if ((int)cloud.width * cloud.height != 1)
      return (false);
  }
  catch (const pcl::InvalidConversionException&)
  {
    return (false);
  }

  // Treat the VFH signature as a single Point Cloud
  pcl::PointCloud <pcl::VFHSignature308> point;
  pcl::io::loadPCDFile (path.string (), point);
  vfh.second.resize (308);

  std::vector <pcl::PCLPointField> fields;
  pcl::getFieldIndex (point, "vfh", fields);

  for (size_t i = 0; i < fields[vfh_idx].count; ++i)
  {
    vfh.second[i] = point.points[0].histogram[i];
  }
  vfh.first = path.string ();
  return (true);
}

/** \brief Load a set of VFH features that will act as the model (training data)
  * \param argc the number of arguments (pass from main ())
  * \param argv the actual command line arguments (pass from main ())
  * \param extension the file extension containing the VFH features
  * \param models the resultant vector of histogram models
  */
void
loadFeatureModels (const boost::filesystem::path &base_dir, const std::string &extension,
                   std::vector<vfh_model> &models)
{
  if (!boost::filesystem::exists (base_dir) && !boost::filesystem::is_directory (base_dir))
    return;

  for (boost::filesystem::directory_iterator it (base_dir); it != boost::filesystem::directory_iterator (); ++it)
  {
    if (boost::filesystem::is_directory (it->status ()))
    {
      std::stringstream ss;
      ss << it->path ();
      pcl::console::print_highlight ("Loading %s (%lu models loaded so far).\n", ss.str ().c_str (), (unsigned long)models.size ());
      loadFeatureModels (it->path (), extension, models);
    }
    if (boost::filesystem::is_regular_file (it->status ()) && boost::filesystem::extension (it->path ()) == extension)
    {
      vfh_model m;
      if (loadHist (base_dir / it->path ().filename (), m))
        models.push_back (m);
    }
  }
}

int
main (int argc, char** argv)
{
  if (argc < 2)
  {
    PCL_ERROR ("Need at least two parameters! Syntax is: %s [model_directory] [options]\n", argv[0]);
    return (-1);
  }

  std::string extension (".pcd");
  transform (extension.begin (), extension.end (), extension.begin (), (int(*)(int))tolower);

  std::string kdtree_idx_file_name = "kdtree.idx";
  std::string training_data_h5_file_name = "training_data.h5";
  std::string training_data_list_file_name = "training_data.list";

  std::vector<vfh_model> models;

  // Load the model histograms
  loadFeatureModels (argv[1], extension, models);
  pcl::console::print_highlight ("Loaded %d VFH models. Creating training data %s/%s.\n",
      (int)models.size (), training_data_h5_file_name.c_str (), training_data_list_file_name.c_str ());

  // Convert data into FLANN format
  flann::Matrix<float> data (new float[models.size () * models[0].second.size ()], models.size (), models[0].second.size ());

  for (size_t i = 0; i < data.rows; ++i)
    for (size_t j = 0; j < data.cols; ++j)
      data[i][j] = models[i].second[j];

  // Save data to disk (list of models)
  flann::save_to_file (data, training_data_h5_file_name, "training_data");
  std::ofstream fs;
  fs.open (training_data_list_file_name.c_str ());
  for (size_t i = 0; i < models.size (); ++i)
    fs << models[i].first << "\n";
  fs.close ();

  // Build the tree index and save it to disk
  pcl::console::print_error ("Building the kdtree index (%s) for %d elements...\n", kdtree_idx_file_name.c_str (), (int)data.rows);
  flann::Index<flann::ChiSquareDistance<float> > index (data, flann::LinearIndexParams ());
  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KDTreeIndexParams (4));
  index.buildIndex ();
  index.save (kdtree_idx_file_name);
  delete[] data.ptr ();

  return (0);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE4_1_GROUP_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE4_1_GROUP_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE4_1_SUPPORT

#include <boost/simd/swar/functions/group.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/dispatch/meta/downgrade.hpp>
#include <boost/simd/include/functions/simd/saturate.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::group_, boost::simd::tag::sse4_1_, (A0)
                            , ((simd_<uint32_<A0>,boost::simd::tag::sse_>))
                              ((simd_<uint32_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename dispatch::meta::downgrade<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 aa0 = boost::simd::saturate<result_type>(a0);
      A0 aa1 = boost::simd::saturate<result_type>(a1);
      return simd::bitwise_cast<result_type>(_mm_packus_epi32(aa0, aa1));
    }
  };
} } }

#endif
#endif


/*
 * pixel_to_millimeter.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dxtbx/model/panel.h>
#include <dxtbx/model/pixel_to_millimeter.h>

namespace dxtbx { namespace model { namespace boost_python {

  using namespace boost::python;

  static
  OffsetPxMmStrategy* OffsetPxMmStrategy_init(
      scitbx::af::versa< double, scitbx::af::flex_grid<> > dx,
      scitbx::af::versa< double, scitbx::af::flex_grid<> > dy) {

    DXTBX_ASSERT(dx.accessor().all().size() == 2);
    DXTBX_ASSERT(dy.accessor().all().size() == 2);
    DXTBX_ASSERT(dx.accessor().all().all_eq(dy.accessor().all()));

    std::size_t ysize = dx.accessor().all()[0];
    std::size_t xsize = dx.accessor().all()[1];

    scitbx::af::c_grid<2> grid(ysize, xsize);
    scitbx::af::versa< double, scitbx::af::c_grid<2> > dx2(dx.handle(), grid);
    scitbx::af::versa< double, scitbx::af::c_grid<2> > dy2(dy.handle(), grid);

    return  new OffsetPxMmStrategy(dx2, dy2);
  }

  static
  OffsetParallaxCorrectedPxMmStrategy* OffsetParallaxCorrectedPxMmStrategy_init(
      double mu,
      double t0,
      scitbx::af::versa< double, scitbx::af::flex_grid<> > dx,
      scitbx::af::versa< double, scitbx::af::flex_grid<> > dy) {

    DXTBX_ASSERT(dx.accessor().all().size() == 2);
    DXTBX_ASSERT(dy.accessor().all().size() == 2);
    DXTBX_ASSERT(dx.accessor().all().all_eq(dy.accessor().all()));

    std::size_t ysize = dx.accessor().all()[0];
    std::size_t xsize = dx.accessor().all()[1];

    scitbx::af::c_grid<2> grid(ysize, xsize);
    scitbx::af::versa< double, scitbx::af::c_grid<2> > dx2(dx.handle(), grid);
    scitbx::af::versa< double, scitbx::af::c_grid<2> > dy2(dy.handle(), grid);

    return  new OffsetParallaxCorrectedPxMmStrategy(mu, t0, dx2, dy2);
  }

  struct PxMmStrategyPickleSuite : boost::python::pickle_suite {
    static
    boost::python::tuple getinitargs(const PxMmStrategy& obj) {
      return boost::python::make_tuple();
    }
  };

  struct ParallaxCorrectedPxMmStrategyPickleSuite : boost::python::pickle_suite {
    static
    boost::python::tuple getinitargs(const ParallaxCorrectedPxMmStrategy& obj) {
      return boost::python::make_tuple(obj.mu(), obj.t0());
    }
  };

  struct OffsetPxMmStrategyPickleSuite : boost::python::pickle_suite {
    static
    boost::python::tuple getinitargs(const OffsetPxMmStrategy& obj) {
      return boost::python::make_tuple(obj.dx(), obj.dy());
    }
  };

  struct OffsetParallaxCorrectedPxMmStrategyPickleSuite : boost::python::pickle_suite {
    static
    boost::python::tuple getinitargs(const OffsetParallaxCorrectedPxMmStrategy& obj) {
      return boost::python::make_tuple(obj.mu(), obj.t0(), obj.dx(), obj.dy());
    }
  };

  void export_pixel_to_millimeter()
  {
    class_<PxMmStrategy, boost::noncopyable>("PxMmStrategy", no_init)
      .def("to_millimeter", &PxMmStrategy::to_millimeter, (
        arg("panel"), arg("xy")))
      .def("to_pixel", &PxMmStrategy::to_pixel, (
        arg("panel"), arg("xy")))
      .def("name", &PxMmStrategy::name)
      .def("__str__", &PxMmStrategy::strategy_name)
      ;

    class_<SimplePxMmStrategy, bases<PxMmStrategy> >("SimplePxMmStrategy")
      .def_pickle(PxMmStrategyPickleSuite())
      ;

    class_<ParallaxCorrectedPxMmStrategy, bases<SimplePxMmStrategy> >(
      "ParallaxCorrectedPxMmStrategy", no_init)
      .def(init<double, double>((arg("mu"), arg("t0"))))
      .def("mu",&ParallaxCorrectedPxMmStrategy::mu)
      .def("t0",&ParallaxCorrectedPxMmStrategy::t0)
      .def_pickle(ParallaxCorrectedPxMmStrategyPickleSuite())
      ;

    class_<OffsetPxMmStrategy, bases<PxMmStrategy> >(
      "OffsetPxMmStrategy", no_init)
      .def("__init__",
          make_constructor(
          &OffsetPxMmStrategy_init,
          default_call_policies(), (
            (arg("dx"),
             arg("dy")))))
      .def("dx", &OffsetPxMmStrategy::dx)
      .def("dy", &OffsetPxMmStrategy::dy)
      .def_pickle(OffsetPxMmStrategyPickleSuite())
      ;

    class_<OffsetParallaxCorrectedPxMmStrategy, bases<ParallaxCorrectedPxMmStrategy> >(
      "OffsetParallaxCorrectedPxMmStrategy", no_init)
      .def("__init__",
          make_constructor(
          &OffsetParallaxCorrectedPxMmStrategy_init,
          default_call_policies(), (
            (arg("mu"),
             arg("t0"),
             arg("dx"),
             arg("dy")))))
      .def("dx", &OffsetParallaxCorrectedPxMmStrategy::dx)
      .def("dy", &OffsetParallaxCorrectedPxMmStrategy::dy)
      .def_pickle(OffsetParallaxCorrectedPxMmStrategyPickleSuite())
      ;

    register_ptr_to_python<shared_ptr<PxMmStrategy> >();
    register_ptr_to_python<shared_ptr<SimplePxMmStrategy> >();
    register_ptr_to_python<shared_ptr<ParallaxCorrectedPxMmStrategy> >();
    register_ptr_to_python<shared_ptr<OffsetPxMmStrategy> >();
    register_ptr_to_python<shared_ptr<OffsetParallaxCorrectedPxMmStrategy> >();
  }

}}} // namespace = dxtbx::model::boost_python


#include "lipton-tarjan.h"
#include <iostream>
#include <vector>
#include <algorithm>
#include <utility>
#include <csignal>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/range/irange.hpp>
using namespace std;
using namespace boost;

/*
int levi_civita(uint i, uint j, uint k)
{
        if( i == j || j == k || k == i ) return 0;
        if( i == 1 && j == 2 && k == 3 ) return 1;
        if( i == 2 && j == 3 && k == 1 ) return 1;
        if( i == 3 && j == 1 && k == 2 ) return 1;
        return -1;
}

struct BFSVertData
{
        VertDescriptor parent;
        int              level;
};

struct BFSVertData2
{
        VertDescriptor parent;
        uint             cost;
};

map<VertDescriptor, vector<VertDescriptor>> children, children2;

map<VertDescriptor, BFSVertData>  bfs_vertex_data; // why can't this be inside bfs_visitor_buildtree
map<VertDescriptor, BFSVertData2> bfs_vertex_data2;
int num_levels = 1;

struct bfs_visitor_buildtree : public default_bfs_visitor
{
        template<typename Edge, typename Graph> void tree_edge(Edge e, Graph const& g)
        {
                cout << "   tree edge " << e << '\n';
                auto parent = source(e, g);
                auto child  = target(e, g);
                bfs_vertex_data[child].parent = parent;
                bfs_vertex_data[child].level  = bfs_vertex_data[parent].level+1;
                cout << "level of " << child << " = " << bfs_vertex_data[child].level << '\n';
                num_levels = max(num_levels, bfs_vertex_data[child].level+1);
                children[parent].push_back(child);
        }

};

bool is_tree_edge(EdgeDescriptor e, Graph const& g)
{
        auto src = source(e, g);
        auto tar = target(e, g);

        return bfs_vertex_data[src].parent == tar ||
               bfs_vertex_data[tar].parent == src;
}

bool is_tree_edge2(EdgeDescriptor e, Graph const& g)
{
        auto src = source(e, g);
        auto tar = target(e, g);

        return bfs_vertex_data2[src].parent == tar ||
               bfs_vertex_data2[tar].parent == src;
}

bool on_cycle(EdgeDescriptor e, vector<VertDescriptor> const& cycle_verts, Graph const& g)
{
        auto src = source(e, g);
        auto tar = target(e, g);
        return find(cycle_verts.begin(), cycle_verts.end(), src) != cycle_verts.end() &&
               find(cycle_verts.begin(), cycle_verts.end(), tar) != cycle_verts.end();
}

bool edge_inside(EdgeDescriptor e, VertDescriptor v, vector<VertDescriptor> const& cycle_verts, Graph const& g, Embedding& em)
{
        cout << "        testing if edge " << e << " is inside the cycle\n";
        auto it     = find(cycle_verts.begin(), cycle_verts.end(), v);
        auto before = it == cycle_verts.begin() ?
                      cycle_verts.end()-1       :
                      it-1;
        auto after  = it+1 == cycle_verts.end() ?
                      cycle_verts.begin()       :
                      it+1;
        cout << "               v: " << v << '\n';
        cout << "               before: " << *before << '\n';
        cout << "               after: " << *after << '\n';

        auto other = (source(e, g) == v) ?
                     target(e, g)        :
                     source(e, g);

        cout << "               other: " << other << '\n';

        vector<uint> perm;
        for( auto& tar_it : em[*it] ){
                assert(source(tar_it, g) == v);
                if( target(tar_it, g) == other   ) perm.push_back(1);
                if( target(tar_it, g) == *before ) perm.push_back(2);
                if( target(tar_it, g) == *after  ) perm.push_back(3);
        }
        assert(perm.size() == 3);
        cout << "               levi civita symbol: " << perm[0] << ' ' << perm[1] << ' ' << perm[2] << '\n';
        return levi_civita(perm[0], perm[1], perm[2]) == 1;
}

vector<pair<VertDescriptor, VertDescriptor>> edges_to_delete;
vector<pair<VertDescriptor, VertDescriptor>> edges_to_add;

struct Lambda
{
        map<VertDescriptor, bool>* table;
        Graph*                       g;
        VertDescriptor             x;
        int                          l0;

        Lambda(map<VertDescriptor, bool>* table, Graph* g, VertDescriptor x, int l0) : table(table), g(g), x(x), l0(l0) {}

        void doit(VertDescriptor V, EdgeDescriptor e)
        {
                auto v = source(e, *g);
                auto w = target(e, *g);
                if( V != v ) swap(v, w);
                assert(V == v);
                if ( !(*table)[w] ){
                        (*table)[w] = true;
                        assert(x != w);
                        cout << "connecting " << w << " with x\n";
                        edges_to_add.push_back(make_pair(x, w));
                }
                edges_to_delete.push_back(make_pair(v, w));
        }

        void finish()
        {
                for( auto& p : edges_to_add    ){
                        cout << "adding edge " << p.first << ", " << p.second << '\n';
                        assert(p.first != p.second);
                        add_edge(p.first, p.second, *g);
                }
                cout << "mid finish\n";
                print_graph(*g);
                for( auto& p : edges_to_delete ){
                        cout << "deleting edge " << p.first << ", " << p.second << '\n';
                        remove_edge(p.first, p.second, *g);
                }
        }
};

void scan_nonsubtree_edges(VertDescriptor v, Graph const& g, Embedding& em, Lambda lambda)
{
        if( bfs_vertex_data[v].level > lambda.l0 ) return;

        for( auto e : em[v]       ){
                if( !is_tree_edge(e, g) ){
                        lambda.doit(v, e);
                        continue;
                }
                auto src = source(e, g);
                auto tar = target(e, g);
                if( src != v ) swap(src, tar);
                assert(src == v);
                if( bfs_vertex_data[tar].level > lambda.l0 ) lambda.doit(v, e);
        }
        for( auto c : children[v] ) scan_nonsubtree_edges(c, g, em, lambda);
}

set<EdgeDescriptor> tree_edges;

struct bfs_visitor_shrinktree : public default_bfs_visitor
{
        template<typename Edge, typename Graph> void tree_edge(Edge e, Graph const& g)
        {
                cout << "shrinktree edge " << e << '\n';
                auto parent = source(e, g);
                auto child  = target(e, g);
                bfs_vertex_data2[child].parent = parent;
                tree_edges.insert(e);

                auto v = child;
                while( true ){ // TODO make this faster
                        ++bfs_vertex_data2[v].cost;
                        if( v == bfs_vertex_data2[v].parent ) break;
                        v = bfs_vertex_data2[v].parent;
                }
                children2[parent].push_back(child);
        }

};
*/

uint theorem4(uint partition, Graph const& g)
{
        /*
        Assume G is connected.
        Partition the vertices into levels according to their distance from some vertex v.
        L[l] = # of vertices on level l
        If r is the maximum distance of any vertex from v, define additional levels -1 and r+1 containing no vertices
        l1 = the level such that the sum of costs in levels 0 thru l1-1 < 1/2, but the sum of costs in levels 0 thru l1 is >= 1/2
        (If no such l1 exists, the total cost of all vertices < 1/2, and B = C = {} and return true)
        k = # of vertices on levels 0 thru l1.
        Find a level l0 such that l0 <= l1 and |L[l0]| + 2(l1-l0) <= 2sqrt(k)
        Find a level l2 such that l1+1 <= l2 and |L[l2] + 2(l2-l1-1) <= 2sqrt(n-k)
        If 2 such levels exist, then by Lemma 3 the vertices of G can be partitioned into three sets A, B, C such that no edge joins a vertex in A with a vertex in B,
        neither A or C has cost > 2/3, and C contains no more than 2(sqrt(k) + sqrt(n-k)) vertices.
        But 2(sqrt(k) + sqrt(n-k) <= 2(sqrt(n/2) + sqrt(n/2)) = 2sqrt(2)sqrt(n)
        Thus the theorem holds if suitable levels l0 and l2 exist
                Suppose a suitable level l0 does not exist.  Then, for i <= l1, L[i] >= 2sqrt(k) - 2(l1-i)
                Since L[0] = 1, this means 1 >= 2sqrt(k) - 2l1 and l1 + 1/2 >= sqrt(k).  Thus l1 = floor(l1 + 1/2) >
                Contradiction

        Now suppose G is not connected
        Let G1, G2, ... , Gk be the connected components of G, with vertex sets V1, V2, ... , Vk respectively.
        If no connected component has total vertex cost > 1/3, let i be the minimum index such that the total cost of V1 U V2 U ... U Vi > 1/3
        A = V1 U V2 U ... U Vi
        B = Vi+1 U Vi+2 U ... U Vk
        C = {}
        Since i is minimum and the cost of Vi <= 1/3, the cost of A <= 2/3. return true;
        If some connected component (say Gi) has total vertex cost between 1/3 and 2/3,
        A = Vi
        B = V1 U ... U Vi-1 U Vi+1 U ... U Vk
        C = {}
        return true

        Finally, if some connected component (say Gi) has total vertex cost exceeding 2/3,
        apply the above argument to Gi
        Let A*, B*, C* be the resulting partition.
        A = set among A* and B* with greater cost
        C = C*
        B = remanining vertices of G
        Then A and B have cost <= 2/3
        return true;

        In all cases the separator C is either empty or contained in only one connected component of G
        */
        return partition;
}

void lemma2()
{
        /*
        Embed G in the plane
        Make each face a triangle by adding a suitable # of additional edges
        Any nontree edge (including each of the added edges) forms a simple cycle with some of the tree edges
        This cycle is of length at most 2r + 1 if it contains the root of the tree, at most 2r-1 otherwise
        The cycle divides the plane (and the graph) into two parts, the inside and outside
        We claim that at least one such cycle separates the graph so that neither the inside nor the outside contains vertices whose total cost > 2/3
                Let (x, z) be the nontree edge whose cycle minimizes the maximum cost either inside or outside the cycle.
                Break ties by choosing the nontree edge whose cycle has the smallest # of faces on the same side as the maximum cost.
                If ties remain, choose arbitrarily
                Suppose wihtout loss of generality that the graph is embedded so that the cost inside the (x, z) cycle is at least as great as the cost outside the cycle.
                If the vertices inside the cycle have total cost <= 2/3, return true
                Suppose that the vertices inside the cycle have total cost > 2/3.  Contradiction.
        */
}

uint lemma3(vector<VertDescriptor> const& cycle_verts, int* l, Graph const& g)
{
        /*
        if( l[1] > l[2] ){
                cout << "A = all verts on levels 0    thru l1-1";
                cout << "B = all verts on levels l1+1 thru r";
                cout << "C = all verts on llevel l1";
        }

        if( l[1] < l[2] ){
                cout << "don't know\n";
                vector<VertDescriptor> zero_one, middle_part, one_two;
                VertIterator vei, vend;
                for( tie(vei, vend) = vertices(g); vei != vend; ++vei ){
                        auto v = *vei;
                        if( bfs_vertex_data[v].level <= l[1] ){
                                cout << "first part: " << v << '\n';
                                zero_one.push_back(v);
                                continue;
                        }
                        if( bfs_vertex_data[v].level >= l[1]+1 &&
                            bfs_vertex_data[v].level <= l[2]-1 ){
                                cout << "middle part: " << v << '\n';
                                middle_part.push_back(v);
                                continue;
                        }
                        if( bfs_vertex_data[v].level >= l[2] ){
                                cout << "last part: " << v << '\n';
                                one_two.push_back(v);
                                continue;
                        }
                        cout << "level: " << bfs_vertex_data[v].level << '\n';
                        assert(0);
                }

                //delete verts in levels l1 and l2
                        this separates remaining vertices into 3 parts: (all of which may be empty)
                                verts on levels 0 thru l1-1
                                verts on level l1+1 thru l2-1
                                verts on levels l2+1 and above
                        the only part which can have cost > 2/3 is the middle part
                        if( middle_part.size() <= 2*num_vertices(g)/3 ){
                                cout << "A = most costly part of the 3\n";
                                cout << "B = remaining 2 parts\n";
                                cout << "C = "; for( auto& v : one_two ) cout << v << ' '; cout << '\n';
                        } else {
                                delete all verts on level l2 and above
                                shrink all verts on levels l1 and belowe to a single vertex of cost zero
                                The new graph has a spanning tree radius of l2 - l1 -1 whose root corresponds to vertices on levels l1 and below in the original graph
                                Apply Lemma 2 to the new graph, A* B* C*
                                cout << "A = set among A* and B* with greater cost\n";
                                cout << "C = verts on levels l1 and l2 in the original graph plus verts in C* minus the root\n";
                                cout << "B = remaining verts\n";
                                By Lemma 2, A has total cost <= 2/3
                                But A U C* has total cost >= 1/3, so B also has total cost <= 2/3
                                Futhermore, C contains no more than L[l1] + L[l2] + 2(l2 - l1 - 1)
                        }
        }
        */
        return 0;
}

void print_canonical_ordering(Graph const& g, vector<VertDescriptor> const& ordering, Embedding const& em)
{
        for( auto& v : ordering ){
                cout << "vertex " << v << "\n";
                for( auto& e : em[v] ){
                        cout << " has incident edge " << e << '\n';
                }
        }
}

void print_bfs_tree(vector<uint> const& L)
{
        /*
        for( auto it = bfs_vertex_data.begin(); it != bfs_vertex_data.end(); ++it ){
                auto v = it->second;
                cout << "parent of " << it->first << ": " << v.parent << ", level: " << v.level << '\n';
        }
        for( uint i = 0; i < L.size();               ++i ) cout << "L[" << i << "] = " << L[i] << '\n';
        */
}

void makemaxplanar(Graph& g)
{
        /*
        auto e_index = get(edge_index, g);
        EdgesSizeType num_edges = 0;
        EdgeIterator ei, ei_end;
        for( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei ) put(e_index, *ei, num_edges++);

        vector<vector<EdgeDescriptor>> em(num_vertices(g));
        boyer_myrvold_planarity_test(g, &em[0]);

        make_biconnected_planar(g, &em[0]);

        num_edges = 0;
        for( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei ) put(e_index, *ei, num_edges++);
        boyer_myrvold_planarity_test(g, &em[0]);

        make_maximal_planar(g, &em[0]);

        num_edges = 0;
        for( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei ) put(e_index, *ei, num_edges++);
        bool planar = boyer_myrvold_planarity_test(g, &em[0]);
        assert(planar);
        */
}

/*
uint edge_cost(EdgeDescriptor e, Graph const& g)
{
        assert(is_tree_edge2(e, g));
        auto v = source(e, g); // assert on the cycle
        auto w = target(e, g); // assert not on the cycle

        return bfs_vertex_data2[w].parent == w ?
               bfs_vertex_data2[w].cost        :
               num_vertices(g) - bfs_vertex_data2[v].cost;
}
*/

Partition lipton_tarjan(Graph const& gin)
{
        Graph g = gin;
/*

        cout << "\n---------------------------- Step 1 --------------------------\n";
        EmbeddingStorage storage(num_vertices(g));
        Embedding        em(storage.begin());
        bool planar = boyer_myrvold_planarity_test(g, em);
        assert(planar);

        print_graph(g);

        cout << "\n---------------------------- Step 2 --------------------------\n";
        vector<uint> vertid_to_component(num_vertices(g));
        uint components = connected_components(g, &vertid_to_component[0]);
        assert(components == 1);

        vector<uint> verts_per_comp(components, 0);
        for( uint i = 0; i < num_vertices(g); ++i ) ++verts_per_comp[vertid_to_component[i]];
        bool too_big = false;
        for( uint i = 0; i < components; ++i ) if( 3*verts_per_comp[i] > 2*num_vertices(g) ){
                too_big = true;
                break;
        }
        if( !too_big ){
                theorem4(0, g);
                Partition p;
                return p;
        }

        cout << "\n---------------------------- Step 3 --------------------------\n";
        bfs_visitor_buildtree vis;
        bfs_vertex_data[0].parent = 0;
        bfs_vertex_data[0].level  = 0;
        breadth_first_search(g, vertex(0, g), visitor(vis));
        for( auto& c : children ) sort(c.second.begin(), c.second.end());

        vector<uint> L(num_levels+1);
        for( auto& d : bfs_vertex_data ) ++L[d.second.level];
        cout << "\n---------------------------- Step 4 --------------------------\n";
        uint k  = L[0];
        int l[3];
        l[1] = 0;
        while( k <= num_vertices(g)/2 ) k += L[++l[1]];
        cout << "l1: " << l[1] << '\n';
        cout << "k: " << k << '\n';

        cout << "\n---------------------------- Step 5 --------------------------\n";
        float sq  = 2 * sqrt(k);
        float snk = 2 * sqrt(num_vertices(g) - k);

        l[0] = l[1];     for( ;; ){ if( L.at(l[0]) + 2*(l[1] - l[0])     <= sq  ) break; --l[0]; }
        l[2] = l[1] + 1; for( ;; ){ if( L.at(l[2]) + 2*(l[2] - l[1] - 1) <= snk ) break; ++l[2]; }
        cout << "l0: " << l[0] << '\n';
        cout << "l2: " << l[2] << '\n';

        print_graph(g);

        for( auto& it : bfs_vertex_data ) cout << "Level of " << it.first << " = " << it.second.level << '\n';

        cout << "\n---------------------------- Step 6 --------------------------\n";
        vector<VertDescriptor> verts_to_be_removed;
        VertIterator vei, vi_end, next;
        {
                tie(vei, vi_end) = vertices(g);
                for( next = vei; vei != vi_end; vei = next ){
                        ++next;
                        auto v = *vei;
                        if( bfs_vertex_data[v].level <= l[0] ){
                                cout << "going to shrink vertex " << v << '\n';
                                cout << "   level[v]: " << bfs_vertex_data[v].level << '\n';
                                cout << "   cutoff: " << l[0] << '\n';
                                verts_to_be_removed.push_back(v);
                        }
                        if( bfs_vertex_data[v].level >= l[2] ){
                                cout << "removing vertex " << v << "because too high\n";
                                clear_vertex(v, g);
                                remove_vertex(v, g);
                                verts_to_be_removed.push_back(v);
                                bfs_vertex_data.erase(bfs_vertex_data.find(v));
                        }
                }
        }
        auto x = add_vertex(g); // represents all verts on level 0 thru l0.
        cout << "adding vertex x" << x << "\n";
        map<VertDescriptor, bool> table;

        for( auto& v : bfs_vertex_data ){
                table[v.first] = v.second.level <= l[0];
                if( table[v.first] ) cout << "table[" << v.first << "] = TRUE";
                else cout << "table[" << v.first << "] = FALSE";
                cout << "level: " << v.second.level << '\n';
        }

        // Scan the edges incident to this tree clockwise around the tree.  When scanning an edge(v,w) with v in the tree...
        Lambda lambda(&table, &g, x, l[0]);
        scan_nonsubtree_edges(0, g, em, lambda);

        cout << "before lambda finishes\n";
        print_graph(g);

        lambda.finish();

        cout << "after lambda finishes\n";
        print_graph(g);

        for( auto& v : verts_to_be_removed ){
                cout << "removing vertex " << v << '\n';
                clear_vertex(v, g);
                remove_vertex(v, g);
        }

        lemma2();

        print_graph(g);

        cout << "\n---------------------------- Step 7 --------------------------\n";
        //if( degree(x, g) == 0 ){
                //cout << "!!!! vertex x (" << x << ") has no inbound vertices!  Deleting!!!!!\n";
                //remove_vertex(x, g);
                //x = 0;
        //}
        cout << "# verts: " << num_vertices(g) << '\n';
        cout << "# edges: " << num_edges   (g) << '\n';
        cout << "x = " << x << '\n';

        bfs_visitor_shrinktree vis2;
        bfs_vertex_data2[x].parent = x;
        breadth_first_search(g, x, visitor(vis2));
        for( auto& c : children2 ) sort(c.second.begin(), c.second.end());

        makemaxplanar(g);

        cout << "make maximal planar - should have " << 3*num_vertices(g) - 6 << " edges\n";
        cout << "# verts: " << num_vertices(g) << '\n';
        cout << "# edges: " << num_edges   (g) << '\n';
        uint e = num_edges(g);
        assert(e == 3*num_vertices(g) - 6);

        cout << "\n---------------------------- Step 8 --------------------------\n";

        print_graph(g);

        EdgeIterator ei, ei_end;
        for( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei ) if( !is_tree_edge2(*ei, g) ) break;
        assert(!is_tree_edge2(*ei, g));
        EdgeDescriptor chosen_edge = *ei;
        cout << "arbitrarily choosing nontree edge: " << chosen_edge << '\n';

        auto v1 = source(chosen_edge, g);
        auto w1 = target(chosen_edge, g);
        vector<VertDescriptor> parents_v, parents_w;

        auto p_v = v1; do { p_v = bfs_vertex_data2[p_v].parent; parents_v.push_back(p_v); } while( p_v );
        auto p_w = w1; do {p_w = bfs_vertex_data2[p_w].parent; parents_w.push_back(p_w); } while( p_w );

        uint i, j;
        for( i = 0; i < parents_v.size(); ++i ) for( j = 0; j < parents_w.size(); ++j ) if( parents_v[i] == parents_w[j] ) goto done;
done:
        assert(parents_v[i] == parents_w[j]);
        auto ancestor = parents_v[i];
        cout << "common ancestor: " << ancestor << '\n';

        vector<VertDescriptor> cycle_verts, tmp;
        VertDescriptor v;
        v = v1; while( v != ancestor ){ cycle_verts.push_back(v); v = bfs_vertex_data2[v].parent; }
        cycle_verts.push_back(ancestor);
        v = w1; while( v != ancestor ){ tmp.push_back(v); v = bfs_vertex_data2[v].parent; }
        reverse(tmp.begin(), tmp.end());
        cycle_verts.insert(cycle_verts.end(), tmp.begin(), tmp.end());

        cout << "cycle verts: ";
        for( auto& v : cycle_verts ) cout << v << ' ';
        cout << '\n';

        EmbeddingStorage storage2(num_vertices(g));
        Embedding        em2(storage2.begin());
        planar = boyer_myrvold_planarity_test(g, em2);
        assert(planar);

        uint cost_inside  = 0;
        uint cost_outside = 0;
        bool cost_swapped = false;

        for( auto& v : cycle_verts ){
                cout << "   scanning cycle vert " << v << '\n';
                auto pai = out_edges(v, g);
                while( pai.first != pai.second ){
                        if( is_tree_edge2(*pai.first, g) && !on_cycle(*pai.first, cycle_verts, g) ){
                                uint cost = edge_cost(*pai.first, g);
                                cout << "      scanning incident tree edge " << *pai.first << "   cost: " << cost << '\n';
                                bool inside = edge_inside(*pai.first, v, cycle_verts, g, em2);
                                inside ? cost_inside : cost_outside += cost;
                                cout << (inside ? "inside\n" : "outside\n");
                        }
                        ++pai.first;
                }
        }

        if( cost_outside > cost_inside ){
                swap(cost_outside, cost_inside);
                cost_swapped = true;
                cout << "cost swapped\n";
        }
        cout << "total inside cost: " << cost_inside << '\n';

        cout << "\n---------------------------- Step 9 --------------------------\n";
        auto vi = source(chosen_edge, g);
        auto wi = target(chosen_edge, g);
        assert(!is_tree_edge2(chosen_edge, g));
        EdgeDescriptor next_edge;
        while( cost_inside > num_vertices(g)*2./3 ){
                cout << "looking for a better cycle\n";

                // Locate the triangle (vi, y, wi) which has (vi, wi) as a boundary edge and lies inside the (vi, wi) cycle.
                set<VertDescriptor> verts_v, verts_w;
                auto pai = out_edges(vi, g);
                while( pai.first != pai.second ){
                        auto vv = target(*pai.first, g);
                        verts_v.insert(vv);
                        cout << "vertex " << vi << "has neighbor " << vv << '\n';
                        ++pai.first;
                }
                pai = out_edges(wi, g);
                while( pai.first != pai.second ){
                        auto vv = target(*pai.first, g);
                        verts_w.insert(vv);
                        cout << "vertex " << wi << "has neighbor " << vv << '\n';
                        ++pai.first;
                }

                // there are 2 triangles with edge (vi, wi), one inside and one outside
                EdgeDescriptor viy, ywi;
                if ( is_tree_edge2(viy, g) || is_tree_edge2(ywi, g) ){
                        next_edge = is_tree_edge2(viy, g) ? ywi : viy;
                        assert(!is_tree_edge2(next_edge, g));
                        // Compute the cost inside the (vi+1, wi+1) cycle from the cost inside the (vi, wi) cycle and the cost of vi, y, and wi.
                } else {
                        // Determine the tree path from y to the (vi, wi) cycle by following parent pointers from y.
                        VertDescriptor z; // the (vi, wi) cycle reached during this search.
                        // Compute the total cost of all vertices except z on this tree path.
                                // Scan the tree edges inside the (y, wi) cycle, alternately scanning an edge in one cycle and an edge in the other cycle.
                                // Stop scanning when all edges inside one of the cycles have been scanned.
                        // Compute the cost inside this cycle by summing the associated costs of all scanned edges.
                        // Use this cost, the cost inside the (vi, wi) cycle, and the cost on the tree path from y to zy to compute the cost inside the other cycle.
                        // Let (vi+1, w+1) be the edge among (vi, y) and (y, wi) whose cycle has more cost inside it.
                }

        }

        cout << "\n---------------------------- Step 10 --------------------------\n";
        uint partition = lemma3(cycle_verts, &l[0], g);
        partition = theorem4(partition, g);
        */
        Partition p;
        return p;
}


/**
 * @file
 * $Revision$
 * $Date$
 *
 *   Unless noted otherwise, the portions of Isis written by the USGS are public
 *   domain. See individual third-party library and package descriptions for
 *   intellectual property information,user agreements, and related information.
 *
 *   Although Isis has been used by the USGS, no warranty, expressed or implied,
 *   is made by the USGS as to the accuracy and functioning of such software
 *   and related material nor shall the fact of distribution constitute any such
 *   warranty, and no responsibility is assumed by the USGS in connection
 *   therewith.
 *
 *   For additional information, launch
 *   $ISISROOT/doc//documents/Disclaimers/Disclaimers.html in a browser or see
 *   the Privacy & Disclaimers page on the Isis website,
 *   http://isis.astrogeology.usgs.gov, and the USGS privacy and disclaimers on
 *   http://www.usgs.gov/privacy.html.
 */

#include <boost/foreach.hpp>
#include "opencv2/opencv.hpp"

#include "BlobDetectionAlgorithm.h"
#include <QVariant>

namespace Isis {

  /**
   * Constructs the algorithm with default variables.
   */
  BlobDetectionAlgorithm::BlobDetectionAlgorithm() :
                 Feature2DAlgorithm("Blob", "Feature2D",
                                    BLOBType::create()) {
   setupParameters();
  }

  /**
   * Constructs the algorithm with the input variables
   *
   * @param cvars  The variables and values the algorithm will use.
   *               Variables that are not included will be set to their default.
   * @param config The config string used to construct cvars.
   */
  BlobDetectionAlgorithm::BlobDetectionAlgorithm(const PvlFlatMap &cvars, const QString &config) :
                          Feature2DAlgorithm("Blob", "Feature2D",
                                              BLOBType::create(), cvars) {
    setConfig(config);
    PvlFlatMap variables = setupParameters();
    variables.merge(cvars);
    cv::SimpleBlobDetector::Params params;
    params.thresholdStep = variables.get("ThresholdStep").toFloat();
    params.minThreshold = variables.get("MinThreshold").toFloat();
    params.maxThreshold = variables.get("MaxThreshold").toFloat();
    params.minRepeatability = variables.get("MinRepeatability").toUInt();
    params.minDistBetweenBlobs = variables.get("MinDistance").toFloat();
    params.filterByColor = toBool(variables.get("FilterByColor"));
    params.blobColor = (unsigned char) toInt(variables.get("BlobColor"));
    params.filterByArea = toBool(variables.get("FilterByArea"));
    params.minArea = variables.get("MinArea").toFloat();
    params.maxArea = variables.get("MaxArea").toFloat();
    params.filterByCircularity = toBool(variables.get("FilterByCircularity"));
    params.minCircularity = variables.get("MinCircularity").toFloat();
    params.maxCircularity = variables.get("maxCircularity").toFloat();
    params.filterByInertia = toBool(variables.get("FilterByInertia"));
    params.minInertiaRatio = variables.get("MinInertiaRatio").toFloat();
    params.maxInertiaRatio = variables.get("MaxInertiaRatio").toFloat();
    params.filterByConvexity = toBool(variables.get("FilterByConvexity"));
    params.minConvexity = variables.get("MinConvexity").toFloat();
    params.maxConvexity = variables.get("MaxConvexity").toFloat();

    m_algorithm = BLOBType::create(params);

    m_variables.merge(variables);
  }

  /**
   * Destroys the algorithm
   */
  BlobDetectionAlgorithm::~BlobDetectionAlgorithm() { }

  /**
   * Sets up the algorithm parameters with default values.
   *
   * @return PvlFlatMap Algorithm parameters and their default values.
   */
  PvlFlatMap BlobDetectionAlgorithm::setupParameters() {
    PvlFlatMap variables;
    variables.add("ThresholdStep",       "10");
    variables.add("MinThreshold",        "50");
    variables.add("MaxThreshold",        "220");
    variables.add("MinRepeatability",    "2");
    variables.add("MinDistance",         "10");
    variables.add("FilterByColor",       "true");
    variables.add("BlobColor",           "0");
    variables.add("FilterByArea",        "true");
    variables.add("MinArea",             "25");
    variables.add("MaxArea",             "5000");
    variables.add("FilterByCircularity", "false");
    variables.add("MinCircularity",      "0.8");
    variables.add("maxCircularity",      "inf");
    variables.add("FilterByInertia",     "true");
    variables.add("MinInertiaRatio",     "0.1");
    variables.add("MaxInertiaRatio",     "inf");
    variables.add("FilterByConvexity",   "true");
    variables.add("MinConvexity",        "0.95");
    variables.add("MaxConvexity",        "inf");
    m_variables = variables;
    return (m_variables);
  }

  /**
   * Returns a description of the algorithm.
   *
   * @return @b QString A description of the algorithm.
   */
  QString BlobDetectionAlgorithm::description() const {
    QString desc = "The OpenCV simple blob detection algorithm."
                   " See the documentation at "
     "http://docs.opencv.org/3.1.0/d0/d7a/classcv_1_1SimpleBlobDetector.html";
    return (desc);
  }

  /**
   * Creates an instance of the algorithm.
   *
   * @param cvars  The variables and values the algorithm will use.
   *               Variables that are not included will be set to their default.
   * @param config The config string used to construct cvars.
   */
  Feature2DAlgorithm *BlobDetectionAlgorithm::create(const PvlFlatMap &vars,
                                                     const QString &config) {
    return ( new BlobDetectionAlgorithm(vars, config) );
  }

  /**
   * Returns true if the algorithm has a detector.
   *
   * @return @b true if the algorithm has a detector.
   */
  bool BlobDetectionAlgorithm::hasDetector() const {
    return true;
  }

  /**
   * Returns true if the algorithm has an extractor.
   *
   * @return @b true if the algorithm has an extractor.
   */
  bool BlobDetectionAlgorithm::hasExtractor() const {
    return false;
  }

  /**
   * Returns true if the algorithm has a matcher.
   *
   * @return @b true if the algorithm has a matcher.
   */
  bool BlobDetectionAlgorithm::hasMatcher() const {
    return false;
  }

  /**
   * Returns the variables and their values used by the algorithm.
   *
   * @return @b PvlFlatMap The variables and their values as keyword, value pairs.
   */
  PvlFlatMap BlobDetectionAlgorithm::getAlgorithmVariables( ) const{
    return ( variables() );
  }

/**
 * @brief Set parameters as provided by the variables
 *
 * @param variables Container of parameters to set
 *
 * @return @b int Always -1, variables cannot be set after initialization.
 *
 * @throws IException::Programmer "BlobDetectionAlgorithm does not have the ability
 *                                 to set algorithm parameters."
 */
  int BlobDetectionAlgorithm::setAlgorithmVariables(const PvlFlatMap &variables) {
    QString msg = "BlobDetectionAlgorithm does not have the ability to set algorithm parameters.";
    throw IException(IException::Programmer, msg, _FILEINFO_);

    return (-1);
  }

};  // namespace Isis


#ifndef EIGEN_BOOST_SERIALIZATION_HPP
#define EIGEN_BOOST_SERIALIZATION_HPP

#include <Eigen/Core>
#include <boost/serialization/array.hpp>

namespace boost
{
namespace serialization {
template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
inline void serialize(
        Archive & ar,
        Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> & t,
        const unsigned int file_version
        )
{
    size_t rows = t.rows(), cols = t.cols();
    ar & rows;
    ar & cols;
    if( rows != static_cast<unsigned long>(t.rows()) || cols != static_cast<unsigned long>(t.cols()) )
        t.resize( rows, cols );

    for(size_t i=0; i< static_cast<unsigned long>(t.size()); i++)
        ar & t.data()[i];
}
}
}
#endif // EIGEN_BOOST_SERIALIZATION_HPP