/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    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 <Eigen/Core>
#include "ch.h"
#include "hal.h"

#include "blink.h"
#include "usbconfig.h"
#include "printf.h"

namespace {
    using real_t = float;
    const unsigned int n = 4;
    const real_t matrix_scale_limit = 100.0f;
    using matrix_t = Eigen::Matrix<real_t, n, n>;
    const matrix_t A((matrix_t() <<
                1.0f, 0.9f, 0.8f, 0.7f,
                0.6f, 0.5f, 0.4f, 0.3f,
                0.2f, 0.1f, 0.09f, 0.08f,
                0.07f, 0.06f, 0.05f, 0.04f).finished());

    const systime_t loop_time = MS2ST(1); // loop at 1 kHz
} // namespace

/*
 * Application entry point.
 */
int main(void) {

    /*
     * System initializations.
     * - HAL initialization, this also initializes the configured device drivers
     *   and performs the board-specific initializations.
     * - Kernel initialization, the main() function becomes a thread and the
     *   RTOS is active.
     */
    halInit();
    chSysInit();

    /*
     * Initializes a serial-over-USB CDC driver.
     */
    sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);

    /*
     * Activates the USB driver and then the USB bus pull-up on D+.
     * Note, a delay is inserted in order to not have to disconnect the cable
     * after a reset.
     */
    board_usb_lld_disconnect_bus();   //usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(1500);
    usbStart(serusbcfg.usbp, &usbcfg);
    board_usb_lld_connect_bus();      //usbConnectBus(serusbcfg.usbp);

    /*
     * Creates the blink thread.
     */
    chBlinkThreadCreateStatic();

    /*
     * Normal main() thread activity. In this demo it simulates multiplies a matrix.
     */
    uint32_t i = 0;
    matrix_t B = matrix_t::Identity();
    rtcnt_t start = 0;
    rtcnt_t dt = 0;
    systime_t deadline = chVTGetSystemTimeX();

    while (true) {
        // Use RTC directly due to a bug in last time computation in TM module.
        start = chSysGetRealtimeCounterX(); // measure computation/transmit time
        deadline += loop_time; // next deadline
        B = A * B;

        /*
         * Rescale matrix entries if too large or too small.
         */
        real_t* data_A = const_cast<real_t*>(A.data());
        real_t* data_B = const_cast<real_t*>(B.data());
        for (int i = 0; i < A.size(); ++i, ++data_A, ++data_B) {
            real_t a = *data_A;
            real_t b = *data_B;
            if ((b > 100*a) || (b < a/100)) {
                *data_B = a;
            }
        }

        /*
         * Transmit loop time and matrix values.
         */
        if (SDU1.config->usbp->state == USB_ACTIVE || SDU1.state == SDU_READY) {
            printf("iteration %d: %d us\r\n", ++i, RTC2US(STM32_SYSCLK, dt));
            real_t* data = B.data();
            for (int i = 0; i < B.size(); ++i) {
                printf("%0.2f", *data++);
                if ((i % 4) == 3) {
                    printf("\r\n");
                } else {
                    printf(", ");
                }
            }
            printf("\r\n");
        }
        dt = chSysGetRealtimeCounterX() - start;

        /*
         * Sleep thread until next deadline if it hasn't already been missed.
         */
        bool miss = false;
        chSysLock();
        systime_t sleep_time = deadline - chVTGetSystemTimeX();
        if ((sleep_time > (systime_t)0) and (sleep_time < loop_time)) {
            chThdSleepS(sleep_time);
        } else {
            miss = true;
        }
        chSysUnlock();

        /*
         * If deadline missed, wait until button is pressed before continuing.
         */
        if (miss) {
            printf("loop time was: %d us\r\n", RTC2US(STM32_SYSCLK, dt));
            printf("Press button to continue.\r\n");
            while (palReadLine(LINE_BUTTON)) { /* Button is active LOW. */
                chThdSleepMilliseconds(10);
            }
            deadline = chVTGetSystemTimeX();
        }
    }
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ROL_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ROL_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-bitwise
    Function object implementing rol capabilities

    Returns the first entry rotated left
    by the value of the second entry.

    @par semantic:
    For any given value @c x of type @c T, @c n  of type @c I:

    @code
    T r = rol(x, n);
    @endcode

    @par Note
    This function asserts if the integral parameter is out of range

    @see  ror, rrol, rror

  **/
  Value rol(Value const & x, IntegerValue const& n);
} }
#endif

#include <boost/simd/function/scalar/rol.hpp>
#include <boost/simd/function/simd/rol.hpp>

#endif


/*ckwg +29
 * Copyright 2014-2015 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither name of Kitware, Inc. nor the names of any contributors may be used
 *    to endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * \file
 * \brief vxl estimate essential matrix implementation
 */

#include "estimate_essential_matrix.h"

#include <boost/foreach.hpp>

#include <maptk/feature.h>
#include <maptk/plugins/vxl/camera.h>

#include <vgl/vgl_point_2d.h>
#include <Eigen/LU>

#include <vpgl/algo/vpgl_em_compute_5_point.h>

namespace maptk
{

namespace vxl
{

/// Private implementation class
class estimate_essential_matrix::priv
{
public:
  /// Constructor
  priv()
  : verbose(false),
    num_ransac_samples(512)
  {
  }

  priv(const priv& other)
  : verbose(other.verbose),
    num_ransac_samples(other.num_ransac_samples)
  {
  }

  bool verbose;
  unsigned num_ransac_samples;
};

/// Constructor
estimate_essential_matrix
::estimate_essential_matrix()
: d_(new priv)
{
}

/// Copy Constructor
estimate_essential_matrix
::estimate_essential_matrix(const estimate_essential_matrix& other)
: d_(new priv(*other.d_))
{
}

/// Destructor
estimate_essential_matrix
::~estimate_essential_matrix()
{
}

/// Get this algorithm's \link maptk::config_block configuration block \endlink
config_block_sptr
estimate_essential_matrix
::get_configuration() const
{
  // get base config from base class
  config_block_sptr config =
      maptk::algo::estimate_essential_matrix::get_configuration();

  config->set_value("verbose", d_->verbose,
                    "If true, write status messages to the terminal showing "
                    "debugging information");

  config->set_value("num_ransac_samples", d_->num_ransac_samples,
                    "The number of samples to use in RANSAC");

  return config;
}

/// Set this algorithm's properties via a config block
void
estimate_essential_matrix
::set_configuration(config_block_sptr config)
{

  d_->verbose = config->get_value<bool>("verbose",
                                        d_->verbose);
  d_->num_ransac_samples = config->get_value<unsigned>("num_ransac_samples",
                                                       d_->num_ransac_samples);
}

/// Check that the algorithm's currently configuration is valid
bool
estimate_essential_matrix
::check_configuration(config_block_sptr config) const
{
  return true;
}

/// Estimate an essential matrix from corresponding points
matrix_3x3d
estimate_essential_matrix
::estimate(const std::vector<vector_2d>& pts1,
           const std::vector<vector_2d>& pts2,
           const camera_intrinsics_d &cal1,
           const camera_intrinsics_d &cal2,
           std::vector<bool>& inliers,
           double inlier_scale) const
{
  vpgl_calibration_matrix<double> vcal1, vcal2;
  maptk_to_vpgl_calibration(cal1, vcal1);
  maptk_to_vpgl_calibration(cal2, vcal2);

  vcl_vector<vgl_point_2d<double> > right_points, left_points;
  BOOST_FOREACH(const vector_2d& v, pts1)
  {
    right_points.push_back(vgl_point_2d<double>(v.x(), v.y()));
  }
  BOOST_FOREACH(const vector_2d& v, pts2)
  {
    left_points.push_back(vgl_point_2d<double>(v.x(), v.y()));
  }

  double sq_scale = inlier_scale * inlier_scale;
  vpgl_em_compute_5_point_ransac<double> em(d_->num_ransac_samples, sq_scale,
                                            d_->verbose);
  vpgl_essential_matrix<double> best_em;
  em.compute(right_points, vcal1, left_points, vcal2, best_em);

  matrix_3x3d E(best_em.get_matrix().data_block());
  E.transposeInPlace();
  matrix_3x3d K1_inv = matrix_3x3d(cal1).inverse();
  matrix_3x3d K2_invt = matrix_3x3d(cal2).transpose().inverse();
  matrix_3x3d F = K2_invt * E * K1_inv;
  matrix_3x3d Ft = F.transpose();

  inliers.resize(pts1.size());
  for(unsigned i=0; i<pts1.size(); ++i)
  {
    const vector_2d& p1 = pts1[i];
    const vector_2d& p2 = pts2[i];
    vector_3d v1(p1.x(), p1.y(), 1.0);
    vector_3d v2(p2.x(), p2.y(), 1.0);
    vector_3d l1 = F * v1;
    vector_3d l2 = Ft * v2;
    double s1 = 1.0 / sqrt(l1.x()*l1.x() + l1.y()*l1.y());
    double s2 = 1.0 / sqrt(l2.x()*l2.x() + l2.y()*l2.y());
    // sum of point to epipolar line distance in both images
    double d = v1.dot(l2) * (s1 + s2);
    inliers[i] = std::fabs(d) < inlier_scale;
  }

  return E;
}

} // end namespace vxl

} // end namespace maptk


#include <toynet/examples/diff/diff.h>
#include <toynet/stlio.h>
#include <toynet/ublas/io.h>
#include <toynet/ublas/convert.h>
#include <iostream>
#include <sstream>
#include <random>
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

using namespace boost::program_options;
using namespace toynet;

void print(const boost::property_tree::ptree& pt)
{
    boost::property_tree::ptree::const_iterator end = pt.end();
    for (boost::property_tree::ptree::const_iterator it = pt.begin(); it != end; ++it) {
        std::cout << it->first << ": " << it->second.get_value<std::string>() << std::endl;
        print(it->second);
    }
}

std::vector<ublas::vector<double>> parse_json_examples(const std::string& s)
{
    std::vector<ublas::vector<double>> ret;
    std::stringstream ss(s);
    boost::property_tree::ptree pt;
    boost::property_tree::read_json(ss, pt);
    for (const auto& e1 : pt) {
        ublas::vector<double> v;
        for (const auto& e2 : e1.second) {
            v.resize(v.size() + 1);
            v[v.size() - 1] = e2.second.get_value<double>();
        }
        ret.push_back(v);
    }
    return ret;
}

struct Options
{
    options_description desc;
    variables_map vm;
    bool help;
    int hidden;
    int width;
    int inputs;
    int epochs;
    float lr;
    std::string training;
    bool progress;

