/* irls.cc
   Jeremy Barnes, 19 March 2004
   Copyright (c) 2004 Jeremy Barnes.  All rights reserved.
   $Source$

   Iteratively Reweighted Least Squares functionality.
*/

#include "irls.h"
#include "glz.h"
#include <boost/timer.hpp>
#include "jml/stats/distribution_simd.h"
#include "jml/utils/vector_utils.h"
#include "jml/boosting/config_impl.h"
#include "jml/arch/simd_vector.h"
#include "jml/arch/timers.h"
#include "multi_array_utils.h"
#include "jml/boosting/config_impl.h"
#include "jml/utils/string_functions.h"
#include "jml/algebra/lapack.h"
#include <boost/version.hpp>

using namespace std;

#ifndef UNDER_VALGRIND
#define UNDER_VALGRIND 0
#endif

// TODO:in which boost version was this added?
#if (BOOST_VERSION > 103500 && !UNDER_VALGRIND)
#include <boost/math/special_functions/erf.hpp>

namespace ML {

__thread std::ostream * debug_irls = 0;

double erf(double x)
{
    return boost::math::erf(x);
}

double erfinv(double y)
{
    return boost::math::erf_inv(y);
}

} // namespace ML

#else

namespace ML {

double erf(double x)
{
    return ::erf(x);
}

double erfinv(double y)
{
    return ::erfinv(y);
}

} // namespace ML

#endif

namespace ML {

__thread std::ostream * debug_remove_dependent = 0;
__thread bool check_remove_dependent = 0;

template<class FloatIn, class FloatCalc>
vector<int> remove_dependent_impl(boost::multi_array<FloatIn, 2> & x,
                                  std::vector<distribution<FloatCalc> > & y,
                                  double tolerance)
{
    boost::timer t;
    size_t nrows = x.shape()[0], ncols = x.shape()[1];
    /* Perform an orthogonalization to determine which variables are
       linearly dependent.  This procedure uses the Modified Gram-Schmidt
       process.
    */

    vector<distribution<FloatCalc> > v(nrows);  // orthonormal basis vectors
    //distribution<Float> r(nrows); // to detect dependent vectors
    vector<distribution<FloatCalc> > r(nrows, distribution<FloatCalc>(nrows, 0.0));

    // matrix such that v[i] = sum j ( x[j] z[i][j] )
    vector<distribution<FloatCalc> > z(nrows, distribution<FloatCalc>(nrows, 0.0));

    for (unsigned i = 0;  i < nrows;  ++i) {
        v[i] = distribution<FloatCalc>(&x[i][0], &x[i][0] + ncols);
        z[i][i] = 1.0;   // because v[i] = x[i]
    }

    for (int i = nrows - 1;  i >= 0;  --i) {
        r[i][i] = v[i].two_norm();

        if (debug_remove_dependent) {
            cerr << "i = " << i << " nrows = " << nrows << " r[i][i] = "
                 << r[i][i] << " v[i] = " << v[i] << " z[i] = "
                 << z[i] << endl;
        }

        if (r[i][i] < tolerance) {
            z[i][i] = 0.0;
            r[i][i] = 0.0;
            continue;  // linearly dependent
        }

        v[i] /= r[i][i];
        z[i] /= r[i][i];

        // Check that our v is orthogonal to all other vs
        if (check_remove_dependent || debug_remove_dependent) {
            bool any_errors = false;
            for (int i2 = nrows - 1;  i2 > i;  --i2) {
                double error = v[i2].dotprod(v[i]);
                if (error > tolerance) {
                    cerr << "error: between " << i << " and " << i2 << ": "
                         << error << endl;
                    if (error > tolerance * 10)
                        any_errors = true;
                }
            }
            if (any_errors) {
                cerr << __PRETTY_FUNCTION__ << endl;
                throw Exception("remove_dependent: not independent");
            }
        }

        bool any_errors = false;

        for (int j = i - 1;  j >= 0;  --j) {
            r[j][i] = v[i].dotprod(v[j]);
            SIMD::vec_add(&v[j][0], -r[j][i], &v[i][0], &v[j][0], ncols);
            SIMD::vec_add(&z[j][0], -r[j][i], &z[i][0], &z[j][0], nrows);

            // Check that v[i] and v[j] are now orthogonal
            if (check_remove_dependent || debug_remove_dependent) {
                double error = v[i].dotprod(v[j]);
                if (error > tolerance) {
                    cerr << "v[i] and v[j] aren't orthogonal to tolerance "
                         << tolerance << ": dot_product = "
                         << error << endl;
                    if (error > tolerance * 10)
                        any_errors = true;
                }
            }
        }

        if (any_errors) {
            cerr << __PRETTY_FUNCTION__ << endl;
            throw Exception("remove_dependent: not independent 2");
        }
    }

    if (debug_remove_dependent) {
        cerr << "finished remove_dependent" << endl;
        for (unsigned i = 0;  i < nrows;  ++i) {
            cerr << "i = " << i << " nrows = " << nrows << " r[i][i] = "
                 << r[i][i] << " v[i] = " << v[i] << " z[i] = "
                 << z[i] << endl;
        }
    }

    //cerr << "done gram schmidt" << endl;

    //for (unsigned i = 0;  i < nrows;  ++i) {
    //    cerr << "r[" << i << "] = " << r[i][i] << "  ";
    //}
    //cerr << endl;

    //for (unsigned i = 0;  i < nrows;  ++i) {
    //    cerr << "z[" << i << "] = " << z[i] << endl;
    //}

    /* Keep track of what is nonsingular, and where they come from. */
    vector<int> source;
    vector<int> dest(nrows, -1);  // -1 indicates it was left out
    for (unsigned i = 0;  i < nrows;  ++i) {
        if (r[i][i] >= tolerance) {
            /* Nonsingular. */
            dest[i] = source.size();
            source.push_back(i);
        }
        else {
            if (debug_remove_dependent) {
                cerr << "column " << i << " is dependent; r " << r[i]
                     << " v " << v[i] << endl;
            }
        }
    }

    if (debug_remove_dependent) {
        cerr << "source = " << source << endl;
        cerr << "dest   = " << dest << endl;
    }

    y.clear();
    for (unsigned i = 0;  i < nrows;  ++i) {
        distribution<FloatCalc> this_y(nrows, 0.0);
        for (unsigned j = 0;  j < nrows;  ++j)
            this_y += r[i][j] * z[j];
        //cerr << "y[" << i << "] = " << this_y << endl;
        y.push_back(this_y);
    }

    //cerr << "done y" << endl;

    unsigned nrows2 = source.size();

    //cerr << "nrows = " << nrows << " nrows2 = " << nrows2 << endl;

    if (nrows2 < nrows) {
        /* We have removed some columns, so we need to rearrange our
           matrix. */
        boost::multi_array<FloatIn, 2> x2(boost::extents[nrows2][ncols]);
        for (unsigned i = 0;  i < nrows2;  ++i)
            for (unsigned j = 0;  j < ncols;  ++j)
                x2[i][j] = x[source[i]][j];

        swap_multi_arrays(x, x2);

        /* Same for the y matrix.  We remove columns which are dependent. */
        vector<distribution<FloatCalc> > y2(nrows, distribution<FloatCalc>(nrows2, 0.0));
        for (unsigned i = 0;  i < nrows;  ++i)
            for (unsigned j = 0;  j < nrows2;  ++j)
                y2[i][j] = y[i][source[j]];
        y.swap(y2);
    }

    //cerr << "remove_dependent: " << nrows << "x" << ncols << ": "
    //     << t.elapsed() << "s" << endl;
    //cerr << "dest = " << dest << endl;

    //for (unsigned i = 0;  i < nrows;  ++i) {
    //    cerr << "y[" << i << "] = " << y[i] << endl;
    //}

    return dest;
}

template vector<int>
remove_dependent_impl(boost::multi_array<float, 2> & x,
                      std::vector<distribution<float> > & y,
                      double tolerance);
template vector<int>
remove_dependent_impl(boost::multi_array<float, 2> & x,
                      std::vector<distribution<double> > & y,
                      double tolerance);
template vector<int>
remove_dependent_impl(boost::multi_array<double, 2> & x,
                      std::vector<distribution<double> > & y,
                      double tolerance);

vector<int> remove_dependent(boost::multi_array<double, 2> & x)
{
    vector<distribution<double> > y;
    return remove_dependent_impl(x, y);
}

vector<int> remove_dependent(boost::multi_array<float, 2> & x)
{
    vector<distribution<float> > y;
    return remove_dependent_impl(x, y);
}

/*****************************************************************************/
/* REGRESSOR                                                                 */
/*****************************************************************************/

Regressor::
~Regressor()
{
}

Least_Squares_Regressor::
~Least_Squares_Regressor()
{
}

distribution<float>
Least_Squares_Regressor::
calc(const boost::multi_array<float, 2> & A,
     const distribution<float> & b) const
{
    return least_squares(A, b);
}

distribution<double>
Least_Squares_Regressor::
calc(const boost::multi_array<double, 2> & A,
     const distribution<double> & b) const
{
    return least_squares(A, b);
}

Ridge_Regressor::
Ridge_Regressor(double lambda)
    : lambda(lambda)
{
}

Ridge_Regressor::
~Ridge_Regressor()
{
}

distribution<float>
Ridge_Regressor::
calc(const boost::multi_array<float, 2> & A,
     const distribution<float> & b) const
{
    return ridge_regression(A, b, lambda);
}

distribution<double>
Ridge_Regressor::
calc(const boost::multi_array<double, 2> & A,
     const distribution<double> & b) const
{
    return ridge_regression(A, b, lambda);
}

const Regressor & default_regressor()
{
    static const Least_Squares_Regressor result;
    return result;
}

/*****************************************************************************/
/* IRLS                                                                      */
/*****************************************************************************/

template<class Float>
distribution<Float>
perform_irls_impl(const distribution<Float> & correct,
                  const boost::multi_array<Float, 2> & outputs,
                  const distribution<Float> & w,
                  Link_Function link_function,
                  bool ridge_regression)
{
    int nx = correct.size();
    int nv = outputs.shape()[0];

    if (outputs.shape()[1] != nx)
        throw Exception("wrong shape for outputs");

    bool verify = false;
    //verify = true;

    Timer timer;

    auto doneTimer = [&] (const std::string & what)
        {
            cerr << "  finished " << what << ": " << timer.elapsed() << endl;
            timer.restart();
        };

    distribution<Float> svalues1
        (std::min(outputs.shape()[0], outputs.shape()[1]) + 1);

    if (verify) {
        boost::multi_array<Float, 2> outputs2 = outputs;

        svalues1.push_back(0.0);

        int result = LAPack::gesdd("N",
                                   outputs2.shape()[1],
                                   outputs2.shape()[0],
                                   outputs2.data(), outputs2.shape()[1],
                                   &svalues1[0], 0, 1, 0, 1);

        if (result != 0)
            throw Exception("error in SVD");

        cerr << "svalues1 = " << svalues1 << endl;
        doneTimer("verify");
    }

    boost::multi_array<Float, 2> outputs3 = outputs;

    /* Factorize the matrix with partial pivoting.  This allows us to find the
       largest number of linearly independent columns possible. */

    distribution<Float> tau(nv, 0.0);
    distribution<int> permutations(nv, 0);

    int res = LAPack::geqp3(outputs3.shape()[1],
                            outputs3.shape()[0],
                            outputs3.data(), outputs3.shape()[1],
                            &permutations[0],
                            &tau[0]);

    doneTimer("geqp3");

    if (res != 0)
        throw Exception(format("geqp3: error in parameter %d", -res));

    // Convert back to c indexes
    permutations -= 1;

    //cerr << "permutations = " << permutations << endl;
    //cerr << "tau = " << tau << endl;

    distribution<Float> diag(nv);
    for (unsigned i = 0;  i < nv;  ++i)
        diag[i] = outputs3[i][i];

    //cerr << "diag = " << diag << endl;

    int nkeep = nv;
    while (nkeep > 0 && fabs(diag[nkeep - 1]) < 0.01) --nkeep;

    //cerr << "keeping " << nkeep << " of " << nv << endl;

    vector<int> new_loc(nv, -1);
    for (unsigned i = 0;  i < nkeep;  ++i)
        new_loc[permutations[i]] = i;

    boost::multi_array<Float, 2> outputs_reduced(boost::extents[nkeep][nx]);
    for (unsigned i = 0;  i < nx;  ++i)
        for (unsigned j = 0;  j < nv;  ++j)
            if (new_loc[j] != -1)
                outputs_reduced[new_loc[j]][i] = outputs[j][i];

    double svreduced = svalues1[nkeep - 1];

    //cerr << "svreduced = " << svreduced << endl;

    doneTimer("reduced");

    if (verify && svreduced < 0.001)
        throw Exception("not all linearly dependent columns were removed");

#if 0
    cerr << "w.size() = " << w.size() << endl;
    cerr << "correct.size() = " << correct.size() << endl;
    cerr << "w.total() = " << w.total() << endl;

    cerr << "outputs_reduced: " << outputs_reduced.shape()[0] << "x"
         << outputs_reduced.shape()[1] << endl;
#endif

    distribution<Float> trained;

    if (link_function == LINEAR && (w.min() == w.max())) {
        if (ridge_regression) {
            Ridge_Regressor regressor(1e-5);
            trained = regressor.calc(transpose(outputs_reduced), correct);
        }
        else {
            Least_Squares_Regressor regressor;
            trained = regressor.calc(transpose(outputs_reduced), correct);
        }
    }
    else {
        if (ridge_regression) {
            Ridge_Regressor regressor(1e-5);
            trained
                = run_irls(correct, outputs_reduced, w, link_function, regressor);
        }
        else {
            Least_Squares_Regressor regressor;
            trained
                = run_irls(correct, outputs_reduced, w, link_function, regressor);
        }
    }

    doneTimer("training");

    distribution<Float> parameters(nv);
    for (unsigned v = 0;  v < nv;  ++v)
        if (new_loc[v] != -1)
            parameters[v] = trained[new_loc[v]];

    if (abs(parameters).max() > 1000.0) {

        distribution<Float> svalues_reduced
            (std::min(outputs_reduced.shape()[0],
                      outputs_reduced.shape()[1]));

#if 0
        filter_ostream out(format("good.model.%d.txt.gz", model));

        out << "model " << model << endl;
        out << "trained " << trained << endl;
        out << "svalues_reduced " << svalues_reduced << endl;
        out << "parameters " << parameters << endl;
        out << "permuations " << permutations << endl;
        out << "svalues1 " << svalues1 << endl;
        out << "correct " << correct << endl;
        out << "outputs_reduced: " << outputs_reduced.shape()[0] << "x"
            << outputs_reduced.shape()[1] << endl;

        for (unsigned i = 0;  i < nx;  ++i) {
            out << "example " << i << ": ";
            for (unsigned j = 0;  j < nkeep;  ++j)
                out << " " << outputs_reduced[j][i];
            out << endl;
        }
#endif

        int result = LAPack::gesdd("N", outputs_reduced.shape()[1],
                                   outputs_reduced.shape()[0],
                                   outputs_reduced.data(),
                                   outputs_reduced.shape()[1],
                                   &svalues_reduced[0], 0, 1, 0, 1);

        doneTimer("gesdd");

        //cerr << "model = " << model << endl;
        cerr << "trained = " << trained << endl;
        cerr << "svalues_reduced = " << svalues_reduced << endl;

        cerr << "parameters.two_norm() = " << parameters.two_norm()
             << endl;

        if (result != 0)
            throw Exception("gesdd returned error");

        if (svalues_reduced.back() <= 0.001) {
            throw Exception("didn't remove all linearly dependent");
        }

        // We reject this later
        //if (abs(parameters).max() > 1000.0) {
        //    throw Exception("IRLS returned inplausibly high weights");
        //}

    }

    //cerr << "irls returned parameters " << parameters << endl;

    return parameters;
}

distribution<float>
perform_irls(const distribution<float> & correct,
             const boost::multi_array<float, 2> & outputs,
             const distribution<float> & w,
             Link_Function link_function,
             bool ridge_regression)
{
    return perform_irls_impl(correct, outputs, w, link_function,
                             ridge_regression);
}

distribution<double>
perform_irls(const distribution<double> & correct,
             const boost::multi_array<double, 2> & outputs,
             const distribution<double> & w,
             Link_Function link_function,
             bool ridge_regression)
{
    return perform_irls_impl(correct, outputs, w, link_function,
                             ridge_regression);
}

distribution<double>
irls_logit(const distribution<double> & correct,
           const boost::multi_array<double, 2> & outputs,
           const distribution<double> & w,
           const Regressor & regressor)
{
    return irls(correct, outputs, w, Logit_Link<double>(),
                Binomial_Dist<double>(), regressor);
}

distribution<double>
irls_log(const distribution<double> & correct,
         const boost::multi_array<double, 2> & outputs,
         const distribution<double> & w,
         const Regressor & regressor)
{
    return irls(correct, outputs, w, Logarithm_Link<double>(),
                Binomial_Dist<double>(), regressor);
}

distribution<double>
irls_linear(const distribution<double> & correct,
            const boost::multi_array<double, 2> & outputs,
            const distribution<double> & w,
            const Regressor & regressor)
{
    return irls(correct, outputs, w, Linear_Link<double>(),
                Normal_Dist<double>(), regressor);
}

distribution<double>
irls_probit(const distribution<double> & correct,
            const boost::multi_array<double, 2> & outputs,
            const distribution<double> & w,
            const Regressor & regressor)
{
    return irls(correct, outputs, w, Probit_Link<double>(),
                Binomial_Dist<double>(), regressor);
}

distribution<double>
irls_complog(const distribution<double> & correct,
             const boost::multi_array<double, 2> & outputs,
             const distribution<double> & w,
             const Regressor & regressor)
{
    return irls(correct, outputs, w, Comp_Log_Log_Link<double>(),
                Binomial_Dist<double>(), regressor);
}

distribution<double>
run_irls(const distribution<double> & correct,
         const boost::multi_array<double, 2> & outputs,
         const distribution<double> & w, Link_Function func,
         const Regressor & regressor)
{
    switch (func) {

    case LOGIT:
        return irls_logit(correct, outputs, w, regressor);

    case LOG:
        return irls_log(correct, outputs, w, regressor);

    case LINEAR:
        return irls_linear(correct, outputs, w, regressor);

    case PROBIT:
        return irls_probit(correct, outputs, w, regressor);

    case COMP_LOG_LOG:
        return irls_complog(correct, outputs, w, regressor);

    default:
        throw Exception(format("run_irls(): function %d "
                               "not implemented", func));
    }
}

distribution<float>
irls_logit(const distribution<float> & correct,
           const boost::multi_array<float, 2> & outputs,
           const distribution<float> & w,
           const Regressor & regressor)
{
    return irls(correct, outputs, w, Logit_Link<float>(),
                Binomial_Dist<float>(), regressor);
}

distribution<float>
irls_log(const distribution<float> & correct,
         const boost::multi_array<float, 2> & outputs,
         const distribution<float> & w,
         const Regressor & regressor)
{
    return irls(correct, outputs, w, Logarithm_Link<float>(),
                Binomial_Dist<float>(), regressor);
}

distribution<float>
irls_linear(const distribution<float> & correct,
            const boost::multi_array<float, 2> & outputs,
            const distribution<float> & w,
            const Regressor & regressor)
{
    return irls(correct, outputs, w, Linear_Link<float>(),
                Normal_Dist<float>(), regressor);
}

distribution<float>
irls_probit(const distribution<float> & correct,
            const boost::multi_array<float, 2> & outputs,
            const distribution<float> & w,
            const Regressor & regressor)
{
    return irls(correct, outputs, w, Probit_Link<float>(),
                Binomial_Dist<float>(), regressor);
}

distribution<float>
irls_complog(const distribution<float> & correct,
             const boost::multi_array<float, 2> & outputs,
             const distribution<float> & w,
             const Regressor & regressor)
{
    return irls(correct, outputs, w, Comp_Log_Log_Link<float>(),
                Binomial_Dist<float>(), regressor);
}

distribution<float>
run_irls(const distribution<float> & correct,
         const boost::multi_array<float, 2> & outputs,
         const distribution<float> & w, Link_Function func,
         const Regressor & regressor)
{
    switch (func) {

    case LOGIT:
        return irls_logit(correct, outputs, w, regressor);

    case LOG:
        return irls_log(correct, outputs, w, regressor);

    case LINEAR:
        return irls_linear(correct, outputs, w, regressor);

    case PROBIT:
        return irls_probit(correct, outputs, w, regressor);

    case COMP_LOG_LOG:
        return irls_complog(correct, outputs, w, regressor);

    default:
        throw Exception(format("run_irls(): function %d "
                               "not implemented", func));
    }
}

double apply_link_inverse(double val, Link_Function func)
{
    switch (func) {
    case LOGIT:
        return Logit_Link<double>::inverse(val);

    case LOG:
        return Logarithm_Link<double>::inverse(val);

    case LINEAR:
        return Linear_Link<double>::inverse(val);

    case PROBIT:
        return Probit_Link<double>::inverse(val);

    case COMP_LOG_LOG:
        return Comp_Log_Log_Link<double>::inverse(val);

    default:
        throw Exception("apply_irls: unknown link function");
    }
}

std::string print(Link_Function link)
{
    switch (link) {
    case LOGIT:        return "LOGIT";
    case LOG:          return "LOG";
    case LINEAR:       return "LINEAR";
    case PROBIT:       return "PROBIT";
    case COMP_LOG_LOG: return "COMP_LOG_LOG";
    default:           return format("Link_Function(%d)", link);
    }
}

Link_Function parse_link_function(const std::string & link_name)
{
    Link_Function link;

    if (lowercase(link_name) == "logit")
        link = LOGIT;
    else if (lowercase(link_name) == "log")
        link = LOG;
    else if (lowercase(link_name) == "linear")
        link = LINEAR;
    else if (lowercase(link_name) == "probit")
        link = PROBIT;
    else if (lowercase(link_name) == "comploglog")
        link = COMP_LOG_LOG;
    else throw Exception("parse_link_function(): link function '"
                         + link_name + "' is not known ('logit', 'log', "
                         "'linear', 'probit', 'comploglog' accepted)");
    return link;
}

/** Put a link function in a stream. */
std::ostream & operator << (std::ostream & stream, Link_Function link)
{
    return stream << print(link);
}

COMPACT_PERSISTENT_ENUM_IMPL(Link_Function);
} // namespace ML