    Options(int argc, char* argv[])
        : desc("Allowed options")
        , help(false)
        , hidden(1)
        , width(2)
        , inputs(2)
        , epochs(10)
        , lr(0.01)
        , training("[[4.0, 3.0]]")
        , progress(true)
    {
        desc.add_options()
            // First parameter describes option name/short name
            // The second is parameter to option
            // The third is description
            ("help,h", bool_switch(&help), "print help message")
            ("hidden,H", value(&hidden), "number of hidden layers")
            ("width,W", value(&width), "width of hidden layers")
            ("inputs,I", value(&inputs), "width of input layer")
            ("epochs,e", value(&epochs), "number of training epochs")
            ("lr", value(&lr), "learning rate")
            ("training", value(&training), "training data")
            ("progress", value(&progress), "show loss at each epoch")
            ;
        store(parse_command_line(argc, argv, desc), vm);
        notify(vm);
    }
};

int main(int argc, char* argv[])
{
    Options opts(argc, argv);

    if (opts.help) {
        std::cout << opts.desc << std::endl;
        return 0;
    }

    const std::vector<ublas::vector<double>> training_examples = parse_json_examples(opts.training);

    // Make sure the dimensions of each training example is equal to opts.inputs
    for (const auto&v : training_examples) {
        if (v.size() != opts.inputs) {
            std::cerr << "Size mismatch: the network has input size "
                      << opts.inputs << " but an example has size "
                      << v.size() << ": " << v << std::endl;
            return 1;
        }
    }

    DiffNumbers network(opts.hidden, opts.width, opts.inputs);
    for (int i = 0;  i < opts.epochs;  ++i) {
        network.forward_backward(training_examples);
        if (opts.progress)
            std::cout << i << " " << network.loss << std::endl;
        network.update_weights(opts.lr);
    }
    std::cout << network << std::endl;

#if 0
    // Let's go for more examples!
    network.init_weights();
    std::random_device rd;
    std::mt19937 e2(rd());
    std::uniform_real_distribution<> dist(-20, 150);
    std::vector<ublas::vector<double>> x4(1000, convert({0.0, 0.0}));
    for (auto & ex : x4) {
        ex[0] = dist(e2);
        ex[1] = dist(e2);
    }
    for (int i = 0;  i < 20;  ++i) {
        network.forward_backward(x4);
        // std::cout << i << " " << network.loss << std::endl;
        // std::cout << network << std::endl;
        network.update_weights(0.00005); // we need a small step size otherwise training doesn't converge
    }
    // std::cout << network << std::endl;
    BOOST_CHECK(network.loss < 1e-6);
#endif

    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to 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 <geometry_test_common.hpp>

#include <boost/geometry/algorithms/for_each.hpp>
#include <boost/geometry/multi/algorithms/for_each.hpp>

#include <boost/geometry/domains/gis/io/wkt/wkt.hpp>

#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/multi/geometries/multi_linestring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

#include <algorithms/test_for_each.hpp>

template <typename P>
void test_all()
{
    test_geometry<bg::model::multi_point<P> >
        (
            "MULTIPOINT((1 1))"

            // per point
            , 1
            , "MULTIPOINT((101 1))"
            , "MULTIPOINT((101 100))"
            // per segment
            , ""
            , 0
            , "MULTIPOINT((1 1))"
        );

    test_geometry<bg::model::multi_linestring<bg::model::linestring<P> > >
        (
        "MULTILINESTRING((1 1,2 2))"

            , 3
            , "MULTILINESTRING((101 1,102 2))"
            , "MULTILINESTRING((101 100,102 200))"

            , "((1, 1), (2, 2))"
            , std::sqrt(2.0)
            , "MULTILINESTRING((10 1,2 2))"
        );

    typedef bg::model::multi_polygon<bg::model::polygon<P> > mp;
    test_geometry<mp>
        (
            "MULTIPOLYGON(((1 1,1 4,4 4,4 1,1 1)))"

            , 11
            , "MULTIPOLYGON(((101 1,101 4,104 4,104 1,101 1)))"
            , "MULTIPOLYGON(((101 100,101 400,104 400,104 100,101 100)))"

            , "((1, 1), (1, 4)) ((1, 4), (4, 4)) ((4, 4), (4, 1)) ((4, 1), (1, 1))"
            , 4 * 3.0
            , "MULTIPOLYGON(((10 1,10 4,4 4,4 1,1 1)))"
        );
}

int test_main( int , char* [] )
{
    test_all<bg::model::d2::point_xy<double> >();

#ifdef HAVE_TTMATH
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/pow_abs.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/table.hpp>

#include <nt2/include/constants/i.hpp>
#include <nt2/include/constants/two.hpp>

NT2_TEST_CASE_TPL ( pow_abs3,  NT2_SIMD_REAL_TYPES)
{
  using nt2::pow_abs;
  using nt2::tag::pow_abs_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<cT,ext_t>                vcT;

  nt2::table<cT> a = nt2::ones(3, 3, nt2::meta::as_<cT>());
  nt2::table<cT> b = nt2::ones(3, 3, nt2::meta::as_<cT>());

  // specific values tests
  for(int i=1; i <= 3; i++)
    NT2_TEST_EQUAL(nt2::splat<vcT>(a(i)), nt2::pow_abs(nt2::splat<vcT>(a(i)), nt2::splat<vcT>(b(i))));
}

NT2_TEST_CASE_TPL ( pow_abs4,  NT2_SIMD_REAL_TYPES)
{
  using nt2::pow_abs;
  using nt2::tag::pow_abs_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<cT,ext_t>                vcT;

  nt2::table<cT> a = nt2::ones(1, 3, nt2::meta::as_<cT>());
  nt2::table<cT> b = T(2)*nt2::ones(1, 3, nt2::meta::as_<cT>());

  for(int i=1; i <= 3; i++)
  {
    a(i) =  cT(i, i);
    // specific values tests
    NT2_TEST_ULP_EQUAL(nt2::pow_abs(nt2::splat<vcT>(a(i)), nt2::splat<vcT>(b(i))), nt2::Two<vcT>()*nt2::splat<vcT>(nt2::sqr(nt2::real(a(i)))), 2.0);
  }
}


#include <boost/math/tools/big_constant.hpp>


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_HMSB_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_HMSB_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-reduction
    Function object implementing hmsb capabilities

    Returns a std::size_t value composed by the highiest bits.
    of each vector element

    @par Semantic:

    @code
    std::size_t r = hmsb(x);
    @endcode

    is similar to:

    @code
      std::size_t r = 0;
      for(result_type i = 0; i != cardinal_of<T>; ++i)
      {
        r |= (bits(x[i]) >> (sizeof(stype)*8 - 1)) << i;
      }
    @endcode

  **/
  std::size_t hmsb(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/hmsb.hpp>
#include <boost/simd/function/simd/hmsb.hpp>

#endif


/*
 * GridTools
 *
 * Copyright (c) 2014-2019, ETH Zurich
 * All rights reserved.
 *
 * Please, refer to the LICENSE file in the root directory.
 * SPDX-License-Identifier: BSD-3-Clause
 */
#include <iostream>

#include <boost/preprocessor/arithmetic/inc.hpp>
#include <boost/preprocessor/repetition/enum_params.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>

#include <gridtools/boundary_conditions/boundary.hpp>
#include <gridtools/common/halo_descriptor.hpp>
#include <gridtools/stencil_composition/stencil_composition.hpp>
#include <gridtools/tools/backend_select.hpp>

using namespace gridtools;
using namespace execute;

template <typename T>
struct direction_bc_input {
    T value;

    GT_FUNCTION
    direction_bc_input() : value(1) {}

    GT_FUNCTION
    direction_bc_input(T v) : value(v) {}

    // relative coordinates
    template <typename Direction, typename DataField0, typename DataField1>
    GT_FUNCTION void operator()(
        Direction, DataField0 &data_field0, DataField1 const &data_field1, uint_t i, uint_t j, uint_t k) const {
        data_field1(i, j, k) = data_field0(i, j, k) * value;
    }

    // relative coordinates
    template <sign I, sign K, typename DataField0, typename DataField1>
    GT_FUNCTION void operator()(
        direction<I, minus_, K>, DataField0 &, DataField1 const &data_field1, uint_t i, uint_t j, uint_t k) const {
        data_field1(i, j, k) = 88 * value;
    }

    // relative coordinates
    template <sign K, typename DataField0, typename DataField1>
    GT_FUNCTION void operator()(
        direction<minus_, minus_, K>, DataField0 &, DataField1 const &data_field1, uint_t i, uint_t j, uint_t k) const {
        data_field1(i, j, k) = 77777 * value;
    }

    template <typename DataField0, typename DataField1>
    GT_FUNCTION void operator()(direction<minus_, minus_, minus_>,
        DataField0 &,
        DataField1 const &data_field1,
        uint_t i,
        uint_t j,
        uint_t k) const {
        data_field1(i, j, k) = 55555 * value;
    }
};

int main(int argc, char **argv) {
    if (argc != 4) {
        std::cout << "Usage: " << argv[0]
                  << " dimx dimy dimz\n"
                     " where args are integer sizes of the data fields"
                  << std::endl;
        return EXIT_FAILURE;
    }

    uint_t d1 = atoi(argv[1]);
    uint_t d2 = atoi(argv[2]);
    uint_t d3 = atoi(argv[3]);

    typedef storage_traits<backend_t>::storage_info_t<0, 3, halo<1, 1, 1>> meta_data_t;
    typedef storage_traits<backend_t>::data_store_t<int_t, meta_data_t> storage_t;

    // Definition of the actual data fields that are used for input/output
    meta_data_t meta_(d1, d2, d3);
    storage_t in_s(meta_, [](int i, int j, int k) { return i + j + k; }, "in");
    storage_t out_s(meta_, 0, "out");

    gridtools::array<gridtools::halo_descriptor, 3> halos;
    halos[0] = gridtools::halo_descriptor(1, 1, 1, d1 - 2, d1);
    halos[1] = gridtools::halo_descriptor(1, 1, 1, d2 - 2, d2);
    halos[2] = gridtools::halo_descriptor(1, 1, 1, d3 - 2, d3);

    // sync the data stores if needed
    in_s.sync();
    out_s.sync();

    gridtools::template boundary<direction_bc_input<uint_t>, backend_t>(halos, direction_bc_input<uint_t>(2))
        .apply(in_s, out_s);

    // sync the data stores if needed
    in_s.sync();
    out_s.sync();

    // making the views to access and check correctness
    auto in = make_host_view(in_s);
    auto out = make_host_view(out_s);

    assert(check_consistency(in_s, in) && "view is in an inconsistent state.");
    assert(check_consistency(out_s, out) && "view is in an inconsistent state.");

    // reactivate views and check consistency
    in_s.reactivate_host_write_views();
    out_s.reactivate_host_write_views();
    assert(check_consistency(in_s, in) && "view is in an inconsistent state.");
    assert(check_consistency(out_s, out) && "view is in an inconsistent state.");

    // check inner domain (should be zero)
    bool error = false;
    for (uint_t i = 1; i < d3 - 1; ++i) {
        for (uint_t j = 1; j < d2 - 1; ++j) {
            for (uint_t k = 1; k < d1 - 1; ++k) {
                if (in(k, j, i) != i + j + k) {
                    std::cout << "Error: INPUT field got modified " << k << " " << j << " " << i << "\n";
                    error = true;
                }
                if (out(k, j, i) != 0) {
                    std::cout << "Error: Inner domain of OUTPUT field got modified " << k << " " << j << " " << i
                              << "\n";
                    error = true;
                }
            }
        }
    }

    // check edge column
    if (out(0, 0, 0) != 111110) {
        std::cout << "Error: edge column values in OUTPUT field are wrong 0 0 0\n";
        error = true;
    }
    for (uint_t k = 1; k < d3; ++k) {
        if (out(0, 0, k) != 155554) {
            std::cout << "Error: edge column values in OUTPUT field are wrong 0 0 " << k << "\n";
            error = true;
        }
    }

    // check j==0 i>0 surface
    for (uint_t i = 1; i < d1; ++i) {
        for (uint_t k = 0; k < d3; ++k) {
            if (out(i, 0, k) != 176) {
                std::cout << "Error: j==0 surface values in OUTPUT field are wrong " << i << " 0 " << k << "\n";
                error = true;
            }
        }
    }

    // check outer domain
    for (uint_t i = 0; i < d1; ++i) {
        for (uint_t j = 0; j < d2; ++j) {
            for (uint_t k = 0; k < d3; ++k) {
                // check outer surfaces of the cube
                if (((i == 0 || i == d1 - 1) && j > 0) || (j > 0 && (k == 0 || k == d3 - 1))) {
                    if (out(i, j, k) != in(i, j, k) * 2) {
                        std::cout << "Error: values in OUTPUT field are wrong " << i << " " << j << " " << k << "\n";
                        error = true;
                    }
                }
            }
        }
    }

    if (error) {
        std::cout << "TEST failed.\n";
        abort();
    }

    return error;
}


//
// Copyright (c) Microsoft. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.
//
// Written by Luke Marshall and Timur Tankayev, 2019
//

#define WIN32_LEAN_AND_MEAN             // Exclude rarely-used stuff from Windows headers
#include <windows.h>

#define BOOST_PYTHON_STATIC_LIB
#include <boost/python.hpp>
#include <boost/python/tuple.hpp>
#include <numpy/ndarrayobject.h>

#include "..\ShiftedPhaseType\ShiftedPhaseType.h"
#include "..\ShiftedPhaseType\DistanceLookup.h"

using namespace ShiftedPhaseType;

// Adapter a member function that returns a unique_ptr to a python function object that
// returns a raw pointer but explicitly passes ownership to Python.
template <typename T, typename C, typename ...Args>
boost::python::object adapt_unique(std::unique_ptr<T>(C::* fn)(Args...))
{
	return boost::python::make_function(
		[fn](C& self, Args... args) { return (self.*fn)(args...).release(); },
		boost::python::return_value_policy<boost::python::manage_new_object>(),
		boost::mpl::vector<T*, C&, Args...>()
	);
}

// Convert std::tuple<T1,T2> to python tuple
template<typename T1, typename T2>
struct std_tuple_to_python_tuple {
	static PyObject* convert(const std::tuple<T1, T2>& t) {
		return boost::python::incref(
			boost::python::object(
				boost::python::make_tuple(std::get<0>(t), std::get<1>(t))).ptr());
	}
};

// Convert std::vector<T> to a python list
template<typename T>
struct std_vector_to_python_list {
	static PyObject* convert(const std::vector<T>& coll) {
		boost::python::list list;

		for (const auto& t : coll) {
			list.append(t);
		}

		return boost::python::incref(boost::python::object(list).ptr());
	}
};

// Specialized wrapper functions for ShiftedPhaseTypeDistribution class, in particular it converts RowVectorXd and SpMat to/from python
class WrapperFuncs
{
public:
	// Constructor wrapper
	static boost::shared_ptr<ShiftedPhaseTypeDistribution> initWrapper(double shift, boost::python::object const& alpha_py, boost::python::object const& t_py)
	{
		if (!PyArray_CheckExact(alpha_py.ptr())) {
			throw std::runtime_error("The given alpha is not a valid array");
		}

		if (!PyArray_CheckExact(t_py.ptr())) {
			throw std::runtime_error("The given T is not a valid array");
		}

		if (PyArray_TYPE(alpha_py.ptr()) != NPY_DOUBLE) {
			throw std::runtime_error("The alpha array should have double precision");
		}

		if (PyArray_TYPE(t_py.ptr()) != NPY_DOUBLE) {
			throw std::runtime_error("The T matrix should have double precision");
		}

		// We assume that there is only one dimension and assume double precision
		int size = (int)PyArray_SHAPE((PyArrayObject*)alpha_py.ptr())[0];

		RowVectorXd alpha(size);
		memcpy(alpha.data(), (double*)PyArray_DATA(alpha_py.ptr()), size * sizeof(double));

		// Assume dense matrix, only two dimensions, double precision
		int rows = (int)PyArray_SHAPE((PyArrayObject*)t_py.ptr())[0];
		int cols = (int)PyArray_SHAPE((PyArrayObject*)t_py.ptr())[1];

		SpMat t(rows, cols);

		// Count non-zero entries
		double* p = (double*)PyArray_DATA(t_py.ptr());

		int nnz = 0;

		for (int j = 0; j < cols; ++j) {
			for (int i = 0; i < rows; ++i) {
				if (p[i * rows + j] != 0.0) {
					++nnz;
				}
			}
		}

		// Load sparse matrix from dense numpy array
		SparseLoad loader(&t, nnz);

		for (int j = 0; j < cols; ++j) {
			for (int i = 0; i < rows; ++i) {
				if (p[i * rows + j] != 0.0) {
					loader.add(p[i * rows + j], i);
				}
			}
			loader.next_col();
		}

		return boost::shared_ptr<ShiftedPhaseTypeDistribution>(new ShiftedPhaseTypeDistribution(shift, alpha, t));
	}

	// Returns copy of Alpha vector
	static boost::python::object ShiftedPhaseType_Alpha(ShiftedPhaseTypeDistribution* d)
	{
		npy_intp shape[1] = { d->Alpha->size() };
		return boost::python::object(boost::python::handle<>(PyArray_SimpleNewFromData(1, shape, NPY_DOUBLE, (double*)d->Alpha->data())));
	}

	// Returns copy of T matrix (converted to dense format)
	static boost::python::object ShiftedPhaseType_T(ShiftedPhaseTypeDistribution* d) {
		npy_intp shape[2] = { d->T->rows(), d->T->cols() };
		PyObject* matrix = PyArray_SimpleNew(2, shape, NPY_DOUBLE);

		double* p = (double*)PyArray_DATA(matrix);

		for (int i = 0; i < d->T->rows(); ++i) {
			for (int j = 0; j < d->T->cols(); ++j) {
				p[i * d->T->rows() + j] = d->T->coeff(i, j);
			}
		}

		return boost::python::object(boost::python::handle<>(matrix));
	}
};

BOOST_PYTHON_MODULE(ShiftedPhaseTypePy)
{
	_import_array();	// numpy requires this

	using namespace boost::python;

	boost::python::to_python_converter<std::tuple<std::vector<double>, std::vector<double>>, std_tuple_to_python_tuple<std::vector<double>, std::vector<double>>>();
	boost::python::to_python_converter<std::tuple<double, double>, std_tuple_to_python_tuple<double, double>>();
	boost::python::to_python_converter<std::vector<double>, std_vector_to_python_list<double>>();

	class_<DistanceLookup>("DistanceLookup", init<std::string>())
		.def("GetDistance", &DistanceLookup::GetDistance)
		.def("FitDistance", &DistanceLookup::FitDistance)
		.def("GetProfile", &DistanceLookup::GetProfile);

	std::unique_ptr<ShiftedPhaseTypeDistribution>(ShiftedPhaseTypeDistribution:: * add1)(double) = &ShiftedPhaseTypeDistribution::Add;
	std::unique_ptr<ShiftedPhaseTypeDistribution>(ShiftedPhaseTypeDistribution:: * add2)(ShiftedPhaseTypeDistribution&, double) = &ShiftedPhaseTypeDistribution::Add;

	class_<ShiftedPhaseTypeDistribution>("ShiftedPhaseTypeDistribution", no_init)
		// Constructors
		.def("__init__", boost::python::make_constructor(&WrapperFuncs::initWrapper))
		.def(init<optional<double>>())
		.def(init<double, double, double>())

		// Member variables
		.def_readonly("Shift", &ShiftedPhaseTypeDistribution::Shift)
		.def_readonly("GreaterThanConditionalProbability", &ShiftedPhaseTypeDistribution::GreaterThanConditionalProbability)
		.def_readonly("GreaterThanConditionalScale", &ShiftedPhaseTypeDistribution::GreaterThanConditionalScale)
		.add_property("Alpha", &WrapperFuncs::ShiftedPhaseType_Alpha)
		.add_property("T", &WrapperFuncs::ShiftedPhaseType_T)

		// Add/Max operators
		.def("Add", adapt_unique(add1))
		.def("Add", adapt_unique(add2))
		.def("ShiftedAndScaledPH", adapt_unique(&ShiftedPhaseTypeDistribution::ShiftedAndScaledPH))
		.def("MaxWith", adapt_unique(&ShiftedPhaseTypeDistribution::MaxWith))
		.def("MaxWithAdd", adapt_unique(&ShiftedPhaseTypeDistribution::MaxWithAdd))
		.def("LinearMapWithMax", adapt_unique(&ShiftedPhaseTypeDistribution::LinearMapWithMax))

		// Member properties
		.def("IsPointMass", &ShiftedPhaseTypeDistribution::IsPointMass)
		.def("Density", &ShiftedPhaseTypeDistribution::Density)
		.def("CumulativeDistribution", &ShiftedPhaseTypeDistribution::CumulativeDistribution)
		.def("Unshifted1Moment", &ShiftedPhaseTypeDistribution::Unshifted1Moment)
		.def("Unshifted2Moments", &ShiftedPhaseTypeDistribution::Unshifted2Moments)
		.def("Sample", &ShiftedPhaseTypeDistribution::Sample)

		// Truncation operators
		.def("LeftTruncMoments", &ShiftedPhaseTypeDistribution::LeftTruncMoments)
		.def("RightTruncMoments", &ShiftedPhaseTypeDistribution::RightTruncMoments)
		.def("RightExpProb", &ShiftedPhaseTypeDistribution::RightExpProb)
		.def("ShiftedAndScaledPH", adapt_unique(&ShiftedPhaseTypeDistribution::ShiftedAndScaledPH))
		.def("ConditionGreaterThan", adapt_unique(&ShiftedPhaseTypeDistribution::ConditionGreaterThan));
}

BOOL APIENTRY DllMain(HMODULE hModule, DWORD  ul_reason_for_call, LPVOID lpReserved)
{
	switch (ul_reason_for_call)
	{
	case DLL_PROCESS_ATTACH:
	case DLL_THREAD_ATTACH:
	case DLL_THREAD_DETACH:
	case DLL_PROCESS_DETACH:
		break;
	}
	return TRUE;
}


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * 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.
 *
 *
 * planeRegistration.cpp
 *
 *  Created on: Jan 28, 2019
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include "mrob/pc_registration.hpp"
#include "mrob/plane_registration.hpp"
#include <Eigen/LU> // for inverse and determinant
#include <Eigen/Eigenvalues>
#include <iostream>

#include <chrono>

using namespace mrob;

PlaneRegistration::PlaneRegistration():
        numberPlanes_(0), numberPoses_(0),numberPoints_(0),isSolved_(0), trajectory_(new std::vector<SE3>(8,SE3())),
        solveMode_(SolveMode::GRADIENT),
        c1_(1e-4), c2_(0.9), alpha_(0.75), beta_(0.1)
{
    // Optmizer does not establish the size for this matrices and thus it is required
    gradient_.resize(6);
    hessian_.resize(6,6);
}

PlaneRegistration::~PlaneRegistration()
{

}

// XXX: is this function used??
void PlaneRegistration::set_number_planes_and_poses(uint_t numberPlanes, uint_t numberPoses)
{
    planes_.clear();
    planes_.reserve(numberPlanes);
    trajectory_->clear();
    trajectory_->resize(numberPoses, SE3());
    numberPlanes_ = numberPlanes;
    numberPoses_ = numberPoses;

    previousState_.clear();
    previousState_.resize(numberPoses, Mat61::Zero());

    numberPoints_ = 0;
}

void PlaneRegistration::reset_solution()
{
    // trajectory is reset
    trajectory_->clear();
    trajectory_->resize(numberPoses_, SE3());
    previousState_.clear();
    previousState_.resize(numberPoses_, Mat61::Zero());
}

uint_t PlaneRegistration::solve(SolveMode mode, bool singleIteration)
{
    // just in case some methods, such as gradient, has several modes
    solveMode_ = mode;
    uint_t iters = 0;
    initial_error_ = get_current_error();

    // time profiling
    time_profiles_.reset();
    switch(mode)
    {
        case SolveMode::INITIALIZE:
            return solve_initialize();
        case SolveMode::GRADIENT_ALL_POSES:
            time_profiles_.start();
            solve_gradient_all_poses();
            time_profiles_.stop();
            break;
        case SolveMode::GRADIENT_BENGIOS_NAG:
            time_profiles_.start();
            solve_interpolate_gradient(singleIteration);
            time_profiles_.stop();
            break;
        case SolveMode::GN_HESSIAN:
            time_profiles_.start();
            solveIters_ = optimize(NEWTON_RAPHSON);
            time_profiles_.stop();
            break;
        case SolveMode::GN_CLAMPED_HESSIAN:
            time_profiles_.start();
            //solveIters_ = solve_interpolate_hessian(singleIteration);//deprecated
            time_profiles_.stop();
            break;
        case SolveMode::LM_SPHER:
            time_profiles_.start();
            solveIters_ = optimize(LEVENBERG_MARQUARDT_SPHER,1e-2);
            time_profiles_.stop();
            break;
        case SolveMode::LM_ELLIP:
            time_profiles_.start();
            solveIters_ = optimize(LEVENBERG_MARQUARDT_ELLIP,1e-2);
            time_profiles_.stop();
            break;
        default:
            return 0;
    }
    return iters;
}

uint_t PlaneRegistration::solve_interpolate_gradient(bool singleIteration)
{
    // This function is from the first implementation, is just kept for comparisons (but should not be used to solve the problem)
    // iterative process, on convergence basis | error_k - error_k-1| < tol
    solveIters_ = 0;
    double previousError = 1e20, diffError = 10;
    do
    {
        // 1) calculate plane estimation given the current trajectory
        double  initialError = 0.0;
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        {
            initialError += it->second->estimate_plane();
        }
        diffError = previousError - initialError;
        previousError = initialError;

        //std::cout << "current error iteration " << solveIters_ << " = "<< initialError << std::endl;

        // 2) calculate Gradient = Jacobian^T. We maintain the nomenclature Jacobian for coherence on the project,
        //    but actually this Jacobian should be transposed.
        Mat61 jacobian = Mat61::Zero(), accumulatedJacobian = Mat61::Zero();
        double  numberPoints, tau = 1.0 / (double)(numberPoses_-1);
        for (uint_t t = 1 ; t < numberPoses_; ++t)
        {
            numberPoints = 0.0;
            jacobian.setZero();
            for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
            {
                jacobian += it->second->calculate_gradient(t);
                numberPoints += it->second->get_number_points(t);
            }
            // XXX this could be changed to time stamps later
            accumulatedJacobian +=  (tau *  t  / numberPoints / numberPoses_) * jacobian;

        }
        // 3) update results Tf = exp(-dxi) * Tf (our convention, we expanded from the left)
        // 3-1)
        Mat61 dxi, xiFinal;
        if (solveMode_ == SolveMode::GRADIENT)
        {
            double alpha = alpha_;
            dxi = -alpha * accumulatedJacobian;
            //std::cout << "\nINterpolate jacobian : = " << accumulatedJacobian.transpose() << ", and increment update = " << dxi << std::endl;
        }
        // 3.4-B) Bengio's NAG: a modification to NAG as proposed in Bengio-2013. Fixed parameters
        //          1) momentum or velocity  v_k = beta_k-1 v _k-1 - alpha_k-1 Grad f (x_k-1)
        //          2) x_k+1 = x_k + beta_k+1 beta_k * v_k - (1 + beta_k+1)*alpha_k * Grad f(x_k)
        if (solveMode_ == SolveMode::GRADIENT_BENGIOS_NAG)
        {
            double alpha = alpha_;
            double beta = beta_;
            // x update
            dxi = beta * beta * previousState_.back() - (1 + beta) * alpha * accumulatedJacobian;

            // momentum
            previousState_.back() = beta * previousState_.back() - alpha * accumulatedJacobian;
        }
        trajectory_->back().update_lhs(dxi);
        xiFinal = trajectory_->back().ln_vee();
        for (uint_t t = 1 ; t < numberPoses_-1; ++t)
        {
            dxi = tau * t * xiFinal;// SE3 does not like all derived classes TODO
            trajectory_->at(t) = SE3(dxi);
        }
        ++solveIters_;
    }while(fabs(diffError) > 1e-4 && !singleIteration && solveIters_ < 1e4);
    return solveIters_;
}

uint_t PlaneRegistration::solve_gradient_all_poses(bool singleIteration)
{
    // This function is from the first implementation too, is just kept for comparisons in a full traj optimization
    solveIters_ = 0;
    double previousError = 1e20, diffError = 10;
    do
    {
        // 1) calculate plane estimation given the current trajectory
        double  initialError = get_current_error();
        diffError = previousError - initialError;
        previousError = initialError;

        //std::cout << "current error iteration " << solveIters_ << " = "<< initialError << std::endl;

        // 2) calculate Gradient
        MatX1 jacobian = MatX1::Zero(numberPoses_*6);
        double  numberPoints, tau = 1.0 / (double)(numberPoses_-1);
        for (uint_t t = 1 ; t < numberPoses_; ++t)
        {
            numberPoints = 0.0;
            for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
            {
                jacobian.segment<6>(t*6) += it->second->calculate_gradient(t);
                numberPoints += it->second->get_number_points(t);
            }

        }
        // 3) update results Tf = exp(-dxi) * Tf (our convention, we expanded from the left)
        // 3-1)
        Mat61 dxi;
        for (uint_t t = 1 ; t < numberPoses_; ++t)
        {
            Mat61 dxi = -(alpha_ / numberPoints / numberPoses_ )*jacobian.segment<6>(t*6);
            trajectory_->at(t).update_lhs(dxi);
        }
        ++solveIters_;
    }while(fabs(diffError) > 1e-4 && !singleIteration && solveIters_ < 1e4);
    return solveIters_;
}

uint_t PlaneRegistration::solve_quaternion_plane()
{
    solveIters_ = 0;
    double previousError = 1e20, diffError = 10;

    // TODO create factor graph or call dense solver?

    return solveIters_;
}

uint_t PlaneRegistration::solve_initialize()
{
    // Initialize matrices of , all points in the Y matrix
    std::vector<Mat31> Y_points_all;
    Y_points_all.reserve(numberPlanes_);

    // update data structure for init. All point in the fist pose
    for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
    {
        it->second->calculate_all_matrices_S();
        if (it->second->get_number_points(0) > 0 )
            Y_points_all.push_back(it->second->get_mean_point(0));
    }

    uint_t t = 0;
    // create points Y,X, for t = 1, ... T, minimum 3 planes per pose
    std::vector<Mat31> X_points, Y_points;
    X_points.reserve(numberPlanes_);
    Y_points.reserve(numberPlanes_);
    uint_t number_points;

    for (t = 1; t < numberPoses_; ++t)
    {
        X_points.clear();
        Y_points.clear();
        number_points = 0;
        uint_t cont =0;
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        {
            if (it->second->get_number_points(t) > 0 )
            {
                X_points.push_back(it->second->get_mean_point(t));
                Y_points.push_back(Y_points_all[cont]);
                number_points++;
            }
            cont++;
        }
        // Create the matrices X,Y
        MatX X(number_points,3), Y(number_points,3);
        for (uint_t i = 0; i < number_points; ++i)
        {
            X.row(i) = X_points[i];
            Y.row(i) = Y_points[i];
        }
        // Arun solver
        SE3 estimatedPose;
        if (!PCRegistration::arun(X,Y,estimatedPose))
            return 0;
        trajectory_->at(t) = estimatedPose;
    }

    // update current trajectory
    return 1;
}

double PlaneRegistration::get_current_error() const
{
    double  currentError = 0.0;
    for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        currentError += it->second->estimate_plane();
    return currentError;
}

void PlaneRegistration::set_last_pose(SE3 &last)
{
    Mat61 xiFinal = last.ln_vee();
    double  tau = 1.0 / (double)(numberPoses_-1);
    Mat61 dxi;
    for (uint_t t = 1 ; t < numberPoses_; ++t)
    {
        dxi = tau * t * xiFinal;
        trajectory_->at(t) = SE3(dxi);
    }
}

void PlaneRegistration::add_plane(uint_t id, std::shared_ptr<Plane> &plane)
{
    plane->set_trajectory(trajectory_);
    planes_.emplace(id, plane);
}

void PlaneRegistration::add_new_plane(uint_t id)
{
    std::shared_ptr<Plane> plane(new Plane(numberPoses_));
    plane->set_trajectory(trajectory_);
    planes_.emplace(id, plane);
}

void PlaneRegistration::plane_push_back_point(uint_t id, uint_t t, Mat31 &point)
{
    planes_.at(id)->push_back_point(point,t);
}

uint_t PlaneRegistration::calculate_total_number_points()
{
    numberPoints_ = 0;
    for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        numberPoints_ += it->second->get_total_number_points();
    return numberPoints_;
}

double PlaneRegistration::calculate_poses_rmse(std::vector<SE3> & groundTruth) const
{
    assert(groundTruth.size() >= numberPoses_ && "PlaneRegistration::calculate_poses_rmse: number of poses from GT is incorrect\n");
    double rmse= 0.0;
    uint_t t = 0;
    for (auto &pose: groundTruth)
    {
        Mat61 dxi = (trajectory_->at(t) * pose.inv()).ln_vee();
        //std::cout << pose.ln_vee().transpose() << " and solution inv first pose \n" << (invFirstPose * trajectory_->at(t) * pose.inv()).ln_vee().transpose() <<std::endl;
        rmse += dxi.dot(dxi)/(double)numberPoses_;
        ++t;
    }
    return std::sqrt(rmse);
}

//XXX this can be a reference, who does this interface with pybinds?
std::vector<Mat31> PlaneRegistration::get_point_cloud(uint_t time)
{
	std::vector<Mat31> aggregated_pc;
	for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
    {
        std::vector<Mat31>& plane_pc = it->second->get_points(time);
        // aggregating elements
        aggregated_pc.insert(aggregated_pc.end(), plane_pc.begin(), plane_pc.end());
	}
	return aggregated_pc;
}

SE3 PlaneRegistration::get_trajectory(uint_t time)
{
    assert(time < numberPoses_ && "CreatePoints::getPointCloud: temporal index larger than number of calculated poses\n");
    if (time < numberPoses_ )
        return trajectory_->at(time);
    return SE3();
}

void PlaneRegistration::print(bool plotPlanes) const
{
    std::cout << "Printing plane registration data :"<< std::endl;
    for (SE3 &transf : *trajectory_)
        transf.print();
    if (plotPlanes)
    {
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
            it->second->print();
    }
}

//         Nplanes[0], Nposes[1], Npoints[2], iters[3],  process-time[4],
//         ini_error[5] error[6], eigenvalues[7-12]
std::vector<double> PlaneRegistration::print_evaluate()
{
    std::vector<double> result(13,0.0);

    result[0] = numberPlanes_;
    result[1] = numberPoses_;
    calculate_total_number_points();
    result[2] = numberPoints_;
    result[3] = solveIters_;
    double k = 1.0;//numberPoints_;
    result[4] = time_profiles_.total_time();
    result[5] = initial_error_/k;
    result[6] = get_current_error()/k; //TODO this does not work for GN (but yes to GN ini...)

    MatX allPlanes(numberPlanes_,4);
    MatX allNormals(numberPlanes_,3);
    uint_t i = 0;

    switch(solveMode_)
    {
        case SolveMode::GRADIENT:
        case SolveMode::GRADIENT_BENGIOS_NAG:
        case SolveMode::GRADIENT_ALL_POSES:
            hessian__.setZero();
            break;
        case SolveMode::GN_HESSIAN:
        case SolveMode::LM_SPHER:
        case SolveMode::LM_ELLIP:
            gradient__ = gradient_;
            hessian__ = hessian_;
    }

    // Normals on planes, check for rank
    for (auto plane : planes_)
    {
        Mat41 pi = plane.second->get_plane();
        //std::cout << "plane : \n" << pi << std::endl;
        allPlanes.row(i) = pi;
        allNormals.row(i) = pi.head(3)/(pi.head(3).norm());
        ++i;
    }
    // Orthogonality between planes (4 dim)
    //std::cout << "current gradient \n" << gradient__ << std::endl;
    // XXX : NO, Orthogonolaity was not an issue
    //std::cout << "solution\n" << allPlanes << "\nOrthogonality between planes: \n" << allPlanes * allPlanes.transpose() <<
    //              "\n and det  = \n" << allPlanes.determinant() << std::endl;

    // XXX No, Orthogonality between normals
    //std::cout << "Orthogonality between normals: \n" << allNormals * allNormals.transpose() <<
    //             "\n and det = \n" << allNormals.determinant() << std::endl;

    // Hessian rank and eigen, look for negative vaps. Lasta hessina calculateds
    Eigen::SelfAdjointEigenSolver<MatX> eigs(hessian__);
    //std::cout << "eigen values are: \n" << eigs.eigenvalues() << std::endl;
    // Determinant of stacked normals
    //std::cout << "det(Hessian) = \n" << hessian__ << std::endl;

    for (uint_t i = 0; i < 6; ++i)
        result[7+i] = eigs.eigenvalues()(i);

    return result;
}

// From parent class Optimizer:
// TOOD for now replicated, later we will substitute
matData_t PlaneRegistration::calculate_error()
{
    return get_current_error();
}

void PlaneRegistration::calculate_gradient_hessian()
{
    Mat61 gradient = Mat61::Zero();
    Mat6 hessian = Mat6::Zero();
    gradient_.setZero();
    hessian_.setZero();
    get_current_error();//this recalculates the current planes estimation, required for LM proper undoing.
    double  tau = 1.0 / (double)(numberPoses_-1);
    for (uint_t t = 1 ; t < numberPoses_; ++t)
    {
        gradient.setZero();
        hessian.setZero();
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        {
            gradient += it->second->calculate_gradient(t);
            hessian += it->second->calculate_hessian(t);
        }
        // TODO this should be changed to time stamps later
        gradient_ +=  (tau *  t)  * gradient;
        hessian_ += (tau *  t) * hessian.selfadjointView<Eigen::Upper>();
    }
}

void PlaneRegistration::update_state(const MatX1 &dx)
{
    trajectory_->back().update_lhs(dx);
    Mat61 xiFinal = trajectory_->back().ln_vee();
    double  tau = 1.0 / (double)(numberPoses_-1);
    Mat61 dxi;
    for (uint_t t = 1 ; t < numberPoses_-1; ++t)
    {
        dxi = tau * t * xiFinal;
        trajectory_->at(t) = SE3(dxi);
    }
}

void PlaneRegistration::bookkeep_state()
{
    bookept_trajectory_ = trajectory_->back();
}

void PlaneRegistration::update_state_from_bookkeep()
{
    trajectory_->back() = bookept_trajectory_;
    Mat61 xiFinal = bookept_trajectory_.ln_vee();
    double  tau = 1.0 / (double)(numberPoses_-1);
    Mat61 dxi;
    for (uint_t t = 1 ; t < numberPoses_-1; ++t)
    {
        dxi = tau * t * xiFinal;
        trajectory_->at(t) = SE3(dxi);
    }
    calculate_error();// planes get recalculated, which is a requisite for later
    //(this class construction, in general grad should be self-contained...)
}


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

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <vector>
#include <cmath>
#include <cstring>

#include <unistd.h>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
#include "NRGOpMatRules.hpp"
#include "NRG_main.hpp"
#include "TwoChQS.hpp"

#ifndef pi
#define pi 3.141592653589793238462643383279502884197169
#endif

#ifndef _CHIN_
#define _CHIN_

double chiN(int Nsites, double Lambda)
{

  double daux[3];
  daux[0]=1.0-pow( Lambda,-((double)(Nsites)+1.0) );
  daux[1]=sqrt( 1.0-pow(Lambda,-(2.0*(double)(Nsites)+1.0)) );
  daux[2]=sqrt( 1.0-pow(Lambda,-(2.0*(double)(Nsites)+3.0)) );

  return(daux[0]/(daux[1]*daux[2]));

}

#endif

int main (int argc, char* argv[]){

// Parameters for command-line passing (GetOpt)

  CNRGCodeHandler ThisCode;
  CNRGarray Aeig;

#include"ModelOptMain.cpp"

  ThisCode.SaveData=false;

  // NRG objects

  CNRGbasisarray AeigCut;
  CNRGbasisarray Abasis;
  CNRGbasisarray SingleSite;

  // STL vector

  CNRGmatrix HN;
  CNRGmatrix Qm1fNQ;

  //CNRGmatrix MQQp1;

  CNRGmatrix* MatArray;
  int NumNRGarrays=4;
  // Jul 09: Will this work???
  vector<CNRGmatrix> STLMatArray;
  CNRGmatrix auxNRGMat;
  // MatArray 0 is f_ch1
  // MatArray 1 is f_ch2
  // MatArray 2 is Sz
  // MatArray 3 is Sz2

  // STL vectors
  vector <double> Params;
  vector <double> ParamsHN;
  vector <double> ParamsBetabar;
  vector<int> CommonQNs;
  vector<int> totSpos;

  // Thermodynamics

  CNRGthermo Suscep;
  CNRGthermo Entropy;

  CNRGthermo *ThermoArray;
  int NumThermoArrays=2;

  double TM=0.0;
  //double DN=0.0;
  //double betabar=0.727;
  ThisCode.Nsites=0;
  ThisCode.betabar=0.727;
  // Add more than one betabar in the code... Done!
  //ThisCode.betabar=0.6;

  // instream
  ifstream InFile;
  // outstream
  ofstream OutFile;

  // Allows for several channels
  vector <double> chi_m1;

  ////////////////////////////////////////////////////
  ////////////////////////////////////////////////////
  ////////////////////////////////////////////////////
  ///                                              ///
  ///                Main code                     ///
  ///                                              ///
  ////////////////////////////////////////////////////
  ////////////////////////////////////////////////////
  ////////////////////////////////////////////////////

  // Read stuff (separate routine)

// Allocate MatArray (hope this works!)
// Yes, but, for some reason, needs to be BEFORE everything!

   MatArray = new CNRGmatrix [NumNRGarrays];
   ThermoArray = new CNRGthermo [NumThermoArrays];

   for (int imat=0;imat<NumNRGarrays;imat++){STLMatArray.push_back(auxNRGMat);}

   ThisCode.Nsites0=0;
   ThisCode.NumChannels=1;
   ThisCode.NumNRGmats=NumNRGarrays;
   ThisCode.NumThermoMats=NumThermoArrays;
   ThisCode.InitialSetUp();
   //InFile.open("input_nrg.dat"); // Needs this here
                                   // new MatArray after!! Why????

   ThisCode.SetSingleSite(&SingleSite);

   // Copy pointers for saving/reading.
   ThisCode.pAbasis=&Abasis;
   //ThisCode.MatArray=MatArray;
   ThisCode.MatArray=&STLMatArray[0];
   ThisCode.pAcut=&AeigCut;

  // Set H0
  // Set initial Aeig and matrices (such as Qm1fNQ[])/Operators
  //  - Model dependent functions (hardest part)
  //  - Set quantum numbers, etc,etc,
  //  - HN. will

  // Thermobasics

   strcpy(ThermoArray[0].ArqName,"SuscepImp_Main.dat");
   strcpy(ThermoArray[0].ChainArqName,"SuscepChain2Ch.dat");
   ThermoArray[0].Calc=CalcSuscep;

   strcpy(ThermoArray[1].ArqName,"EntropyImp_Main.dat");
   strcpy(ThermoArray[1].ChainArqName,"EntropyChain2Ch.dat");
   ThermoArray[1].Calc=CalcEntropy;

   for (int ithermo=0;ithermo<NumThermoArrays;ithermo++){
     if (ThisCode.calcdens==3)
       ThermoArray[ithermo].CalcChain=true;
     else
       ThermoArray[ithermo].CalcChain=false;
   }
   // HN basics
   HN.NeedOld=false;
   HN.UpperTriangular=true;
   HN.CheckForMatEl=Diag_check;

   // To do: send ALL THIS to codeHandler!!
   /// For all codes
   double Gamma=0.0282691;
   double Lambda=2.5;
   if (ThisCode.dInitParams.size()>1)
     Gamma=ThisCode.dInitParams[1];
   if (ThisCode.Lambda>0.0)
     Lambda=ThisCode.Lambda;
   //double HalfLambdaFactor=0.5*(1.0+(1.0/Lambda)); // also defined in CNRGcodehandler
   double HalfLambdaFactor=ThisCode.HalfLambdaFactor; // fix this double counting later

//    double chi_m1=sqrt(2.0*Gamma/pi)/(sqrt(Lambda)*HalfLambdaFactor);
   // NumChannels was set in ThisCode.InitialSetUp(). Set
   chi_m1.insert(chi_m1.begin(),ThisCode.NumChannels,0.0);
   chi_m1[0]=sqrt(2.0*Gamma/pi)/(sqrt(Lambda)*HalfLambdaFactor);

   // added: eN for chains away from phs (getting en for N=0)
   double eN=ThisCode.Chains[0].GetEn(0);

   cout << " Fsq = " << ThisCode.Chains[0].Fsq
	<< " chi_m1 = " << chi_m1[0]
	<< " eN(0) = " << eN << endl;

   // Adding A_Lambda factor (except if using Campo-Oliveira)
   if (ThisCode.Chains[0].DiscScheme!=1){
     chi_m1[0]*=sqrt(0.5*log(Lambda)*(Lambda+1)/(Lambda-1));
   }

   cout  << " sqrt(ALambda)*chi_m1 = " << chi_m1[0]  << endl;

   // Sep 08: 1ChQ chain calculations diverted to Anderson (ModelNo=0)
   // Aug 09: Why?? Is this correct?

   if ( (ThisCode.ModelNo==3)&&(ThisCode.SymNo==3) ){
     ThisCode.ZeroParams();
     ThisCode.ModelNo=0;
   }

   double DeltaSC=0.0;
   // Set 2015: added SC leads
   if ( (ThisCode.SymNo==7)||(ThisCode.SymNo==8) ){
     if (ThisCode.ModelNo==0){
       DeltaSC=ThisCode.dInitParams[3];
       cout << " DeltaSC = " << DeltaSC << endl;
       }
   }
   // end add DeltaSC

   /***************/
   // Model specific hamiltonians //
   /***************/

   // to do: models for which Nsites0=1 need eN as input here

   ThisCode.ModelSwitch(CommonQNs,totSpos,&Aeig,&SingleSite,
			&HN,STLMatArray,ThermoArray,chi_m1);

   CutStates(Aeig, AeigCut, ThisCode.Ncutoff);

   // watch out for the "Nsites0-2"
   ThisCode.DN=HalfLambdaFactor*pow(Lambda,(-(ThisCode.Nsites0-2)/2.0) );
   // Aug 2010: Z-trick factor added into HalfLambdaFactor
   TM=ThisCode.DN/ThisCode.betabar;
   cout << "DN = " << ThisCode.DN << "  TM = " << TM << endl;

   //
   for (int ithermo=0;ithermo<NumThermoArrays;ithermo++){
     ThisCode.ThermoSTLArray.push_back(ThermoArray[ithermo]);
   }

   if (ThisCode.Nsites0==1) // Calculates Thermo from N=0 on (not N=-1)
     ThisCode.CalcThermo(ThermoArray,&Aeig);

   ThisCode.CalcStuff(&Aeig,&AeigCut);

   // Save Params?
   if (ThisCode.SaveData){ThisCode.SaveGenPars();}

   // Ok, with that we can go into the main loop

   // Parameters for HN (always the same?)
   ParamsHN.push_back(Lambda);
   for (int ich=0;ich<ThisCode.NumChannels;ich++)
     ParamsHN.push_back(chi_m1[ich]);
   // This is not good. Depends on setting chi_m1 at ModelSwitch!!
   // Also: if Nsites0 is NOT -1, we need something like this
   //        ParamsHN[ich+1]=ThisCode.chain.GetChin(ThisCode.Nsites);
   // What if the chains are not equivalent?

   // added: chains away from phs
   ParamsHN.push_back(eN);
   // added: superconducting leads
   ParamsHN.push_back(DeltaSC);

   // Entering calculation of H_Nsites0
   ThisCode.Nsites=ThisCode.Nsites0;

   while (ThisCode.Nsites<ThisCode.Nsitesmax){
     ThisCode.DN=HalfLambdaFactor*pow(Lambda,(-(ThisCode.Nsites-1)/2.0) );
     // Aug 2010: Z-trick factor added into HalfLambdaFactor
     TM=ThisCode.DN/ThisCode.betabar;
     cout << "DN = " << ThisCode.DN << " TM = " << TM << endl;
     // Debugging...
     //bool disp=(ThisCode.Nsites==1?true:false);
     //bool disp=true;
     bool disp=false;

     // Build Basis

     //SingleSite.PrintQNumbers();
     //Aeig.PrintQNumbers();

     // If UpdateBefCut=1, StCameFrom is the "old" (Before Cutting) one!
     // Matrices will be in sync with Aeig, not AeigCut

     BuildBasis(CommonQNs, totSpos,&AeigCut,&Abasis,
		&SingleSite,ThisCode.UpdateBefCut);

     // Diagonalize HN

     cout << "Diagonalizing H(N="<<ThisCode.Nsites<<")... " << endl;
     cout << " chi_N = " << ParamsHN[1] << endl;

     // Need to adapt ALL models to use STLMatArray...
     // in the meantime, this will work.
     if ( (ThisCode.ModelNo==4) ){
       HN.DiagHN(ParamsHN,&Abasis,&SingleSite,MatArray,&Aeig);
     }else{
       HN.DiagHN(ParamsHN,&Abasis,&SingleSite,&STLMatArray[0],&Aeig,disp);
     }

     cout << "... done diagonalizing HN. " << endl;

     Aeig.SetKept(ThisCode.Ncutoff);
     Aeig.PrintEn();

     //       for (int ibl=0;ibl<Aeig.NumBlocks();ibl++)
     // 	Aeig.PrintBlock(ibl);

     // Calculate Susceptibility/Entropy (does not need update)

     ThisCode.CalcThermo(ThermoArray,&Aeig);

     // NEW (updates after cutting)

     if (ThisCode.UpdateBefCut==0){
       // Update AFTER CUTTING (faster)
       CutStates(Aeig, AeigCut, ThisCode.Ncutoff);
     }else{
       // Update BEFORE CUTTING
       //Aeig.SetKept(ThisCode.Ncutoff);
       CutStates(Aeig, AeigCut, 2*Aeig.Nstates());
       // AeigCut is essentially a copy of Aeig
     }
     // end if UpdateBefCut

     // Update matrices (in either case)

     if (ThisCode.Nsites<ThisCode.Nsitesmax){
       // Need to adapt ALL models to use STLMatArray...
       // in the meantime, this will work.
       if ( (ThisCode.ModelNo==4) ){
	 UpdateMatrices(&SingleSite,&AeigCut,
			&Abasis,MatArray, ThisCode.NumNRGmats);

       }else{
	 //disp=(ThisCode.Nsites==1?true:false);
	 // UpdateMatrices_uBLAS(&SingleSite,&AeigCut,
	 // 		&Abasis,&STLMatArray[0],STLMatArray.size(),disp);
	 // Testing...
	 UpdateMatrices(&SingleSite,&AeigCut,
	 		&Abasis,&STLMatArray[0],STLMatArray.size(),disp);
	 // Lightning FAST!!
	 //disp=false;
       }
     }
     // end update matrices

     // Save stuff to files

     if (ThisCode.SaveData){ThisCode.SaveArrays();}

     // Calculate Other things that need updated matrices
     //
     // The idea is to eliminate this and put into CalcStuff
     // (need to test with OneChQ!!
     if (ThisCode.SymNo==3){
       ParamsBetabar.clear();
       ParamsBetabar.push_back(ThisCode.betabar);
       double dSz=CalcOpAvg(ParamsBetabar,&AeigCut,&STLMatArray[2],false,0);
       double dSz2=CalcOpAvg(ParamsBetabar,&AeigCut,&STLMatArray[3],false,0);
       //double dSz=CalcOpAvg(ParamsBetabar,&AeigCut,MatArray,false,0);
       //double dSz2=CalcOpAvg(ParamsBetabar,&AeigCut,MatArray,false,0);

       cout << " T = " << TM << " Sz = " << dSz << " Sz2 = " << dSz2
	    << "  T_M chi = " << dSz2-dSz*dSz << endl;
     }

     ThisCode.CalcStuff(&Aeig,&AeigCut);

     if (ThisCode.UpdateBefCut==1){
       // Now we cut for real
       CutStates(Aeig, AeigCut, ThisCode.Ncutoff);
     }

     cout << " N = " << ThisCode.Nsites << endl;

     // Update ParamsHN
     //ParamsHN[1]=chiN(ThisCode.Nsites,Lambda); // Old
     for (int ich=0;ich<ThisCode.NumChannels;ich++)
       //ParamsHN[ich+1]=chiN(ThisCode.Nsites,Lambda);
       // Try this...
       ParamsHN[ich+1]=ThisCode.Chains[0].GetChin(ThisCode.Nsites);

     // Update sites
     ThisCode.Nsites++;

     // Adding eN: is this before or after Nsites is updated? After!
     if (ParamsHN.size()>ThisCode.NumChannels+2){
       eN=ThisCode.Chains[0].GetEn(ThisCode.Nsites);
       ParamsHN[ThisCode.NumChannels+1]=eN;
       cout << " eN_" << ThisCode.Nsites << " = " << eN << endl;
       // Adding DeltaSC RENORMALIZED
       double ScaleFactorNm1=ThisCode.Chains[0].ScaleFactor(ThisCode.Nsites-1);
       ParamsHN[ThisCode.NumChannels+2]=ScaleFactorNm1*DeltaSC;
       cout << " ScaleFactorN_" << ThisCode.Nsites-1 << " = " << ScaleFactorNm1
	    << " DeltaSC_scaled = " << " DeltaSC_scaled = " << ScaleFactorNm1*DeltaSC
	    << endl;

     }
     // apparently this is ALWAYS true!

   }
   // end Nsites loop

   // De-allocate MatArray
   delete[] ThermoArray;
   delete[] MatArray;

   ThisCode.WrapUp();

}
// end main

//
// Trash
//
//////////////////////////


#include "FWCore/Utilities/interface/EDMException.h"
#include "PhysicsTools/Utilities/interface/Parameter.h"
#include "PhysicsTools/Utilities/interface/Gaussian.h"
#include "PhysicsTools/Utilities/interface/Numerical.h"
#include "PhysicsTools/Utilities/interface/Exponential.h"
#include "PhysicsTools/Utilities/interface/Polynomial.h"
#include "PhysicsTools/Utilities/interface/Constant.h"
#include "PhysicsTools/Utilities/interface/Operations.h"
#include "PhysicsTools/Utilities/interface/MultiHistoChiSquare.h"
#include "PhysicsTools/Utilities/interface/RootMinuit.h"
#include "PhysicsTools/Utilities/interface/RootMinuitCommands.h"
#include "PhysicsTools/Utilities/interface/FunctClone.h"
#include "PhysicsTools/Utilities/interface/rootPlot.h"
#include "PhysicsTools/Utilities/interface/Expression.h"
#include "PhysicsTools/Utilities/interface/HistoPdf.h"
#include "TROOT.h"
#include "TH1.h"
#include "TFile.h"
#include <boost/program_options.hpp>
using namespace boost;
namespace po = boost::program_options;

#include <iostream>
#include <algorithm>
#include <exception>
#include <iterator>
#include <string>
#include <vector>
using namespace std;

// A helper function to simplify the main part.
template<class T>
ostream& operator<<(ostream& os, const vector<T>& v) {
  copy(v.begin(), v.end(), ostream_iterator<T>(cout, " "));
  return os;
}

//A function that sets istogram contents to 0
//if they are too small
void fix(TH1* histo) {
  for(int i = 1; i <= histo->GetNbinsX(); ++i) {
    if(histo->GetBinContent(i) < 0.1) {
      histo->SetBinContent(i, 0.0);
      histo->SetBinError(i, 0.0);
    }
  }
}

typedef funct::FunctExpression Expr;
typedef fit::MultiHistoChiSquare<Expr, Expr, Expr, Expr> ChiSquared;

struct sig_tag;
struct bkg_tag;

int main(int ac, char *av[]) {
  gROOT->SetStyle("Plain");
  try {

    double fMin, fMax;
    string ext, variable, muCharge;
    int binNumber;
    po::options_description desc("Allowed options");
    desc.add_options()
      ("help,h", "produce help message")
      ("input-file,i", po::value< vector<string> >(), "input file")
      ("min,m", po::value<double>(&fMin)->default_value(60), "minimum value for fit range")
      ("max,M", po::value<double>(&fMax)->default_value(120), "maximum value for fit range")
      ("eta_or_pt,v", po::value<string>(&variable)->default_value("eta"), "variable to study (eta or pt)")
      ("charge,q", po::value<string>(&muCharge)->default_value("minus"),"muon charge to study (minus or plus)")
      ("binNum,b", po::value<int>(&binNumber)->default_value(0), "cynematic bin to fit")
      ("plot-format,p", po::value<string>(&ext)->default_value("ps"),
       "output plot format")
      ;

    po::positional_options_description p;
    p.add("input-file", -1);

    po::variables_map vm;
    po::store(po::command_line_parser(ac, av).
	    options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << "Usage: options_description [options]\n";
      cout << desc;
      return 0;
      }

    fit::RootMinuitCommands<ChiSquared> commands("csa08NewZFit.txt");

    const unsigned int rebinMuMu = 1, rebinMuTk = 2, rebinMuSa = 1;
    // assume that the bin size is 1 GeV!!!
    funct::Constant rebinMuMuConst(rebinMuMu), rebinMuTkConst(rebinMuTk), rebinMuSaConst(rebinMuSa);

    if (vm.count("input-file")) {
      cout << "Input files are: "
	   << vm["input-file"].as< vector<string> >() << "\n";
      vector<string> v_file = vm["input-file"].as< vector<string> >();
      for(vector<string>::const_iterator it = v_file.begin();
	  it != v_file.end(); ++it) {
	TFile * root_file = new TFile(it->c_str(),"read");
	cout <<"start " << endl;
	// variable and charge definition at moment in manual way
	//	string variable = "eta";
	//	string muCharge = "minus";
	////////////////////////////////////////

	stringstream sslabelZMuMu2HLT;
	sslabelZMuMu2HLT << "zMuMu_efficiencyAnalyzer/" << variable << "Intervals" << "/zmumu2HLT" << muCharge << "_" << variable << "Range" << binNumber;
	stringstream sslabelZMuMu1HLT;
	sslabelZMuMu1HLT << "zMuMu_efficiencyAnalyzer/" << variable << "Intervals" << "/zmumu1HLT" << muCharge << "_" << variable << "Range" << binNumber;
	stringstream sslabelZMuTk;
	sslabelZMuTk << "zMuMu_efficiencyAnalyzer/" << variable << "Intervals" << "/zmutrack" << muCharge << "_" << variable << "Range" << binNumber;
	stringstream sslabelZMuSa;
	sslabelZMuSa << "zMuMu_efficiencyAnalyzer/" << variable << "Intervals" << "/zmusta" << muCharge << "_" << variable << "Range" << binNumber;

	cout << "histoZMuMu2HLT:   " << sslabelZMuMu2HLT.str() << endl;
	TH1D * histoZMuMu2HLT = (TH1D*) root_file->Get(sslabelZMuMu2HLT.str().c_str());
	histoZMuMu2HLT->Rebin(rebinMuMu);
	fix(histoZMuMu2HLT);
	cout << "histoZMuMu1HLT:   " << sslabelZMuMu1HLT.str() << endl;
	TH1D * histoZMuMu1HLT = (TH1D*) root_file->Get(sslabelZMuMu1HLT.str().c_str());
	histoZMuMu1HLT->Rebin(rebinMuMu);
	fix(histoZMuMu1HLT);
	cout << "histoZMuTk:   " << sslabelZMuTk.str() << endl;
	TH1D * histoZMuTk = (TH1D*) root_file->Get(sslabelZMuTk.str().c_str());
	//	histoZMuTk->Rebin(rebinMuTk);
	fix(histoZMuTk);
	cout << "histoZMuSa:   " << sslabelZMuSa.str() << endl;
	TH1D * histoZMuSa = (TH1D*) root_file->Get(sslabelZMuSa.str().c_str());
	//	histoZMuSa->Rebin(rebinMuSa);
	fix(histoZMuSa);
	cout << ">>> histogram loaded\n";

	string f_string = *it;
	replace(f_string.begin(), f_string.end(), '.', '_');
	replace(f_string.begin(), f_string.end(), '/', '_');
	string plot_string = f_string + "." + ext;
	cout << ">>> Input files loaded\n" << f_string << endl;

	const char * kYieldZMuMu = "YieldZMuMu";
	const char * kEfficiencyHLT = "EfficiencyHLT";
	const char * kEfficiencyTk = "EfficiencyTk";
	const char * kEfficiencySa = "EfficiencySa";
	const char * kYieldBkgZMuTk = "YieldBkgZMuTk";
	const char * kBeta = "Beta";
	const char * kLambda = "Lambda";
	//	const char * kA0 = "A0";
	//	const char * kA1 = "A1";
	//	const char * kA2 = "A2";

	funct::Parameter yieldZMuMu(kYieldZMuMu, commands.par(kYieldZMuMu));
	funct::Parameter effHLT(kEfficiencyHLT, commands.par(kEfficiencyHLT));
	funct::Parameter effTk(kEfficiencyTk, commands.par(kEfficiencyTk));
	funct::Parameter effSa(kEfficiencySa, commands.par(kEfficiencySa));
	funct::Parameter yieldBkgZMuTk(kYieldBkgZMuTk, commands.par(kYieldBkgZMuTk));
	funct::Parameter beta(kBeta, commands.par(kBeta));
	funct::Parameter lambda(kLambda, commands.par(kLambda));
	//	funct::Parameter a0(kA0, commands.par(kA0));
	//	funct::Parameter a1(kA1, commands.par(kA1));
	//	funct::Parameter a2(kA2, commands.par(kA2));
	funct::Constant cFMin(fMin), cFMax(fMax);

	// add zMuMu2HLT and zMuMu1HLT to build pdf
	TH1D *histoZMuMu = (TH1D *) histoZMuMu2HLT->Clone();
	histoZMuMu->Sumw2();
	histoZMuMu->Add(histoZMuMu2HLT,histoZMuMu1HLT);

	// count ZMuMu Yield
	double nZMuMu = 0;
	{
	  unsigned int nBins = histoZMuMu->GetNbinsX();
	  double xMin = histoZMuMu->GetXaxis()->GetXmin();
	  double xMax = histoZMuMu->GetXaxis()->GetXmax();
	  double deltaX =(xMax - xMin) / nBins;
	  for(unsigned int i = 0; i < nBins; ++i) {
	    double x = xMin + (i +.5) * deltaX;
	    if(x > fMin && x < fMax)
	      nZMuMu += histoZMuMu->GetBinContent(i+1);
	  }
	}

	// count ZMuMu2HLT Yield
	double nZMuMu2HLT = 0;
	{
	  unsigned int nBins = histoZMuMu2HLT->GetNbinsX();
	  double xMin = histoZMuMu2HLT->GetXaxis()->GetXmin();
	  double xMax = histoZMuMu2HLT->GetXaxis()->GetXmax();
	  double deltaX =(xMax - xMin) / nBins;
	  for(unsigned int i = 0; i < nBins; ++i) {
	    double x = xMin + (i +.5) * deltaX;
	    if(x > fMin && x < fMax)
	      nZMuMu2HLT += histoZMuMu2HLT->GetBinContent(i+1);
	  }
	}

	// count ZMuMu1HLT Yield
	double nZMuMu1HLT = 0;
	{
	  unsigned int nBins = histoZMuMu1HLT->GetNbinsX();
	  double xMin = histoZMuMu1HLT->GetXaxis()->GetXmin();
	  double xMax = histoZMuMu1HLT->GetXaxis()->GetXmax();
	  double deltaX =(xMax - xMin) / nBins;
	  for(unsigned int i = 0; i < nBins; ++i) {
	    double x = xMin + (i +.5) * deltaX;
	    if(x > fMin && x < fMax)
	      nZMuMu1HLT += histoZMuMu1HLT->GetBinContent(i+1);
	  }
	}

	// count ZMuSa Yield (too low statistis so we just check the number assuming 0 background)
	double nZMuSa = 0;
	{
	  unsigned int nBins = histoZMuSa->GetNbinsX();
	  double xMin = histoZMuSa->GetXaxis()->GetXmin();
	  double xMax = histoZMuSa->GetXaxis()->GetXmax();
	  double deltaX =(xMax - xMin) / nBins;
	  for(unsigned int i = 0; i < nBins; ++i) {
	    double x = xMin + (i +.5) * deltaX;
	    if(x > fMin && x < fMax)
	      nZMuSa += histoZMuSa->GetBinContent(i+1);
	  }
	}

	cout << ">>> count of ZMuMu yield in the range [" << fMin << ", " << fMax << "]: " << nZMuMu << endl;
	cout << ">>> count of ZMuMu2HLT yield in the range [" << fMin << ", " << fMax << "]: " << nZMuMu2HLT << endl;
	cout << ">>> count of ZMuMu1HLT yield in the range [" << fMin << ", " << fMax << "]: " << nZMuMu1HLT << endl;
	cout << ">>> count of ZMuSa yield in the range [" << fMin << ", " << fMax << "]: " << nZMuSa << endl;

	funct::RootHistoPdf zPdfMuTk(*histoZMuMu, fMin, fMax);
	zPdfMuTk.rebin(rebinMuTk);

	funct::Numerical<2> _2;
	funct::Numerical<1> _1;

	Expr zMuMu2HLTEffTerm = effTk *  effSa * effHLT;
	Expr zMuMu1HLTEffTerm = effTk *  effSa * (_1 - effHLT);
	Expr zMuTkEffTerm = effTk * (_1 - effSa);
	Expr zMuSaEffTerm = effSa * (_1 - effTk);

	Expr zMuMu2HLT = rebinMuMuConst * zMuMu2HLTEffTerm * yieldZMuMu;
	Expr zMuMu1HLT = rebinMuMuConst * zMuMu1HLTEffTerm * yieldZMuMu;

	Expr zMuTkBkg = yieldBkgZMuTk * funct::Exponential(lambda);
	  //* funct::Polynomial<2>(a0, a1, a2);
	Expr zMuTkBkgScaled = rebinMuTkConst * zMuTkBkg;
	Expr zMuTk = rebinMuTkConst * (zMuTkEffTerm * yieldZMuMu * zPdfMuTk + zMuTkBkg);
	Expr zMuSa = rebinMuSaConst * zMuSaEffTerm * yieldZMuMu;

	TH1D histoZMM2HLTCount("histoZMM2HLTCount", "", 1, fMin, fMax);
	histoZMM2HLTCount.Fill(100, nZMuMu2HLT);
	TH1D histoZMM1HLTCount("histoZMM1HLTCount", "", 1, fMin, fMax);
	histoZMM1HLTCount.Fill(100, nZMuMu1HLT);
	TH1D histoZMSCount("histoZMSCount", "", 1, fMin, fMax);
	histoZMSCount.Fill(100, nZMuSa);

	ChiSquared chi2(zMuMu2HLT, & histoZMM2HLTCount,
			zMuMu1HLT, & histoZMM1HLTCount,
			zMuTk, histoZMuTk,
			zMuSa,  & histoZMSCount,
			fMin, fMax);
	cout << "N. deg. of freedom: " << chi2.degreesOfFreedom() << endl;
	fit::RootMinuit<ChiSquared> minuit(chi2, true);
	commands.add(minuit, yieldZMuMu);
	commands.add(minuit, effHLT);
	commands.add(minuit, effTk);
	commands.add(minuit, effSa);
	commands.add(minuit, yieldBkgZMuTk);
	commands.add(minuit, lambda);
	commands.add(minuit, beta);
	//	commands.add(minuit, a0);
	//	commands.add(minuit, a1);
	//	commands.add(minuit, a2);
	commands.run(minuit);
	const unsigned int nPar = 7;//WARNING: this must be updated manually for now
	ROOT::Math::SMatrix<double, nPar, nPar, ROOT::Math::MatRepSym<double, nPar> > err;
	minuit.getErrorMatrix(err);
	std::cout << "error matrix:" << std::endl;
	for(unsigned int i = 0; i < nPar; ++i) {
	  for(unsigned int j = 0; j < nPar; ++j) {
	    std::cout << err(i, j) << "\t";
	  }
	  std::cout << std::endl;
	}
	minuit.printFitResults();

	double s;
	s = 0;
	for(int i = 1; i <= histoZMuMu2HLT->GetNbinsX(); ++i)
	  s += histoZMuMu2HLT->GetBinContent(i);
	histoZMuMu2HLT->SetEntries(s);
	s = 0;
	for(int i = 1; i <= histoZMuMu1HLT->GetNbinsX(); ++i)
	  s += histoZMuMu1HLT->GetBinContent(i);
	histoZMuMu1HLT->SetEntries(s);
	s = 0;
	for(int i = 1; i <= histoZMuTk->GetNbinsX(); ++i)
	  s += histoZMuTk->GetBinContent(i);
	histoZMuTk->SetEntries(s);
	s = 0;
	for(int i = 1; i <= histoZMuSa->GetNbinsX(); ++i)
	  s += histoZMuSa->GetBinContent(i);
	histoZMuSa->SetEntries(s);
	stringstream mybin;
	mybin << muCharge << "_" << variable << binNumber << "_";
	string ZMuMu2HLTPlot = "ZMuMu2HLTFit_muon" + mybin.str() + plot_string;
	root::plot<Expr>(ZMuMu2HLTPlot.c_str(), *histoZMuMu2HLT, zMuMu2HLT, fMin, fMax,
			  effHLT, effTk, effSa, yieldZMuMu,
			  kRed, 2, kDashed, 100,
			  "Z -> #mu #mu mass (2HLT)", "#mu #mu invariant mass (GeV/c^{2})",
			  "Events");

	string ZMuMu1HLTPlot = "ZMuMu1HLTFit_muon" + mybin.str() + plot_string;
	root::plot<Expr>(ZMuMu1HLTPlot.c_str(), *histoZMuMu1HLT, zMuMu1HLT, fMin, fMax,
			  effHLT, effTk, effSa, yieldZMuMu,
			  kRed, 2, kDashed, 100,
			  "Z -> #mu #mu mass (1HLT)", "#mu #mu invariant mass (GeV/c^{2})",
			  "Events");

	string ZMuTkPlot = "ZMuTkFit_muon" + mybin.str() + plot_string;
	root::plot<Expr>(ZMuTkPlot.c_str(), *histoZMuTk, zMuTk, fMin, fMax,
			 effHLT, effTk, effSa, yieldZMuMu,
			 yieldBkgZMuTk, lambda,
			 //a0, a1, a2,
			 kRed, 2, kDashed, 100,
                         "Z -> #mu + (unmatched) track mass", "#mu #mu invariant mass (GeV/c^{2})",
                         "Events");
	//	string ZMuTkPlot = "ZMuTkFit_muon" + muCharge + variable + binNumber + plot_string;
	TF1 funZMuTk = root::tf1_t<sig_tag, Expr>("ZMuTkFunction", zMuTk, fMin, fMax,
						  effHLT, effTk, effSa, yieldZMuMu,
						  yieldBkgZMuTk, lambda);
	funZMuTk.SetLineColor(kRed);
	funZMuTk.SetLineWidth(2);
	funZMuTk.SetLineStyle(kDashed);
	funZMuTk.SetNpx(10000);
	TF1 funZMuTkBkg = root::tf1_t<bkg_tag, Expr>("ZMuTkBack", zMuTkBkgScaled, fMin, fMax,
						     yieldBkgZMuTk, lambda);
	funZMuTkBkg.SetLineColor(kGreen);
	funZMuTkBkg.SetLineWidth(2);
	funZMuTkBkg.SetLineStyle(kDashed);
	funZMuTkBkg.SetNpx(10000);
	histoZMuTk->SetTitle("Z -> #mu + (unmatched) track mass");
	histoZMuTk->SetXTitle("#mu + (unmatched) track invariant mass (GeV/c^{2})");
	histoZMuTk->SetYTitle("Events");
	TCanvas *canvas = new TCanvas("canvas");
	histoZMuTk->Draw("e");
	funZMuTkBkg.Draw("same");
	funZMuTk.Draw("same");
	canvas->SaveAs(ZMuTkPlot.c_str());
	canvas->SetLogy();
	string logZMuTkPlot = "log_" + ZMuTkPlot;
	canvas->SaveAs(logZMuTkPlot.c_str());
	string ZMuSaPlot = "ZMuSaFit_muon" + mybin.str() + plot_string;
	root::plot<Expr>(ZMuSaPlot.c_str(), *histoZMuSa, zMuSa, fMin, fMax,
			 effHLT, effSa, effTk, yieldZMuMu,
			 kRed, 2, kDashed, 10000,
			 "Z -> #mu + (unmatched) standalone mass",
			 "#mu + (unmatched) standalone invariant mass (GeV/c^{2})",
			 "Events");

      }

    }
  }
  catch(exception& e) {
    cerr << "error: " << e.what() << "\n";
    return 1;
  }
  catch(...) {
    cerr << "Exception of unknown type!\n";
  }

  return 0;
}