ENUM_INFO_NAMESPACE

const Enum_Opt<ML::Link_Function>
Enum_Info<ML::Link_Function>::OPT[5] = {
    { "logit",        ML::LOGIT          },
    { "probit",       ML::PROBIT         },
    { "comp_log_log", ML::COMP_LOG_LOG   },
    { "linear",       ML::LINEAR         },
    { "log",          ML::LOG            } };

const char * Enum_Info<ML::Link_Function>::NAME
   = "Link_Function";

END_ENUM_INFO_NAMESPACE


/*
  Transport PK

  Copyright 2010-201x held jointly by LANS/LANL, LBNL, and PNNL.
  Amanzi is released under the three-clause BSD License.
  The terms of use and "as is" disclaimer for this license are
  provided in the top-level COPYRIGHT file.

  Author: Konstantin Lipnikov (lipnikov@lanl.gov)
*/

// TPLs
#include <boost/algorithm/string.hpp>

#include "Transport_PK.hh"

namespace Amanzi {
namespace Transport {

/* *******************************************************************
* Re-partition components between liquid and gas phases.
******************************************************************* */
void Transport_PK::PrepareAirWaterPartitioning_()
{
  henry_law_ = true;
  for (int i = 0; i < num_gaseous; i++) {
    int ig = num_aqueous + i;
    std::string name_l = boost::replace_all_copy(component_names_[ig], "(g)", "(l)");

    int il = FindComponentNumber(name_l);
    air_water_map_.push_back(il);

    if (il < 0 || il >= num_aqueous) {
      henry_law_ = false;
      if (vo_->getVerbLevel() >= Teuchos::VERB_MEDIUM) {
        Teuchos::OSTab tab = vo_->getOSTab();
        *vo_->os() << "Gas component \"" << component_names_[ig]
                   << "\" has no matching liquid component \"" << name_l << "\"\n";
      }
      break;
    }
  }

  if (henry_law_) {
    Teuchos::Array<double> empty;
    kH_ = tp_list_->sublist("molecular diffusion")
        .get<Teuchos::Array<double> >("air-water partitioning coefficient", empty).toVector();
  } else {
    air_water_map_.clear();
  }
}

/* *******************************************************************
* Re-partition components between liquid and gas phases.
******************************************************************* */
void Transport_PK::MakeAirWaterPartitioning_()
{
  Epetra_MultiVector& tcc_c = *tcc_tmp->ViewComponent("cell", false);
  const Epetra_MultiVector& sat_l = *ws;

  int ig, il;
  double sl, total;
  for (int i = 0; i < num_gaseous; ++i) {
    ig = num_aqueous + i;
    il = air_water_map_[i];

    for (int c = 0; c < ncells_owned; c++) {
      sl = sat_l[0][c];
      total = tcc_c[il][c] * sl + tcc_c[ig][c] * (1.0 - sl);
      tcc_c[ig][c] = total / (1.0 + (kH_[i] - 1.0) * sl);
      tcc_c[il][c] = tcc_c[ig][c] * kH_[i];
    }
  }
}

}  // namespace Transport
}  // namespace Amanzi


//  ================================================================
//  Created by Gregory Kramida on 10/9/18.
//  Copyright (c) 2018 Gregory Kramida
//  Licensed under the Apache License, Version 2.0 (the "License");
//  you may not use this file except in compliance with the License.
//  You may obtain a copy of the License at

//  http://www.apache.org/licenses/LICENSE-2.0

//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
//  ================================================================
#pragma once
//stdlib

//libraries
#include <Eigen/Eigen>
#include <boost/python.hpp>
#include <unsupported/Eigen/NumericalDiff>

//local
#include "../../math/typedefs.hpp"

namespace bp = boost::python;
namespace eig = Eigen;

namespace nonrigid_optimization {
namespace slavcheva{

void compute_data_term_gradient(
		math::MatrixXv2f& data_term_gradient, float& data_term_energy,
		const eig::MatrixXf& warped_live_field, const eig::MatrixXf& canonical_field,
		const math::MatrixXv2f& warped_live_field_gradient,
		float scaling_factor = 10.0f);

void compute_data_term_gradient_within_band_union(
		math::MatrixXv2f& data_term_gradient, float& data_term_energy,
		const eig::MatrixXf& warped_live_field, const eig::MatrixXf& canonical_field,
		const math::MatrixXv2f& warped_live_field_gradient,
		float scaling_factor = 10.0f);

void compute_local_data_term_gradient(const eig::MatrixXf& warped_live_field, const eig::MatrixXf& canonical_field,
                                      int x, int y,
                                      const eig::MatrixXf& live_gradient_x_field,
                                      const eig::MatrixXf& live_gradient_y_field,
                                      float& data_gradient_x, float& data_gradient_y, float& local_energy_contribution);
bp::tuple py_data_term_at_location(eig::MatrixXf warped_live_field, eig::MatrixXf canonical_field, int i_col, int i_row,
                                   eig::MatrixXf live_gradient_x, eig::MatrixXf live_gradient_y);
} //namespace slavcheva
}//namespace nonrigid_optimization


#ifndef RNDCMP_INCLUDE_BFLOAT16_HPP_
#define RNDCMP_INCLUDE_BFLOAT16_HPP_

#include <type_traits>
#include <iostream>
#include <cstring>
#include <cmath>
#include <Eigen/Core>

namespace rndcmp {
    class bfloat16 {
    public:
        bfloat16(): bfloat16(0.0f) {};

        bfloat16(float rhs): value(round(rhs)) {};

        bfloat16& operator=(const bfloat16& rhs) = default;

        bfloat16& operator=(float rhs) { value = round(rhs); return *this; }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        operator T() const {
            int32_t shifted_val = value << 16;
            float float_val = reinterpret_cast<float&>(shifted_val);
            return static_cast<T>(float_val);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        operator T() const {
            int32_t shifted_val = value << 16;
            float float_val = reinterpret_cast<float&>(shifted_val);
            return static_cast<T>(float_val);
        }

        /* + operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bfloat16 operator+(const T& rhs) const {
            float v = static_cast<float>(*this) + static_cast<float>(rhs);
            return bfloat16(v);
        }

        bfloat16 operator+(const bfloat16& rhs) const {
            float v = static_cast<float>(*this) + static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bfloat16 operator+(T lhs, const bfloat16& rhs) {
            float v = static_cast<float>(lhs) + static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bfloat16& operator+=(const T& rhs) {
            float v = static_cast<float>(*this) + static_cast<float>(rhs);
            value = round(v);
            return *this;
        }

        bfloat16& operator+=(const bfloat16& rhs) {
            float v = static_cast<float>(*this) + static_cast<float>(rhs);
            value = round(v);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bfloat16 operator+(const T& rhs) const {
            float v = static_cast<float>(*this) + rhs;
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bfloat16 operator+(T lhs, const bfloat16& rhs) {
            float v = lhs + static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bfloat16& operator+=(const T& rhs) {
            float v = static_cast<float>(*this) + rhs;
            value = round(v);
            return *this;
        }

        /* - operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bfloat16 operator-(const T& rhs) const {
            float v = static_cast<float>(*this) - static_cast<float>(rhs);
            return bfloat16(v);
        }

        bfloat16 operator-(const bfloat16& rhs) const {
            float v = static_cast<float>(*this) - static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bfloat16& operator-=(const T& rhs) {
            float v = static_cast<float>(*this) - static_cast<float>(rhs);
            value = round(v);
            return *this;
        }

        bfloat16& operator-=(const bfloat16& rhs) {
            float v = static_cast<float>(*this) - static_cast<float>(rhs);
            value = round(v);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bfloat16 operator-(T lhs, const bfloat16& rhs) {
            float v = static_cast<float>(lhs) - static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bfloat16 operator-(const T& rhs) const {
            float v = static_cast<float>(*this) - static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bfloat16 operator-(T lhs, const bfloat16& rhs) {
            float v = static_cast<float>(lhs) - static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bfloat16& operator-=(const T& rhs) {
            float v = static_cast<float>(*this) - rhs;
            value = round(v);
            return *this;
        }

        // Unary minus
        bfloat16 operator-() const {
            float v = - static_cast<float>(*this);
            return bfloat16(v);
        }

        /* multiply operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bfloat16 operator*(const T& rhs) const {
            float v = static_cast<float>(*this) * static_cast<float>(rhs);
            return bfloat16(v);
        }

        bfloat16 operator*(const bfloat16& rhs) const {
            float v = static_cast<float>(*this) * static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bfloat16& operator*=(const T& rhs) {
            float v = static_cast<float>(*this) * static_cast<float>(rhs);
            value = round(v);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bfloat16 operator*(T lhs, const bfloat16& rhs) {
            float v = static_cast<float>(lhs) * static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bfloat16 operator*(const T& rhs) const {
            float v = static_cast<float>(*this) * rhs;
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bfloat16 operator*(T lhs, const bfloat16& rhs) {
            float v = lhs * static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bfloat16& operator*=(const T& rhs) {
            float v = static_cast<float>(*this) * rhs;
            value = round(v);
            return *this;
        }

        bfloat16& operator*=(const bfloat16& rhs) {
            float v = static_cast<float>(*this) * static_cast<float>(rhs);
            value = round(v);
            return *this;
        }

        /* divide operators */

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bfloat16 operator/(const T& rhs) const {
            float v = static_cast<float>(*this) / static_cast<float>(rhs);
            return bfloat16(v);
        }

        bfloat16 operator/(const bfloat16& rhs) const {
            float v = static_cast<float>(*this) / static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bfloat16& operator/=(const T& rhs) {
            float v = static_cast<float>(*this) / static_cast<float>(rhs);
            value = round(v);
            return *this;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bfloat16 operator/(T lhs, const bfloat16& rhs) {
            float v = static_cast<float>(lhs) / static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bfloat16 operator/(const T& rhs) const {
            float v = static_cast<float>(*this) / rhs;
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bfloat16 operator/(T lhs, const bfloat16& rhs) {
            float v = lhs / static_cast<float>(rhs);
            return bfloat16(v);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bfloat16& operator/=(const T& rhs) {
            float v = static_cast<float>(*this) / rhs;
            round(v);
            return *this;
        }

        bfloat16& operator/=(const bfloat16& rhs) {
            float v = static_cast<float>(*this) / static_cast<float>(rhs);
            value = round(v);
            return *this;
        }

        /* comparison operators */

        /* less */

        bool operator<(const bfloat16& rhs) const {
            return value < rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator<(const T& rhs) const {
            return static_cast<T>(*this) < rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator<(const T& rhs) const {
            return static_cast<T>(*this) < rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator<(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) < static_cast<float>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator<(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) < static_cast<float>(lhs);
        }

        /* greater */

        bool operator>(const bfloat16& rhs) const {
            return value > rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator>(const T& rhs) const {
            return static_cast<T>(*this) > rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator>(const T& rhs) const {
            return static_cast<T>(*this) > rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator>(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) > static_cast<float>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator>(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) > static_cast<float>(lhs);
        }

        /* equal */

        bool operator==(const bfloat16& rhs) const {
            return value == rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator==(const T& rhs) const {
            return static_cast<T>(*this) == rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator==(const T& rhs) const {
            return static_cast<T>(*this) == rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator==(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) == static_cast<float>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator==(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) == static_cast<float>(lhs);
        }

        /* not equal */

        bool operator!=(const bfloat16& rhs) const {
            return value != rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator!=(const T& rhs) const {
            return static_cast<T>(*this) != rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator!=(const T& rhs) const {
            return static_cast<T>(*this) != rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator!=(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) != static_cast<float>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator!=(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) != static_cast<float>(lhs);
        }

        /* less or equal */

        bool operator<=(const bfloat16& rhs) const {
            return value <= rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator<=(const T& rhs) const {
            return static_cast<T>(*this) <= rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator<=(const T& rhs) const {
            return static_cast<T>(*this) <= rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator<=(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) <= static_cast<float>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator<=(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) <= static_cast<float>(lhs);
        }

        /* greater or equal */

        bool operator>=(const bfloat16& rhs) const {
            return value >= rhs.value;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        bool operator>=(const T& rhs) const {
            return static_cast<T>(*this) >= rhs;
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        bool operator>=(const T& rhs) const {
            return static_cast<T>(*this) >= rhs;
        }

        template<typename T, std::enable_if_t<std::is_floating_point_v<T>, int> = 0>
        friend bool operator>=(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) >= static_cast<float>(lhs);
        }

        template<typename T, std::enable_if_t<std::is_integral_v<T>, int> = 0>
        friend bool operator>=(const T& rhs, const bfloat16& lhs) {
            return static_cast<float>(rhs) >= static_cast<float>(lhs);
        }

        /* ostream overload */
        friend std::ostream& operator<<(std::ostream& os, const bfloat16& v) {
            os << float(v);
            return os;
        }

        /* Trigonometric functions */
        friend inline bfloat16 cos(const bfloat16&  x)  { return cos(static_cast<float>(x)); }
        friend inline bfloat16 sin(const bfloat16&  x)  { return sin(static_cast<float>(x)); }
        friend inline bfloat16 tan(const bfloat16&  x)  { return tan(static_cast<float>(x)); }
        friend inline bfloat16 acos(const bfloat16&  x)  { return acos(static_cast<float>(x)); }
        friend inline bfloat16 asin(const bfloat16&  x)  { return asin(static_cast<float>(x)); }
        friend inline bfloat16 atan(const bfloat16&  x)  { return atan(static_cast<float>(x)); }

        /* Hyperbolic functions */
        friend inline bfloat16 cosh(const bfloat16&  x)  { return cosh(static_cast<float>(x)); }
        friend inline bfloat16 sinh(const bfloat16&  x)  { return sinh(static_cast<float>(x)); }
        friend inline bfloat16 tanh(const bfloat16&  x)  { return tanh(static_cast<float>(x)); }
        friend inline bfloat16 acosh(const bfloat16&  x)  { return acosh(static_cast<float>(x)); }
        friend inline bfloat16 asinh(const bfloat16&  x)  { return asinh(static_cast<float>(x)); }
        friend inline bfloat16 atanh(const bfloat16&  x)  { return atanh(static_cast<float>(x)); }

        /* Exponential and logarithmic functions */
        friend inline bfloat16 exp(const bfloat16&  x)  { return exp(static_cast<float>(x)); }
        friend inline bfloat16 log(const bfloat16&  x)  { return log(static_cast<float>(x)); }
        friend inline bfloat16 log10(const bfloat16&  x)  { return log10(static_cast<float>(x)); }
        friend inline bfloat16 logb(const bfloat16&  x)  { return logb(static_cast<float>(x)); }

        /* Power functions */
        friend inline bfloat16 pow(const bfloat16&  base, double exponent)  { return pow(static_cast<float>(base), exponent); }
        friend inline bfloat16 sqrt(const bfloat16&  x)  { return sqrt(static_cast<float>(x)); }
        friend inline bfloat16 cbrt(const bfloat16&  x)  { return cbrt(static_cast<float>(x)); }

        friend inline bfloat16 scalbn(const bfloat16&  x, int n)  { return scalbn(static_cast<float>(x), n); }

        /* Other functions */
        friend inline bfloat16 abs(const bfloat16&  x)  { return abs(static_cast<float>(x)); }
        friend inline bfloat16 fabs(const bfloat16&  x)  { return fabs(static_cast<float>(x)); }
        friend inline bfloat16 abs2(const bfloat16& x)  { return x*x; }

        friend inline bfloat16 copysign(const bfloat16&  x1, const bfloat16& x2)  { return copysign(static_cast<double>(x1), static_cast<double>(x2)); }
        friend inline bfloat16 fmax(const bfloat16&  x1, const bfloat16&  x2) { return x1 < x2 ? x1 : x2; }
        friend inline bool isfinite(const bfloat16& x) { return true; }

    protected:
        int16_t round(float rhs) {
            int16_t val = 0;

            // Magic value for rounding to nearest (simply just push bottom bits into the upper)
            rhs *= 1.001957f;
            #if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
                memcpy(&val, &rhs, sizeof(val));
            #else
                memcpy(&val, reinterpret_cast<char *>(&rhs) + sizeof(rhs) - sizeof(val), sizeof(val));
            #endif
            return val;
        }

        int16_t value;
    };
}

namespace Eigen {
    // Inheritance from float is a temporary bad solution. Need specify all NumTraits explicitly
    template<> struct NumTraits<rndcmp::bfloat16>: NumTraits<float> {
        typedef rndcmp::bfloat16 Real;
        typedef rndcmp::bfloat16 NonInteger;
        typedef rndcmp::bfloat16 Nested;

        enum {
            IsComplex = 0,
            IsInteger = 0,
            IsSigned = 1,
            RequireInitialization = 1,
            ReadCost = 2,
            AddCost = 3,
            MulCost = 3
        };
    };
}

#endif  // RNDCMP_INCLUDE_BFLOAT16_HPP_


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     isovist_maxima.cpp
 * \author   Collin Johnson
 *
 * Definition of VoronoiIsovistMaxima.
 */

#include "hssh/local_topological/area_detection/gateways/isovist_maxima.h"
#include "hssh/local_topological/area_detection/local_topo_isovist_field.h"
#include "hssh/local_topological/area_detection/voronoi/voronoi_edges.h"
#include "hssh/local_topological/voronoi_skeleton_grid.h"
#include "math/savitzky_golay.h"
#include "math/statistics.h"
#include "utils/algorithm_ext.h"
#include <boost/range/algorithm_ext.hpp>
#include <boost/range/as_array.hpp>
#include <boost/range/iterator_range.hpp>
#include <cassert>
#include <fstream>
#include <iostream>
#include <numeric>
#include <sstream>

// #define DEBUG_VISIBLE_UNITS

namespace vulcan
{
namespace hssh
{

isovist_local_maximum_t create_local_maximum(CellConstIter begin,
                                             CellConstIter end,
                                             CellConstIter beginEdgeCells,
                                             CellConstIter endEdgeCells,
                                             const VoronoiIsovistGradients& gradients,
                                             const VoronoiSkeletonGrid& skeleton);

VoronoiIsovistMaxima::VoronoiIsovistMaxima(const VoronoiIsovistGradients& gradients,
                                           const VoronoiEdges& edges,
                                           const VoronoiSkeletonGrid& grid,
                                           int numAboveMean,
                                           bool saveData)
: numAboveMean_(numAboveMean)
, saveData_(saveData)
, gradients_(&gradients)
, grid_(&grid)
{
    for (auto& e : edges) {
        if (e.size() >= 5) {
            calculateEdgeDerivatives(e.begin(), e.end());
            findMaximaAlongEdge(e.begin(), e.end(), 2);
        }
    }
}

void VoronoiIsovistMaxima::calculateEdgeDerivatives(CellConstIter edgeBegin, CellConstIter edgeEnd)
{
    edgeGradients_.clear();
    edgeDerivs_.clear();

    std::transform(edgeBegin, edgeEnd, std::back_inserter(edgeGradients_), [this](cell_t cell) {
        return gradients_->gradientAt(cell).value;
    });

    edgeDerivs_.resize(edgeGradients_.size());

    if (edgeGradients_.size() >= 7) {
        math::savitzky_golay_first_deriv(edgeGradients_.begin(),
                                         edgeGradients_.end(),
                                         edgeDerivs_.begin(),
                                         math::SGWindowRadius::three);
    } else if (edgeGradients_.size() >= 5) {
        math::savitzky_golay_first_deriv(edgeGradients_.begin(),
                                         edgeGradients_.end(),
                                         edgeDerivs_.begin(),
                                         math::SGWindowRadius::two);
    } else {
        std::adjacent_difference(edgeGradients_.begin(), edgeGradients_.end(), edgeDerivs_.begin());
        edgeDerivs_[0] = 0.0;
    }

    // Save the original data
    if (saveData_) {
        std::ostringstream file;
        file << "values_" << edgeBegin->x << '_' << (edgeEnd - 1)->x << ".dat";
        std::ofstream out(file.str());

        int index = 0;
        for (auto cell : boost::make_iterator_range(edgeBegin, edgeEnd)) {
            out << index << ' ' << gradients_->gradientAt(cell).data << '\n';
            ++index;
        }
    }

    // Save the gradient
    if (saveData_) {
        std::ostringstream file;
        file << "gradients_" << edgeBegin->x << '_' << (edgeEnd - 1)->x << ".dat";
        std::ofstream out(file.str());

        int index = 0;
        for (auto& diff : edgeGradients_) {
            out << index << ' ' << diff << '\n';
            ++index;
        }
    }

    // Save the derivate of the gradient
    if (saveData_) {
        std::ostringstream file;
        file << "deriv_" << edgeBegin->x << '_' << (edgeEnd - 1)->x << ".dat";
        std::ofstream out(file.str());

        int index = 0;
        for (auto& diff : edgeDerivs_) {
            out << index << ' ' << diff << '\n';
            ++index;
        }
    }
}

void VoronoiIsovistMaxima::findMaximaAlongEdge(CellConstIter edgeBegin, CellConstIter edgeEnd, int descendingCutoff)
{
    // Need the start index for saving the data later, as only want to incorporate maxima added on this update
    std::size_t maximaStartIndex = maxima_.size();

    std::vector<double> absGradients(std::distance(edgeBegin, edgeEnd));
    std::transform(edgeGradients_.begin(), edgeGradients_.end(), absGradients.begin(), [](double val) {
        return std::abs(val);
    });

    const double meanGradient = math::mean(absGradients.begin(), absGradients.end());

    int maximaStart = 0;
    int maximaEnd = 0;
    bool haveOppositeFlip = false;
    bool haveSameFlip = false;

    // The gradients are always positive as calculated. Thus, the start of a change will in the derivative will always
    // be positive to start.
    for (std::size_t n = 0; n < edgeGradients_.size(); ++n) {
        if (std::signbit(edgeDerivs_[n]) != std::signbit(edgeDerivs_[n - 1])) {
            if (std::signbit(edgeDerivs_[n]) == std::signbit(edgeGradients_[n])) {
                haveSameFlip = true;
            } else   // signs are different
            {
                haveOppositeFlip = true;
                haveSameFlip = false;
            }
        }

        // If increasing again and was already increasing and decreasing, then end of change region has been found
        bool isGradientEnd =
          std::signbit(edgeGradients_[n]) != std::signbit(edgeGradients_[n - 1]) || (haveSameFlip && haveOppositeFlip);
        bool isEndOfEdge = n + 1 == edgeGradients_.size();
        if ((isGradientEnd || isEndOfEdge)) {
            maximaEnd = isEndOfEdge ? n + 1 : n;

            int outlierCount = std::count_if(absGradients.begin() + maximaStart,
                                             absGradients.begin() + maximaEnd,
                                             [&meanGradient](double gradient) {
                                                 return gradient > meanGradient;
                                             });

            if (outlierCount >= numAboveMean_) {
                maxima_.push_back(create_local_maximum(edgeBegin + maximaStart,
                                                       edgeBegin + maximaEnd,
                                                       edgeBegin,
                                                       edgeEnd,
                                                       *gradients_,
                                                       *grid_));
            }

            maximaStart = maximaEnd;
            haveSameFlip = false;
            haveOppositeFlip = false;
        }
    }

    if (saveData_) {
        std::ostringstream file;
        file << "maxima_" << edgeBegin->x << '_' << (edgeEnd - 1)->x << ".dat";
        std::ofstream out(file.str());

        for (auto& maximum : boost::make_iterator_range(maxima_.begin() + maximaStartIndex, maxima_.end())) {
            int index = std::distance(edgeBegin, std::find(edgeBegin, edgeEnd, maximum.maximum.position));
            out << index << ' ' << maximum.totalChange << '\n';
        }
    }
}

isovist_local_maximum_t create_local_maximum(CellConstIter begin,
                                             CellConstIter end,
                                             CellConstIter beginEdgeCells,
                                             CellConstIter endEdgeCells,
                                             const VoronoiIsovistGradients& gradients,
                                             const VoronoiSkeletonGrid& skeleton)
{
    assert(std::distance(begin, end) > 1);

    // Find all source cells for the maxima skeleton cells and count the gradient
    isovist_local_maximum_t maximum;
    maximum.totalChange = 0.0;

    CellSet maximaSources;
    for (auto cell : boost::make_iterator_range(begin, end)) {
        maximaSources.insert(skeleton.beginSourceCells(cell), skeleton.endSourceCells(cell));

        maximum.skeletonCells.emplace_back(gradients.gradientAt(cell));
        maximum.totalChange += std::abs(maximum.skeletonCells.back().value);
    }

    std::sort(maximum.skeletonCells.begin(),
              maximum.skeletonCells.end(),
              [](position_value_t lhs, position_value_t rhs) {
                  return std::abs(lhs.value) > std::abs(rhs.value);
              });

    maximum.maximum = maximum.skeletonCells.front();

    // For each cell along the edge, if it shares all of the source cells, then it should be added to the cells for
    // the maxima, but not counted towards the total value

    for (auto edgeCell : boost::make_iterator_range(beginEdgeCells + 1, endEdgeCells - 1)) {
        // Only add cells that are not along the edge. Both edgeCells and maxima cells are sorted so they iterate in
        // the same order
        if (edgeCell == *begin) {
            ++begin;
            continue;
        }

        bool hasMaximaSource = true;

        for (auto source :
             boost::make_iterator_range(skeleton.beginSourceCells(edgeCell), skeleton.endSourceCells(edgeCell))) {
            hasMaximaSource &= maximaSources.find(source) != maximaSources.end();
        }

        if (hasMaximaSource) {
            maximum.skeletonCells.emplace_back(gradients.gradientAt(edgeCell));
        }
    }

    return maximum;
}

}   // namespace hssh
}   // namespace vulcan


/**
 * Copyright Soramitsu Co., Ltd. 2018 All Rights Reserved.
 * http://soramitsu.co.jp
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "utils/amount_utils.hpp"

#include <boost/format.hpp>

#include "builders/default_builders.hpp"
#include "builders/protobuf/common_objects/proto_amount_builder.hpp"

namespace shared_model {
  namespace detail {
    const boost::multiprecision::uint256_t ten = 10;

    boost::multiprecision::uint256_t increaseValuePrecision(
        boost::multiprecision::uint256_t value, int degree) {
      return value * pow(ten, degree);
    }

    /**
     * Sums up two amounts.
     * Result is returned
     * @param a left term
     * @param b right term
     */
    iroha::expected::PolymorphicResult<shared_model::interface::Amount,
                                       std::string>
    operator+(const shared_model::interface::Amount &a,
              const shared_model::interface::Amount &b) {
      auto max_precision = std::max(a.precision(), b.precision());
      auto val_a =
          increaseValuePrecision(a.intValue(), max_precision - a.precision());
      auto val_b =
          increaseValuePrecision(b.intValue(), max_precision - b.precision());
      if (val_a < a.intValue() || val_b < b.intValue() || val_a + val_b < val_a
          || val_a + val_b < val_b) {
        return iroha::expected::makeError(std::make_shared<std::string>(
            (boost::format("addition overflows (%s + %s)") % a.intValue().str()
             % b.intValue().str())
                .str()));
      }
      return shared_model::builder::AmountBuilderWithoutValidator()
          .precision(max_precision)
          .intValue(val_a + val_b)
          .build();
    }

    /**
     * Subtracts two amounts.
     * Result is returned
     * @param a left term
     * @param b right term
     */
    iroha::expected::PolymorphicResult<shared_model::interface::Amount,
                                       std::string>
    operator-(const shared_model::interface::Amount &a,
              const shared_model::interface::Amount &b) {
      // check if a greater than b
      if (a.intValue() < b.intValue()) {
        return iroha::expected::makeError(std::make_shared<std::string>(
            (boost::format("minuend is smaller than subtrahend (%s - %s)")
             % a.intValue().str() % b.intValue().str())
                .str()));
      }
      auto max_precision = std::max(a.precision(), b.precision());
      auto val_a =
          increaseValuePrecision(a.intValue(), max_precision - a.precision());
      auto val_b =
          increaseValuePrecision(b.intValue(), max_precision - b.precision());
      if (val_a < a.intValue() || val_b < b.intValue()) {
        return iroha::expected::makeError(
            std::make_shared<std::string>("new precision overflows number"));
      }
      return shared_model::builder::AmountBuilderWithoutValidator()
          .precision(max_precision)
          .intValue(val_a - val_b)
          .build();
    }

    /**
     * Make amount with bigger precision
     * Result is returned
     * @param a amount
     * @param b right term
     */
    iroha::expected::PolymorphicResult<shared_model::interface::Amount,
                                       std::string>
    makeAmountWithPrecision(const shared_model::interface::Amount &amount,
                            const int new_precision) {
      if (amount.precision() > new_precision) {
        return iroha::expected::makeError(std::make_shared<std::string>(
            (boost::format("new precision is smaller than current (%d < %d)")
             % new_precision % amount.precision())
                .str()));
      }
      auto val_amount = increaseValuePrecision(
          amount.intValue(), new_precision - amount.precision());
      if (val_amount < amount.intValue()) {
        return iroha::expected::makeError(
            std::make_shared<std::string>("operation overflows number"));
      }
      return shared_model::builder::AmountBuilderWithoutValidator()
          .precision(new_precision)
          .intValue(val_amount)
          .build();
    }

    int compareAmount(const shared_model::interface::Amount &a,
                      const shared_model::interface::Amount &b) {
      if (a.precision() == b.precision()) {
        return (a.intValue() < b.intValue())
            ? -1
            : (a.intValue() > b.intValue()) ? 1 : 0;
      }
      // when different precisions transform to have the same scale
      auto max_precision = std::max(a.precision(), b.precision());

      auto val1 =
          increaseValuePrecision(a.intValue(), max_precision - a.precision());
      auto val2 =
          increaseValuePrecision(b.intValue(), max_precision - b.precision());
      return (val1 < val2) ? -1 : (val1 > val2) ? 1 : 0;
    }
  }  // namespace detail
}  // namespace shared_model


// Header
#include <polyvec/curve-tracer/curve_solver.hpp>
#include <polyvec/core/log.hpp>

// Eigen
#include <Eigen/Core>
#include <Eigen/Jacobi>

// Polyvec
#include <polyvec/api.hpp>
#include <polyvec/utils/union-find.hpp>

#include <nse/MathematicaFormatter.h>

#include <fstream>
#include <iostream>
#include <iomanip>

#define PRINT_JACOBIAN 0
#define PRINT_ITERATION_STATS 0

#define ENABLE_DERIVATIVE_CHECK 0

namespace polyvec {
// ============= EIGEN INTERFACE FOR MARQUART_LEVENBERG
int
GlobFitter::inputs() {
    return n_parameters();
}

int
GlobFitter::values() {
    return n_equations();
}

int
GlobFitter::operator() ( const Eigen::VectorXd& x, Eigen::VectorXd& fvec ) {
    const double tol = 1e-10;

    if ( ( _cached_params.size() ==0 ) || ( ( _cached_params - x ).norm() > tol ) ) {
        set_variables ( x );
        _cached_params = x;
        compute_objective_and_jacobian ( _cached_objective, _cached_jacobian );
    }

    fvec = _cached_objective;
    return 0;
}

int
GlobFitter::df ( const Eigen::VectorXd& x, Eigen::SparseMatrix<double>& fjac ) {
    const double tol = 1e-10;

    if ( ( _cached_params.size() ==0 ) || ( ( _cached_params - x ).norm() > tol ) ) {
        set_variables ( x );
        _cached_params = x;
        compute_objective_and_jacobian ( _cached_objective, _cached_jacobian );
    }

    fjac = _cached_jacobian;
    return 0;
}

// Separate file as it takes ages to compile.
int
GlobFitter::run_fitter ( FILE* log_file, std::function<void ( int ) > callback ) {
    assert_break ( _is_setup ); //

    Eigen::VectorXd x0 = get_variables();
	set_variables(x0); //accomodate for fixed parameters
    Eigen::VectorXd x = x0;

	if (n_parameters() == 0 || n_equations() == 0)
		return 0;

    // Init Eigen
    Eigen::LevenbergMarquardt<GlobFitter> lm ( *this );
	lm.setGtol(1e-5);
	lm.setXtol(1e-5);
	lm.setFtol(1e-5);
    using Eigen::LevenbergMarquardtSpace::Running;
    using Eigen::LevenbergMarquardtSpace::Status;

    Status status = lm.minimizeInit ( x );

    int iter_id = 0;

    if ( log_file ) {
        fprintf ( log_file, " ==== MARQURT-LEVENBERG for Bezier Fitting ===== \n" );
		fprintf(log_file, " %i equations, %i degrees of freedom\n", n_equations(), n_parameters());
        fprintf ( log_file, " %10s %20s %20s \n", "ITER", "OBJ", "GRAD" );
        fflush ( log_file );
    }

#if PRINT_ITERATION_STATS
	printf("------------------------\n");
#endif

    bool keep_running = true;

	while (keep_running && iter_id < _max_iterations) {
        status = lm.minimizeOneStep ( x );

#if ENABLE_DERIVATIVE_CHECK
		check_derivatives();
#endif

#if PRINT_ITERATION_STATS
		std::cout << lm.gnorm() << "  " << lm.fnorm() * lm.fnorm() << " " << status << std::endl;
#endif
        if ( log_file ) {
            if ( ( iter_id % 5 == 0 ) || ( status != Running ) ) {
                fprintf ( log_file, " %10d %20.8e %20.8e \n",
                         iter_id,
                         _cached_objective.norm(),
                         ( _cached_jacobian.transpose() * _cached_objective ).norm() );
                fflush ( log_file );
            }
        }

        ++iter_id;

        if ( callback ) {
            callback ( iter_id );
        }

        if ( status != Running ) {
            keep_running = false;
        } else if ( iter_id >= _max_iterations ) {
			if (log_file) {
				fprintf(log_file, " %10d %20.8e %20.8e \n",
					iter_id,
					_cached_objective.norm(),
					(_cached_jacobian.transpose() * _cached_objective).norm());
				fflush(log_file);
			}
        }

    }

    if ( log_file ) {
        switch ( status ) {
#define TAKECAREOF(XX) \
  case(Eigen::LevenbergMarquardtSpace::XX): fprintf(log_file, "FINISHED, Status: %s \n", #XX); break

            TAKECAREOF ( NotStarted );
            TAKECAREOF ( Running );
            TAKECAREOF ( ImproperInputParameters );
            TAKECAREOF ( RelativeReductionTooSmall );
            TAKECAREOF ( RelativeErrorTooSmall );
            TAKECAREOF ( RelativeErrorAndReductionTooSmall );
            TAKECAREOF ( CosinusTooSmall );
            TAKECAREOF ( TooManyFunctionEvaluation );
            TAKECAREOF ( FtolTooSmall );
            TAKECAREOF ( XtolTooSmall );
            TAKECAREOF ( GtolTooSmall );
            TAKECAREOF ( UserAsked );

#undef TAKECAREOF
        }

        // Print the objectives
        for(int i = 0 ; i < (int)_objectives.size() ; ++i)
        {
            fprintf ( log_file, "%s: %g \n",
             globfitobjectivetype_as_string(_objectives[i]->get_type()).c_str(),
             _cached_objective(i) );
        }

        fflush ( log_file );
    }

	set_variables(x);

#if PRINT_ITERATION_STATS
	std::cout << "Energy per objective type:" << std::endl;
	std::map<GlobFitObjectiveType, double> energy_per_type;
	for (int i = 0; i < _objectives.size(); ++i)
	{
		energy_per_type[_objectives[i]->get_type()] += _cached_objective.segment(_xadj_equations[i], _xadj_equations[i + 1] - _xadj_equations[i]).squaredNorm();
	}
	for (auto& entry : energy_per_type)
	{
		std::cout << globfitobjectivetype_as_string(entry.first) << ": " << entry.second << std::endl;
	}

#endif

	return iter_id;
}

void GlobFitter::set_curves(const std::vector<GlobFitCurveParametrization*>& curves_in) {
	_curves = curves_in;
	_is_setup = false;
}

void GlobFitter::set_objectives(const std::vector<GlobFitObjective*>& objectives_in) {
	_objectives = objectives_in;
	_is_setup = false;
}

void GlobFitter::set_constraints(const std::vector<GlobFitConstraint*>& c) {
	_constraints = c;
	constraint_gradients.resize(c.size());
	_is_setup = false;
}

void GlobFitter::setup(const int max_iterations) {
	_max_iterations = max_iterations;

	curve_to_index.clear();
	for (int i = 0; i < _curves.size(); ++i)
		curve_to_index[_curves[i]] = i;

	//Assign a temporary index to each parameter
	nextParameter = 0;
	for (int i = 0; i < (int)_curves.size(); ++i) {
		auto curve = _curves[i];
		auto& info = curve->get_parameter_info();
		for (auto& entry : info) {
			entry.variable_id = nextParameter++;
			entry.constraint_id = -1;
			entry.fixedValuePropagated = std::numeric_limits<double>::quiet_NaN();
		}
	}

	//Assign constraint information
	for(int i = 0; i < _constraints.size(); ++i)
	{
		auto& target = _constraints[i]->get_target_param();
		auto& param = target.curve->get_parameter_info()[target.internal_parameter];
		assert_break_msg(param.constraint_id == -1, "Only one constraint is permitted per parameter.");
		assert_break_msg(std::isnan(param.fixedValue), "A fixed parameter cannot have an additional constraint.");
		param.constraint_id = i;
	}

	//Check if there are chained constraints
	for (auto& c : _constraints)
	{
		for (auto& source : c->get_source_params())
		{
			assert_break_msg(source.curve->get_parameter_info()[source.internal_parameter].constraint_id == -1, "Chained constraints are not supported.");
		}
	}

	//Find the coupled parameters with a union-find
	struct EntryData
	{
		int variable_id;
		double fixedValue = std::numeric_limits<double>::quiet_NaN();
		bool is_constrained = false;
	};

	UnionFind<EntryData> uf(nextParameter);

	for (int i = 0; i < (int)_curves.size(); ++i) {
		auto curve = _curves[i];
		auto& info = curve->get_parameter_info();
		for (auto& entry : info) {
			uf[entry.variable_id].is_constrained = entry.constraint_id != -1;
			for (auto& c : entry.coupled_parameters) {
				auto& param2 = c.curve->get_parameter_info()[c.internal_parameter];
				auto coupledParamId = param2.variable_id;
				uf.merge(entry.variable_id, coupledParamId);
				assert_break_msg(entry.constraint_id == -1 && param2.constraint_id == -1, "Constraining coupled parameters is not supported.");
			}
		}
	}

	//set fixed values to parents
	for (int i = 0; i < (int)_curves.size(); ++i) {
		auto curve = _curves[i];
		auto& info = curve->get_parameter_info();
		for (auto& entry : info) {
			if (!std::isnan(entry.fixedValue)) {
				auto& parentFixed = uf[uf.getRepresentative(entry.variable_id)].fixedValue;
				assert_break(std::isnan(parentFixed) || std::abs(parentFixed - entry.fixedValue) < 0.0001);
				parentFixed = entry.fixedValue;
			}
		}
	}

	//assign actual parameter indices to parents
	nextParameter = 0;
	for (int i = 0; i < uf.size(); ++i)
		if (uf[i].parent == i && std::isnan(uf[i].fixedValue) && !uf[i].is_constrained)
			uf[i].variable_id = nextParameter++;

	//assign parameter indices to children
	for (int i = 0; i < (int)_curves.size(); ++i) {
		auto curve = _curves[i];
		auto& info = curve->get_parameter_info();
		for (auto& entry : info) {
			auto& parent = uf[uf.getRepresentative(entry.variable_id)];
			// reset the variable id
			entry.variable_id = -1;
			if (!std::isnan(parent.fixedValue))
				// this parameter is fixed
				entry.fixedValuePropagated = parent.fixedValue;
			else if (entry.constraint_id != -1)
				// this parameter is constrained
				;
			else
				// this parameter has a variable
				entry.variable_id = parent.variable_id;
		}
	}

	//
	// Equation bookkeeping
	//
	_xadj_equations.resize(_objectives.size() + 1);
	_xadj_equations[0] = 0;
	variables_in_objective.clear();
	variables_in_objective.resize(_objectives.size());
	for (int i = 0; i < (int)_objectives.size(); ++i) {
		_xadj_equations[i + 1] = _xadj_equations[i] + _objectives[i]->n_equations();

		auto& o = _objectives[i];
		for (auto c : o->get_curves()) {
			for (auto& p : c->get_parameter_info()) {
				if (p.variable_id != -1)
					variables_in_objective[i].push_back(p.variable_id);
				else if (p.constraint_id != -1)
					for (auto& constraint_source : _constraints[p.constraint_id]->get_source_params())
					{
						auto& source_param = constraint_source.curve->get_parameter_info()[constraint_source.internal_parameter];
						if (source_param.variable_id != -1)
							variables_in_objective[i].push_back(source_param.variable_id);
					}
			}
		}
	}

	//
	// Initial params
	//
	_is_setup = true;

	//check_derivatives();
}

int GlobFitter::n_parameters() {
	assert_break(_is_setup);
	return nextParameter;
}

int GlobFitter::n_equations() {
	assert_break(_is_setup);
	return _xadj_equations.back();
}

bool GlobFitter::is_setup() {
	return _is_setup;
}

std::vector<GlobFitCurveParametrization*> GlobFitter::get_curves() {//
	return _curves;
}

std::vector<GlobFitObjective*> GlobFitter::get_objectives() {//
	return _objectives;
}

void GlobFitter::compute_objective_and_jacobian(Eigen::VectorXd& obj, Eigen::SparseMatrix<double>& jac) {//
	assert_break(_is_setup);

	obj.setZero(n_equations());
	if (jac.isCompressed()) {
		jac.resize(n_equations(), n_parameters());
		//reserve space for jacobian, this is only done once
		Eigen::VectorXi entriesPerColumn(n_parameters());
		entriesPerColumn.setZero();
		for (int i = 0; i < _objectives.size(); ++i)
		{
			for (auto variable : variables_in_objective[i])
				entriesPerColumn(variable) += _objectives[i]->n_equations();
		}
		_cached_jacobian.reserve(entriesPerColumn);
	}

	Eigen::VectorXd sub_obj;
	Eigen::MatrixXd sub_jac;

	for (int objid = 0; objid < (int)_objectives.size(); ++objid) {
		const int eq_beg = _xadj_equations[objid];
		const int eq_len = _xadj_equations[objid + 1] - _xadj_equations[objid];

		// Eval objective
		_objectives[objid]->compute_objective_and_jacobian(sub_obj, sub_jac);

		// Set obj
		obj.segment(eq_beg, eq_len) = _objectives[objid]->get_sqrt_weight() * sub_obj;

		// Set Jacobian
		// Initialize to zero
		for (auto variable : variables_in_objective[objid])
		{
			for (int i = _xadj_equations[objid]; i < _xadj_equations[objid + 1]; ++i)
				jac.coeffRef(i, variable) = 0;
		}
		// Add the partial gradient contributions
		int subParamId = 0;
		for (auto curve : _objectives[objid]->get_curves()) {
			auto& params = curve->get_parameter_info();
			for (auto& p : params) {
				if (p.variable_id != -1) {
					//the parameter is a variable
					for(int eq = eq_beg; eq != _xadj_equations[objid + 1]; ++eq)
						jac.coeffRef(eq, p.variable_id) += _objectives[objid]->get_sqrt_weight() * sub_jac.coeff(eq - eq_beg, subParamId);
				}
				else if (p.constraint_id != -1) {
					//the parameter is derived from other variables
					auto& sources = _constraints[p.constraint_id]->get_source_params();
					auto& dpdsources = constraint_gradients[p.constraint_id];
					for (int iSource = 0; iSource < sources.size(); ++iSource)
					{
						auto& source = sources[iSource];
						auto& psource = source.curve->get_parameter_info()[source.internal_parameter];
						if (psource.variable_id != -1)
						{
							for (int eq = eq_beg; eq != _xadj_equations[objid + 1]; ++eq)
								//apply chain rule  d obj / d source = d obj / d target * d target / d source
								jac.coeffRef(eq, psource.variable_id) += _objectives[objid]->get_sqrt_weight() * sub_jac.coeff(eq - eq_beg, subParamId) * dpdsources(iSource);
						}
					}
				}
				++subParamId;
			}
		} // end of curves_outline
	} // end of objectives

#if PRINT_JACOBIAN
	{
		std::ofstream file("jacobian.txt", std::ios::trunc | std::ios::out);
		file << "Variables: \n";
		file << get_variables().transpose() << "\n\n";
		file << "Obj                                                  | Jacobian: \n";
		for (int i_obj = 0; i_obj < _objectives.size(); ++i_obj)
		{
			auto row_start = _xadj_equations[i_obj];
			auto row_end = _xadj_equations[i_obj + 1];
			for (int row = row_start; row < row_end; ++row)
			{
				std::string type = (row == row_start ? globfitobjectivetype_as_string(_objectives[i_obj]->get_type()) : "");
				file << std::setw(40) << type << std::setw(12) << obj(row) << " | ";
				for (int col = 0; col < jac.cols(); ++col)
					file << std::setw(14) << jac.coeff(row, col);
				file << '\n';
			}
		}
		file << '\n';

		file << "Curve parameters (variable id, fixed value, fixed value propagated): \n";
		for (auto& c : get_curves()) {
			for (auto& p : c->get_parameter_info())
				file << "(" << p.variable_id << ", " << p.fixedValue << ", " << p.fixedValuePropagated << ") ";
			file << '\n';
		}
		file << '\n';

		//file << "Sparse Jacobian: \n";
		//file << nse::util::FormatMathematica(jac);
		file.close();
	}
#endif
}

void GlobFitter::check_derivatives() {
	auto x = get_variables();
	Eigen::VectorXd obj;
	Eigen::SparseMatrix<double> jac;
	jac.resize(n_equations(), n_parameters());
	_cached_jacobian.resize(n_equations(), n_parameters());
	const double eps = 0.001;

	Eigen::VectorXd objPlus, objMinus;

	bool error = false;
	compute_objective_and_jacobian(obj, jac);
	for (int ip = 0; ip < n_parameters(); ++ip)
	{
		auto xCopy = x;
		xCopy(ip) += eps;
		set_variables(xCopy);
		compute_objective_and_jacobian(objPlus, _cached_jacobian);

		xCopy(ip) -= 2 * eps;
		set_variables(xCopy);
		compute_objective_and_jacobian(objMinus, _cached_jacobian);

		Eigen::VectorXd expectedDeriv = (objPlus - objMinus) / (2 * eps);
		for (int i = 0; i < n_equations(); ++i)
		{
			auto error = jac.coeff(i, ip) - expectedDeriv(i);
			auto relativeError = error / std::max(std::abs(obj(i)), std::abs(jac.coeff(i, ip)));

			if (std::abs(relativeError) > 0.001 && std::abs(error) > 1e-10)
			{
				error = true;
				std::cout << "Wrong derivative at parameter " << ip << ", equation " << i << ". Numerical derivative: " << expectedDeriv(i) <<
					", computed derivative: " << jac.coeff(i, ip) << ", objective value: " << obj(i) << ", parameter value: " << x(ip) << std::endl;
				int obj_id = 0;
				while (_xadj_equations[obj_id] <= i)
					++obj_id;
				--obj_id;
				std::cout << "Equation belongs to objective " << obj_id << " (type " << globfitobjectivetype_as_string(_objectives[obj_id]->get_type()) << ", local equation " << i - _xadj_equations[obj_id] << ")" << std::endl;
				std::cout << "Parameter belongs to the following curves: ";
				for (int ic = 0; ic < _curves.size(); ++ic)
				{
					auto curve = _curves[ic];
					auto& curve_params = curve->get_parameter_info();
					for (int icp = 0; icp < curve_params.size(); ++icp)
					{
						if (curve_params[icp].variable_id == ip)
							std::cout << "curve " << ic << " (type " << typeid(*curve).name() << ", local parameter " << icp << ") ";
					}
				}
				std::cout << std::endl;
			}
		}
	}
	if (!error)
		std::cout << "Derivatives seem good." << std::endl;
	set_variables(x);
}

Eigen::VectorXd GlobFitter::get_variables() {//
	assert_break(_is_setup);
	Eigen::VectorXd ans(n_parameters());

	for(auto curve : _curves) {
		auto params = curve->get_params();
		auto& pInfo = curve->get_parameter_info();
		for (int i = 0; i < pInfo.size(); ++i) {
			if (pInfo[i].variable_id != -1)
				ans(pInfo[i].variable_id) = params(i);
		}
	}

	return ans;
}

void GlobFitter::set_variables(const Eigen::VectorXd& params_in) { //
	std::vector<Eigen::VectorXd> curve_params(_curves.size());

	//Fill the variables into curve parameters, consider fixed and coupled parameters
	for (int iCurve = 0; iCurve < _curves.size(); ++iCurve) {
		auto curve = _curves[iCurve];
		auto& pInfo = curve->get_parameter_info();
		auto& params = curve_params[iCurve];
		params.resize(pInfo.size());
		for (int i = 0; i < pInfo.size(); ++i) {
			if (pInfo[i].variable_id != -1)
				params(i) = params_in(pInfo[i].variable_id);
			else if (!std::isnan(pInfo[i].fixedValuePropagated))
				params(i) = pInfo[i].fixedValuePropagated;
		}
		//at this point, the constrained parameters are not yet filled in
		//set them anyway, so that the calculation of constraints can access them
		curve->set_params(params);
	}

	std::vector<bool> curve_has_constrained_parameters(_curves.size(), false);
	//Derive parameters from constraints
	for (int iConstraint = 0; iConstraint < _constraints.size(); ++iConstraint) {
		auto c = _constraints[iConstraint];
		auto& target = c->get_target_param();
		auto curve_id = curve_to_index.at(target.curve);
		//We calculate the value of this constraint and assume the following:
		//  * all source parameters are set on the underlying parametrizations (no chained constraints)
		std::tie(curve_params[curve_id](target.internal_parameter), constraint_gradients[iConstraint]) = c->f();
		curve_has_constrained_parameters[curve_id] = true;
	}

	//Set the new curve parameters that are changed by constraints
	for (int iCurve = 0; iCurve < _curves.size(); ++iCurve) {
		if (!curve_has_constrained_parameters[iCurve])
			continue;
		auto curve = _curves[iCurve];
		curve->set_params(curve_params[iCurve]);
	}
}

void GlobFitter::report_errors(FILE* error_file) {
	for (size_t i = 0; i < _curves.size(); ++i) {
		fprintf(error_file, "curve id %d\n", (int)i);
		fprintf(error_file, "----------------------------------------------\n");

		for (size_t j = 0; j < _objectives.size(); ++j) {
			GlobFitObjective* obj = _objectives[j];

			Eigen::VectorXd res;
			Eigen::MatrixXd jac;
			obj->compute_objective_and_jacobian(res, jac);
			auto type = globfitobjectivetype_as_string(obj->get_type());
			fprintf(error_file, "objective: %s\n", type);

			fprintf(error_file, "values: ");
			for (Eigen::Index k = 0; k < res.size(); ++k) {
				fprintf(error_file, "%f ", res(k));
			}
			fprintf(error_file, "\n");
		}
	}
}
}


// Copyright (C) 2016-2018 T. Zachary Laine
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
#include <boost/yap/expression.hpp>

#include <boost/test/minimal.hpp>

#include <sstream>

template<typename T>
using term = boost::yap::terminal<boost::yap::expression, T>;

template<typename T>
using ref = boost::yap::expression_ref<boost::yap::expression, T>;

namespace yap = boost::yap;
namespace bh = boost::hana;

namespace user {

    struct number
    {
        explicit operator double() const { return value; }

        double value;
    };

    number naxpy(number a, number x, number y)
    {
        return number{a.value * x.value + y.value + 10.0};
    }

    inline auto max(int a, int b) { return a < b ? b : a; };

    struct tag_type
    {};

    inline number tag_function(double a, double b) { return number{a + b}; }

    struct eval_xform_tag
    {
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::call>, tag_type, number a, double b)
        {
            return tag_function(a.value, b);
        }

        int operator()(
            yap::expr_tag<yap::expr_kind::call>, tag_type, double a, double b)
        {
            return 42;
        }

        char const * operator()() { return "42"; }
    };

    struct empty_xform
    {};

    struct eval_xform_expr
    {
        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::call,
                                  bh::tuple<
                                      ref<term<user::tag_type>>,
                                      term<user::number>,
                                      term<int>>> const & expr)
        {
            using namespace boost::hana::literals;
            return tag_function(
                (double)yap::value(expr.elements[1_c]).value,
                (double)yap::value(expr.elements[2_c]));
        }

        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::call,
                                  bh::tuple<
                                      ref<term<user::tag_type>>,
                                      ref<term<user::number>>,
                                      term<int>>> const & expr)
        {
            using namespace boost::hana::literals;
            return tag_function(
                (double)yap::value(expr.elements[1_c]).value,
                (double)yap::value(expr.elements[2_c]));
        }
    };

    struct eval_xform_both
    {
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::call>,
            tag_type,
            user::number a,
            double b)
        {
            return tag_function(a.value, b);
        }

        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::call,
                                  bh::tuple<
                                      ref<term<user::tag_type>>,
                                      term<user::number>,
                                      term<int>>> const & expr)
        {
            using namespace boost::hana::literals;
            throw std::logic_error("Oops!  Picked the wrong overload!");
            return tag_function(
                (double)yap::value(expr.elements[1_c]).value,
                (double)yap::value(expr.elements[2_c]));
        }

        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::call,
                                  bh::tuple<
                                      ref<term<user::tag_type>>,
                                      ref<term<user::number>>,
                                      term<int>>> const & expr)
        {
            using namespace boost::hana::literals;
            throw std::logic_error("Oops!  Picked the wrong overload!");
            return tag_function(
                (double)yap::value(expr.elements[1_c]).value,
                (double)yap::value(expr.elements[2_c]));
        }
    };
}

int test_main(int, char * [])
{
    {
        using namespace boost::yap::literals;

        {
            auto plus = yap::make_terminal(user::tag_type{});
            auto expr = plus(user::number{13}, 1);

            {
                transform(expr, user::empty_xform{});
            }

            {
                user::number result = transform(expr, user::eval_xform_tag{});
                BOOST_CHECK(result.value == 14);
            }

            {
                user::number result = transform(expr, user::eval_xform_expr{});
                BOOST_CHECK(result.value == 14);
            }

            {
                user::number result = transform(expr, user::eval_xform_both{});
                BOOST_CHECK(result.value == 14);
            }
        }

        {
            auto plus = yap::make_terminal(user::tag_type{});
            auto thirteen = yap::make_terminal(user::number{13});
            auto expr = plus(thirteen, 1);

            {
                user::number result = transform(expr, user::eval_xform_tag{});
                BOOST_CHECK(result.value == 14);
            }

            {
                user::number result = transform(expr, user::eval_xform_expr{});
                BOOST_CHECK(result.value == 14);
            }

            {
                user::number result = transform(expr, user::eval_xform_both{});
                BOOST_CHECK(result.value == 14);
            }
        }

        {
            term<user::number> a{{1.0}};
            term<user::number> x{{42.0}};
            term<user::number> y{{3.0}};
            auto n = yap::make_terminal(user::naxpy);

            auto expr = n(a, x, y);
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 55);
            }
        }
    }

    return 0;
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm statistic binary_min
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/core/data_customization_point/vector.h"
#include "fern/feature/core/data_customization_point/array.h"
#include "fern/algorithm/statistic/binary_min.h"

namespace fa = fern::algorithm;

template<
    class Value1,
    class Value2,
    class Result>
void verify_value(
    Value1 const& value1,
    Value2 const& value2,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::statistic::binary_min(sequential, value1, value2, result_we_get);
    BOOST_CHECK_EQUAL(result_we_get, result_we_want);
}

BOOST_AUTO_TEST_CASE(algorithm)
{
    {
        verify_value<int8_t, int8_t, int8_t>( 5,  5,  5);
        verify_value<int8_t, int8_t, int8_t>(-5, -5, -5);
        verify_value<int8_t, int8_t, int8_t>( 0,  0,  0);
        verify_value<int8_t, int8_t, int8_t>( 5, -5, -5);
        verify_value<int8_t, int8_t, int8_t>(-5,  5, -5);
    }
}


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 * Some examples of loading and blocking single-file matrices into distributed memory.
 */

#include <iostream>

#include <boost/math/distributions/normal.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/io.h>
#include <util/random.h>

#include <mpi2/mpi2.h>

#include <mf/mf.h>

using namespace std;
using namespace mpi2;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

int main(int argc, char* argv[]) {
	boost::mpi::communicator& world = mfInit(argc, argv);
	mfStart();

	if (world.rank() == 0) {
		mf_size_type size1 = 10;
		mf_size_type size2 = 8;
		mf_size_type nnz = 20;
		mf_size_type r = 2;
		///////////////////
		mf_size_type max;
		///////////////////
		// an empty distributed matrix
		SparseMatrix sTemp;
		LOG4CXX_INFO(logger, "");
		LOG4CXX_INFO(logger, "Creating an empty distributed sparse matrix (2x2)");
		DistributedSparseMatrix z22("z22", size1, size2, 2, 2); // this creates only a descriptor
		z22.create();                                           // this creates the empty blocks
		LOG4CXX_INFO(logger, rg::paste("Descriptor:\n", z22));
		LOG4CXX_INFO(logger, rg::paste("Nnz: ", mf::nnz(z22)));
		unblock(z22, sTemp);
		LOG4CXX_INFO(logger, rg::paste("Unblocked value: ", sTemp));
		LOG4CXX_INFO(logger, rg::paste("Unblocked nnz: ", mf::nnz(sTemp)));

		// create some matrices to play with
		LOG4CXX_INFO(logger, "");
		LOG4CXX_INFO(logger, "Generating some matrices: s, w, h");
		Random32 random;
		SparseMatrix s(size1, size2);
		DenseMatrix mDense(size1, size2), w(size1, r);
		DenseMatrixCM h(r, size2);
		generateRandom(s, nnz, random, boost::normal_distribution<>(0, 1));
		generateRandom(w, random, boost::uniform_real<>(-0.5, 0.5));
		generateRandom(h, random, boost::uniform_real<>(-0.5, 0.5));

		// write them out
		LOG4CXX_INFO(logger, "Writing the matrices to disk");
		string fs = "/tmp/v";
		string fw = "/tmp/w";
		string fh = "/tmp/h";
		writeMatrix(fs, s, MM_COORD);
		writeMatrix(fw, w, MM_ARRAY);
		writeMatrix(fh, h, MM_ARRAY);

		// load as a distributed 1x1 sparse matrix
		LOG4CXX_INFO(logger, "");
		LOG4CXX_INFO(logger, "Loading s as a distributed sparse matrix (1x1)");
		DistributedSparseMatrix s11 = loadMatrix<SparseMatrix>(
				"s11", 1, 1, true, fs, MM_COORD);
		LOG4CXX_INFO(logger, rg::paste("Descriptor:\n", s11));
		unblock(s11, sTemp);
		LOG4CXX_INFO(logger, rg::paste("Unblocked value: ", sTemp));
		LOG4CXX_INFO(logger, rg::paste("Unblocked nnz: ", mf::nnz(sTemp)));

		// load as a distributed 1x1 dense matrix
		DenseMatrix dTemp;
		LOG4CXX_INFO(logger, "");
		LOG4CXX_INFO(logger, "Loading s as a distributed dense matrix (1x1)");
		DistributedDenseMatrix d11 = loadMatrix<DenseMatrix>(
				"d11", 1, 1, true, fs, MM_COORD);
		LOG4CXX_INFO(logger, rg::paste("Descriptor:\n", d11));
		mDense.clear(); unblock(d11, dTemp);
		LOG4CXX_INFO(logger, rg::paste("Unblocked value: ", dTemp));

		// load as a distributed 2x2 sparse matrix
		LOG4CXX_INFO(logger, "");
		LOG4CXX_INFO(logger, "Loading s as a distributed sparse matrix (2x2)");
		DistributedSparseMatrix s22 = loadMatrix<SparseMatrix>(
				"s22", 2, 2, true, fs, MM_COORD);
		LOG4CXX_INFO(logger, rg::paste("Descriptor:\n", s22));
		std::vector<mf_size_type> nnz1, nnz2;
		mf::nnz12(s22, nnz1, nnz2,max);///////////////////////////////////////////////
		LOG4CXX_INFO(logger, rg::paste("nnz1:", nnz1));
		LOG4CXX_INFO(logger, rg::paste("nnz2:", nnz2));
		unblock(s22, sTemp);
		LOG4CXX_INFO(logger, rg::paste("Unblocked value: ", sTemp));
		LOG4CXX_INFO(logger, rg::paste("Unblocked nnz: ", mf::nnz(sTemp)));
		mf::nnz12(sTemp, nnz1, nnz2,max);///////////////////////////////////////
		LOG4CXX_INFO(logger, rg::paste("Unblocked nnz1:", nnz1));
		LOG4CXX_INFO(logger, rg::paste("Unblocked nnz2:", nnz2));

		// load a 2x1 dense matrix
		LOG4CXX_INFO(logger, "");
		LOG4CXX_INFO(logger, "Loading w as a distributed dense matrix (2x1)");
		DistributedDenseMatrix w21 = loadMatrix<DenseMatrix>(
				"w21", 2, 1, true, fw, MM_ARRAY);
		LOG4CXX_INFO(logger, rg::paste("Descriptor:\n", w21));
		mDense.clear(); unblock(w21, dTemp);
		LOG4CXX_INFO(logger, rg::paste("Unblocked value: ", dTemp));

		// load a 1x2 dense matrix, partition by column
		LOG4CXX_INFO(logger, "");
		LOG4CXX_INFO(logger, "Loading h as a distributed dense matrix (1x2, CM)");
		DistributedDenseMatrixCM h12 = loadMatrix<DenseMatrixCM>(
				"h12", 1, 2, false, fh, MM_ARRAY);
		LOG4CXX_INFO(logger, rg::paste("Descriptor:\n", h12));
		mDense.clear(); unblock(h12, dTemp);
		LOG4CXX_INFO(logger, rg::paste("Unblocked value: ", dTemp));

		// print the environments
		LOG4CXX_INFO(logger, "");
		LOG4CXX_INFO(logger, "Printing the environments of each rank");
		lsAll();

		// some statistcs
		LOG4CXX_INFO(logger, "");
		LOG4CXX_INFO(logger, "Comparing results of operations on matrices / distributed matrices");
		LOG4CXX_INFO(logger, "Nnz: " << mf::nnz(s) << " " << mf::nnz(s22) << " " << mf::nnz(s22, 2));
		LOG4CXX_INFO(logger, "Sum: " << sum(s) << " " << sum(s22) << " " << sum(s22, 2));
		LOG4CXX_INFO(logger, "Sum of squares: " << l2(s) << " " << l2(s22) << " " << l2(s22, 2));
		LOG4CXX_INFO(logger, "nzsl: " << nzsl(s, w, h) << " " << nzsl(s22, w21, h12) << " " << nzsl(s22, w21, h12, 2));
	}

	mfStop();
	mfFinalize();

	return 0;
}