//==================================================================================================
/**
  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_INTERLEAVE_ODD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_INTERLEAVE_ODD_HPP_INCLUDED

namespace boost { namespace simd
{
#if defined(DOXYGEN_ONLY)
  /**
    @ingroup group-swar
    This function object computes a vector from a combination of the two inputs.

    @par Header <boost/simd/function/interleave_odd.hpp>

    @par Semantic:

    For every parameters of types respectively T

    @code
    T r = interleave_odd(x, y);
    @endcode

    is equivalent to :

    @code
    r = [ x[1] y[1] x[3] y[3] ... x[n/2+1] y[n/2+1] ]
    @endcode

    with <tt> n = cardinal_of<T>::value </tt>

  **/
  Value interleave_odd(Value const& x, Value const& y);
#endif
} }

#include <boost/simd/function/simd/interleave_odd.hpp>

#endif


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

#include <boost/simd/arithmetic/functions/toints.hpp>
#include <boost/simd/include/functions/scalar/is_nan.hpp>
#include <boost/simd/include/functions/scalar/saturate.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/simd/sdk/config.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( toints_, tag::cpu_
                                    , (A0)
                                    , (scalar_< int_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( toints_, tag::cpu_
                                    , (A0)
                                    , (scalar_< uint_<A0> >)
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return result_type(saturate<result_type>(a0));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( toints_, tag::cpu_
                                    , (A0)
                                    , (scalar_< floating_<A0> >)
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
    #ifndef BOOST_SIMD_NO_NANS
      if (boost::simd::is_nan(a0))       return Zero<result_type>();
    #endif
      if (a0 >= Valmax<result_type>())   return Valmax<result_type>();
      if (a0 <= Valmin<result_type>())   return Valmin<result_type>();
      return result_type(a0);
    }
  };
} } }

#endif


/*
 * Copyright 2015-2017 Guillermo Frontera <guillermo.frontera@upm.es>
 *
 * 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 "random_input_generator.h"

#include <boost/random/uniform_real_distribution.hpp>

#include <log/logger.h>
#include <env/obstacle.h>
#include <exc/exception.h>
#include <geom/algorithms_2d.h>

using namespace ues::sim::pf;

const std::string component_name = "Random input generator";

typedef boost::random::uniform_real_distribution< ues::math::numeric_type > uniform_distribution;

const unsigned int random_input_generator::MAX_ITERATIONS_FACTOR = 2;

random_input_generator::random_input_generator () noexcept
    : boundaries ( ues::geom::box_2d ( 0, 0, 100, 100 ) ),
      min_height ( 1 ),
      max_height ( 8 ),
      min_side ( 2 ),
      max_side ( 5 ),
      obstacle_number ( 0 ),
      rotate_obstacles ( true )
{
}

void random_input_generator::set_boundaries ( ues::geom::box_2d boundaries ) noexcept
{
    this->boundaries = std::move ( boundaries );
}

void random_input_generator::set_heights ( ues::math::numeric_type min, ues::math::numeric_type max )
{
    if ( min > max )
    {
        throw ues::exc::exception ( "Invalid obstacle heights (minimum is greater than maximum value)", UES_CONTEXT );
    }
    this->min_height = min;
    this->max_height = max;
}

void random_input_generator::set_sides ( ues::math::numeric_type min, ues::math::numeric_type max )
{
    if ( min > max )
    {
        throw ues::exc::exception ( "Invalid obstacle side lengths (minimum is greater than maximum value)", UES_CONTEXT );
    }
    this->min_side = min;
    this->max_side = max;
}

void random_input_generator::set_obstacle_number ( unsigned int obstacle_number ) noexcept
{
    this->obstacle_number = obstacle_number;
}

void random_input_generator::set_rotation ( bool rotation )
{
    rotate_obstacles = rotation;
}

ues::geom::point<3> random_input_generator::generate_point ( const ues::env::obstacle_vector & obstacles )
{
    uniform_distribution distribution_x ( boundaries.get_min_x(), boundaries.get_max_x() );
    uniform_distribution distribution_y ( boundaries.get_min_y(), boundaries.get_max_y() );
    uniform_distribution distribution_z ( 0, max_height );
    ues::geom::point<3> result = ues::geom::point<3> ( distribution_x ( generator ), distribution_y ( generator ), distribution_z ( generator ) );

    while ( obstacles.contains_point ( result ) )
    {
        result = ues::geom::point<3> ( distribution_x ( generator ), distribution_y ( generator ), distribution_z ( generator ) );
    }

    return result;
}

input random_input_generator::generate_next_input()
{
    ues::log::logger lg;

    input result;

    uniform_distribution distribution_x ( boundaries.get_min_x(), boundaries.get_max_x() );
    uniform_distribution distribution_y ( boundaries.get_min_y(), boundaries.get_max_y() );
    uniform_distribution distribution_z ( 0, max_height );

    uniform_distribution distribution_height ( min_height, max_height );
    uniform_distribution distribution_side ( min_side, max_side );
    uniform_distribution distribution_rotation ( 0, 2 * ues::math::pi );

    const unsigned int max_iterations = obstacle_number * MAX_ITERATIONS_FACTOR;
    unsigned int num_iterations = 0;

    while ( result.environment.get_obstacles().size() < obstacle_number && num_iterations < max_iterations )
    {
        ues::math::numeric_type pos_x = distribution_x ( generator );
        ues::math::numeric_type pos_y = distribution_y ( generator );
        ues::math::numeric_type side_x = ( min_side != max_side ) ? distribution_side ( generator ) : max_side;
        ues::math::numeric_type side_y = ( min_side != max_side ) ? distribution_side ( generator ) : max_side;
        ues::math::numeric_type rotation = ( rotate_obstacles ) ? distribution_rotation ( generator ) : 0;

        ues::geom::polygon poly { { 0, 0 }, { 0, side_y }, { side_x, side_y }, { side_x, 0 } };
        poly.transform ( ues::geom::rotation_matrix_2d ( rotation ) );
        poly.transform ( ues::geom::translation_matrix_2d ( pos_x, pos_y ) );
        ues::env::obstacle obs ( poly, ( min_height != max_height ) ? distribution_height ( generator ) : max_height );

        bool intersection_found = false;
        for ( const ues::env::obstacle & other_obs : result.environment.get_obstacles() )
        {
            if ( poly.check_intersection ( other_obs.get_shape() ) )
            {
                intersection_found = true;
                break;
            }
        }

        if ( !intersection_found )
        {
            result.environment.add_obstacle ( std::move ( obs ) );
        }

        num_iterations++;
    }

    if ( result.environment.get_obstacles().size() < obstacle_number )
    {
        if ( lg.min_level() <= ues::log::WARNING_LVL )
        {
            ues::log::event e ( ues::log::WARNING_LVL, component_name, "Cannot fit that many obstacles in scenario" );
            e.message() << "Trying to generate " << obstacle_number << " obstacles but only " << result.environment.get_obstacles().size() << " could be generated.";
            lg.record ( std::move ( e ) );
        }
    }

    result.origin = generate_point ( result.environment.get_obstacles() );
    result.target = generate_point ( result.environment.get_obstacles() );

    return result;
}


// kate: replace-tabs off; indent-width 4; indent-mode normal
// vim: ts=4:sw=4:noexpandtab
/*

Copyright (c) 2010--2018,
François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
You can contact the authors at <f dot pomerleau at gmail dot com> and
<stephane at magnenat dot net>

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 the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/
#include "SurfaceNormalGICPCovariance.h"

// Eigenvalues
#include "Eigen/QR"
#include "Eigen/Eigenvalues"

#include "PointMatcherPrivate.h"
#include "IO.h"
#include "MatchersImpl.h"

#include <boost/format.hpp>

#include "DataPointsFilters/utils/utils.h"

// SurfaceNormalGICPCovarianceDataPointsFilter
// Constructor
template<typename T>
SurfaceNormalGICPCovarianceDataPointsFilter<T>::SurfaceNormalGICPCovarianceDataPointsFilter(const Parameters& params):
        PointMatcher<T>::DataPointsFilter("SurfaceNormalGICPCovarianceDataPointsFilter",
                                          SurfaceNormalGICPCovarianceDataPointsFilter::availableParameters(), params),
        knn(Parametrizable::get<int>("knn")),
        maxDist(Parametrizable::get<T>("maxDist")),
        epsilon(Parametrizable::get<T>("epsilon")),
        keepNormals(Parametrizable::get<bool>("keepNormals")),
        keepDensities(Parametrizable::get<bool>("keepDensities")),
        keepEigenValues(Parametrizable::get<bool>("keepEigenValues")),
        keepEigenVectors(Parametrizable::get<bool>("keepEigenVectors")),
        keepMatchedIds(Parametrizable::get<bool>("keepMatchedIds")),
        keepMeanDist(Parametrizable::get<bool>("keepMeanDist")),
        keepGICPCovariance(Parametrizable::get<bool>("keepGICPCovariance")),
        sortEigen(Parametrizable::get<bool>("sortEigen")),
        smoothNormals(Parametrizable::get<bool>("smoothNormals")),
        measurementCovariance(Parametrizable::get<T>("measurementCovariance")),
        normalVariance(Parametrizable::get<T>("normalVariance"))
{
}

// Compute
template<typename T>
typename PointMatcher<T>::DataPoints
SurfaceNormalGICPCovarianceDataPointsFilter<T>::filter(
        const DataPoints& input)
{
    DataPoints output(input);
    inPlaceFilter(output);
    return output;
}

template<typename T>
typename PointMatcher<T>::Matrix inverseSquareRootDiagonalMatrix(typename PointMatcher<T>::Matrix matrix)
{
    typename PointMatcher<T>::Matrix inverseSquareRootedMatrix = PointMatcher<T>::Matrix::Zero(matrix.rows(), matrix.cols());
    for(size_t i=0; i < inverseSquareRootedMatrix.rows(); ++i)
    {
        inverseSquareRootedMatrix(i, i) = 1.0 / std::sqrt(matrix(i, i));
    }
    return inverseSquareRootedMatrix;
}

// In-place filter
template<typename T>
void SurfaceNormalGICPCovarianceDataPointsFilter<T>::inPlaceFilter(
        DataPoints& cloud)
{
    typedef typename DataPoints::View View;
    typedef typename DataPoints::Label Label;
    typedef typename DataPoints::Labels Labels;
    typedef typename MatchersImpl<T>::KDTreeMatcher KDTreeMatcher;
    typedef typename PointMatcher<T>::Matches Matches;

    using namespace PointMatcherSupport;

    const int pointsCount(cloud.features.cols());
    const int featDim(cloud.features.rows());
    const int descDim(cloud.descriptors.rows());
    const unsigned int labelDim(cloud.descriptorLabels.size());

    // Validate descriptors and labels
    int insertDim(0);
    for(unsigned int i = 0; i < labelDim ; ++i)
        insertDim += cloud.descriptorLabels[i].span;
    if (insertDim != descDim)
        throw InvalidField("SurfaceNormalCovarianceDataPointsFilter: Error, descriptor labels do not match descriptor data");

    // Reserve memory for new descriptors
    const int dimNormals(featDim-1);
    const int dimDensities(1);
    const int dimEigValues(featDim-1);
    const int dimEigVectors((featDim-1)*(featDim-1));
    //const int dimMatchedIds(knn);
    const int dimMeanDist(1);
    const int dimGICPCovariance((featDim-1)*(featDim-1));

    boost::optional<View> normals;
    boost::optional<View> densities;
    boost::optional<View> eigenValues;
    boost::optional<View> eigenVectors;
    boost::optional<View> matchedValues;
    boost::optional<View> matchIds;
    boost::optional<View> meanDists;
    boost::optional<View> gicpCovariance;

    Labels cloudLabels;
    if (keepNormals)
        cloudLabels.push_back(Label("normals", dimNormals));
    if (keepDensities)
        cloudLabels.push_back(Label("densities", dimDensities));
    if (keepEigenValues)
        cloudLabels.push_back(Label("eigValues", dimEigValues));
    if (keepEigenVectors)
        cloudLabels.push_back(Label("eigVectors", dimEigVectors));
    if (keepMatchedIds)
        cloudLabels.push_back(Label("matchedIds", knn));
    if (keepMeanDist)
        cloudLabels.push_back(Label("meanDists", dimMeanDist));
    if (keepGICPCovariance)
        cloudLabels.push_back(Label("gicpCovariance", dimGICPCovariance));

    // Reserve memory
    cloud.allocateDescriptors(cloudLabels);

    if (keepNormals)
        normals = cloud.getDescriptorViewByName("normals");
    if (keepDensities)
        densities = cloud.getDescriptorViewByName("densities");
    if (keepEigenValues)
        eigenValues = cloud.getDescriptorViewByName("eigValues");
    if (keepEigenVectors)
        eigenVectors = cloud.getDescriptorViewByName("eigVectors");
    if (keepMatchedIds)
        matchIds = cloud.getDescriptorViewByName("matchedIds");
    if (keepMeanDist)
        meanDists = cloud.getDescriptorViewByName("meanDists");
    if (keepGICPCovariance)
        gicpCovariance = cloud.getDescriptorViewByName("gicpCovariance");

    using namespace PointMatcherSupport;
    // Build kd-tree
    Parametrizable::Parameters param;
    boost::assign::insert(param) ( "knn", toParam(knn) );
    boost::assign::insert(param) ( "epsilon", toParam(epsilon) );
    boost::assign::insert(param) ( "maxDist", toParam(maxDist) );

    KDTreeMatcher matcher(param);
    matcher.init(cloud);

    Matches matches(typename Matches::Dists(knn, pointsCount), typename Matches::Ids(knn, pointsCount));
    matches = matcher.findClosests(cloud);

    const auto& intensities = cloud.getDescriptorViewByName("intensity");

    // Search for surrounding points and compute descriptors
    int degenerateCount(0);
    for (int i = 0; i < pointsCount; ++i)
    {
        bool isDegenerate = false;
        // Mean of nearest neighbors (NN)
        Matrix d(featDim-1, knn);
        int realKnn = 0;
        for(int j = 0; j < int(knn); ++j)
        {
            if (matches.dists(j,i) != Matches::InvalidDist)
            {
                const int refIndex(matches.ids(j,i));
                d.col(realKnn) = cloud.features.block(0, refIndex, featDim-1, 1);
                ++realKnn;
            }
        }
        d.conservativeResize(Eigen::NoChange, realKnn);

        const Vector mean = d.rowwise().sum() / T(realKnn);
        const Matrix NN = d.colwise() - mean;

        const Matrix C((NN * NN.transpose()) / T(realKnn));
        Vector eigenVa = Vector::Zero(featDim-1, 1);
        Matrix eigenVe = Matrix::Zero(featDim-1, featDim-1);
        // Ensure that the matrix is suited for eigenvalues calculation
        Matrix pointWeightedCovarianceInScanFrame;
        if(keepGICPCovariance)
        {
            if(C.fullPivHouseholderQr().rank()+1 >= featDim-1) {
                const Eigen::EigenSolver<Matrix> solver(C);
                eigenVa = solver.eigenvalues().real();
                eigenVe = solver.eigenvectors().real();
                pointWeightedCovarianceInScanFrame = Matrix::Zero(featDim - 1, featDim - 1);
                // sort eigen vectors first
                const std::vector<size_t> idx = sortIndexes<T>(eigenVa);
                const size_t idxSize = idx.size();
                Vector tmp_eigenVa = eigenVa;
                Matrix tmp_eigenVe = eigenVe;
                for (size_t j = 0; j < idxSize; ++j) {
                    eigenVa(j, 0) = tmp_eigenVa(idx[j], 0);
                    eigenVe.col(j) = tmp_eigenVe.col(idx[j]);
                }
                Matrix projectionMatrix(2, 3);
                projectionMatrix.row(0) = eigenVe.col(1).transpose();
                projectionMatrix.row(1) = eigenVe.col(2).transpose();
                Matrix descriptorWeights(1, realKnn);
                for (int j = 0; j < int(knn); ++j) {
                    if (matches.ids(0, i) != Matches::InvalidId) {
                        descriptorWeights(0, j) = exp(
                                -0.5 * std::pow((intensities(0, matches.ids(j)) - intensities(0, i)), 2) /
                                measurementCovariance);
                    }
                }
                Matrix projectedNeighbors(2, realKnn);
                Matrix weightedProjectedNeighbors(2, realKnn);
                for (int j = 0; j < int(knn); ++j) {
                    if (matches.ids(0, i) != Matches::InvalidId) {
                        projectedNeighbors.col(j) = projectionMatrix * cloud.features.col(matches.ids(j)).head(3);
                        weightedProjectedNeighbors.col(j) = descriptorWeights(0, j) * projectedNeighbors.col(j);
                    }
                }
                T weightedSumInverse = 1 / descriptorWeights.sum();
                Vector weightedMean = weightedSumInverse * weightedProjectedNeighbors.rowwise().sum();
                Matrix weightedCovariance = Matrix::Zero(featDim - 2, featDim - 2);
                for (int j = 0; j < int(knn); ++j) {
                    if (matches.ids(0, i) != Matches::InvalidId) {
                        Vector projectedNeighborError = projectedNeighbors.col(j) - weightedMean;
                        weightedCovariance +=
                                descriptorWeights(0, j) * (projectedNeighborError * projectedNeighborError.transpose());
                    }
                }
                weightedCovariance = weightedSumInverse * weightedCovariance;
                Matrix populationCovariance = Matrix::Zero(featDim - 2, featDim - 2);
                populationCovariance(0, 0) = eigenVa(1);
                populationCovariance(1, 1) = eigenVa(2);
                Matrix populationCovarianceInverseSquareRoot = inverseSquareRootDiagonalMatrix<T>(populationCovariance);
                weightedCovariance = populationCovarianceInverseSquareRoot * weightedCovariance *
                                     populationCovarianceInverseSquareRoot;
                Matrix pointWeightedCovarianceInPlane = Matrix::Zero(featDim - 1, featDim - 1);
                pointWeightedCovarianceInPlane.topLeftCorner(featDim - 2, featDim - 2) = weightedCovariance;
                pointWeightedCovarianceInPlane(featDim - 2, featDim - 2) = normalVariance;
                Matrix rotationPlaneToScan(featDim - 1, featDim - 1);
                rotationPlaneToScan.col(0) = eigenVe.col(1);
                rotationPlaneToScan.col(1) = eigenVe.col(2);
                rotationPlaneToScan.col(2) = eigenVe.col(
                        0); // NOTE: The eigen vectors order is determined because surface normal is along the Z axis
                pointWeightedCovarianceInScanFrame =
                        rotationPlaneToScan * pointWeightedCovarianceInPlane * rotationPlaneToScan.transpose();
            }
            else
            {
                pointWeightedCovarianceInScanFrame = Matrix::Zero(featDim - 1, featDim - 1);
            }
        }
        if(keepNormals || keepEigenValues || keepEigenVectors)
        {
            if(C.fullPivHouseholderQr().rank()+1 >= featDim-1)
            {
                const Eigen::EigenSolver<Matrix> solver(C);
                eigenVa = solver.eigenvalues().real();
                eigenVe = solver.eigenvectors().real();

                if(sortEigen)
                {
                    const std::vector<size_t> idx = sortIndexes<T>(eigenVa);
                    const size_t idxSize = idx.size();
                    Vector tmp_eigenVa = eigenVa;
                    Matrix tmp_eigenVe = eigenVe;
                    for(size_t i=0; i<idxSize; ++i)
                    {
                        eigenVa(i,0) = tmp_eigenVa(idx[i], 0);
                        eigenVe.col(i) = tmp_eigenVe.col(idx[i]);
                    }
                }
            }
            else
            {
                //std::cout << "WARNING: Matrix C needed for eigen decomposition is degenerated. Expected cause: no noise in data" << std::endl;
                ++degenerateCount;
                isDegenerate = true;
            }
        }

        if(keepNormals)
        {
            if(sortEigen)
                normals->col(i) = eigenVe.col(0);
            else
                normals->col(i) = computeNormal<T>(eigenVa, eigenVe);

            // clamp normals to [-1,1] to handle approximation errors
            normals->col(i) = normals->col(i).cwiseMax(-1.0).cwiseMin(1.0);
        }
        if(keepDensities)
        {
            if(isDegenerate)
                (*densities)(0, i) = 0.;
            else
                (*densities)(0, i) = computeDensity<T>(NN);
        }
        if(keepEigenValues)
            eigenValues->col(i) = eigenVa;
        if(keepEigenVectors)
            eigenVectors->col(i) = serializeEigVec<T>(eigenVe);
        if(keepMeanDist)
        {
            if(isDegenerate)
                (*meanDists)(0, i) = std::numeric_limits<std::size_t>::max();
            else
            {
                const Vector point = cloud.features.block(0, i, featDim-1, 1);
                (*meanDists)(0, i) = (point - mean).norm();
            }
        }
        if(keepGICPCovariance)
        {
            gicpCovariance->col(i) = serializeEigVec<T>(pointWeightedCovarianceInScanFrame);
        }

    }

    if(keepMatchedIds)
    {
        matchIds.get() = matches.ids.template cast<T>();
    }

    if(smoothNormals)
    {
        for (int i = 0; i < pointsCount; ++i)
        {
            const Vector currentNormal = normals->col(i);
            Vector mean = Vector::Zero(featDim-1);
            int n=0;
            for(int j = 0; j < int(knn); ++j)
            {
                if (matches.dists(j,i) != Matches::InvalidDist)
                {
                    const int refIndex(matches.ids(j,i));
                    const Vector normal = normals->col(refIndex);
                    if(currentNormal.dot(normal) > 0.)
                        mean += normal;
                    else // flip normal vector
                        mean -= normal;

                    ++n;
                }
            }

            normals->col(i) = mean / T(n);
        }
    }

    if (degenerateCount)
    {
        LOG_WARNING_STREAM("WARNING: Matrix C needed for eigen decomposition was degenerated in " << degenerateCount << " points over " << pointsCount << " (" << float(degenerateCount)*100.f/float(pointsCount) << " %)");
    }

}

template struct SurfaceNormalGICPCovarianceDataPointsFilter<float>;
template struct SurfaceNormalGICPCovarianceDataPointsFilter<double>;


// Copyright (c) 2021 George E. Brown and Rahul Narain
//
// WRAPD uses the MIT License (https://opensource.org/licenses/MIT)
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is furnished
// to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
// Original Author: Ioannis Karamouzas
// Modified by George E. Brown (https://www-users.cse.umn.edu/~brow2327/)

#ifndef MCL_SPLITLBFGS_H
#define MCL_SPLITLBFGS_H

#include "SplitMinimizer.hpp"
#include <Eigen/Sparse>
#include "MicroTimer.hpp"
#include "LinearSolver.hpp"

namespace mcl {
namespace optlib {

// L-BFGS implementation based on Nocedal & Wright Numerical Optimization book (Section 7.2)
// DIM = dimension of the problem
// M = history window
//
// Original Author: Ioannis Karamouzas
//
template<typename Scalar, int DIM, int M=6>
class SplitLBFGS : public SplitMinimizer<Scalar,DIM> {
private:
	typedef Eigen::Matrix<Scalar,DIM,M> MatM;
	typedef Eigen::Matrix<Scalar,M,1> VecM;
	typedef Eigen::Matrix<Scalar,DIM,3> MatX3;

	std::vector<MatX3> s;
	std::vector<MatX3> y;
	std::vector<double> alpha;
	std::vector<double> rho;

	MatX3 q;
	MatX3 grad;
	MatX3 grad_old;
	MatX3 x_old;
	MatX3 x_last;

	double m_tol;
	int m_k;

	mcl::MicroTimer t;

public:
	#if USE_PARDISO
		PardisoSolver m_pardiso_solver;
	#else
		LinearSolver m_linear_solver;
	#endif
	bool show_denom_warning; // Print out warning for zero denominators
	bool m_has_A0;
	double m_kappa;

	SplitLBFGS() : show_denom_warning(false) {
		this->m_settings.max_iters = 50;
		show_denom_warning = this->m_settings.verbose > 0 ? true : false;
		m_has_A0 = false;
		m_tol = 1.e-15;
		m_kappa = 1.0;
		m_k = 0;
	}

	void update_A0(Eigen::SparseMatrix<double, Eigen::RowMajor> &A0, bool first_time = true) {
		m_has_A0 = true;
		if (first_time) {
			#ifdef USE_PARDISO
				m_pardiso_solver.compute(A0);
			#else
				m_linear_solver.compute(A0);
			#endif
		} else {
			#ifdef USE_PARDISO
				m_pardiso_solver.factorize(A0);
			#else
				m_linear_solver.factorize(A0);
			#endif
		}
		m_k = 0;
	}

	void clear_history() {
		m_k = 0;
	}

	void update_kappa(double kappa) {
		m_kappa = kappa;
	}

	void update_earlyexit_tol(double tol) {
		m_tol = tol;
	}

	// Returns number of iterations used
	int minimize(SplitProblem<Scalar,DIM> &problem, Eigen::MatrixX3d &x) {

		if(m_k == 0 && DIM==Eigen::Dynamic ){
			int dim = x.rows();
			s.resize(M);
			y.resize(M);
			alpha.resize(M);
			rho.resize(M);
			q = MatX3::Zero(dim, 3);
			grad = MatX3::Zero(dim, 3);
			grad_old = MatX3::Zero(dim, 3);
			x_old = MatX3::Zero(dim, 3);
			x_last = MatX3::Zero(dim, 3);
		}

		problem.gradient(grad);

		Scalar alpha_init = 1.0;

		int global_iter = 0;
		int max_iters = this->m_settings.max_iters;
		int verbose = this->m_settings.verbose;

		if ((grad - grad_old).cwiseProduct(grad - grad_old).sum() < 1.e-13) {
			return global_iter;
		} else {
			global_iter = 1;
		}

		bool breaking_out = false;
		for(; global_iter <= max_iters; ++global_iter) {

			x_old = x;
			grad_old = grad;
			q = grad;

			// L-BFGS first - loop recursion
			int iter = std::min(M, m_k);
			for(int i = iter - 1; i >= 0; --i){
				rho[i] = 1.0 / ((s[i].cwiseProduct(y[i])).sum());
				alpha[i] = rho[i] * (s[i].cwiseProduct(q)).sum();
				q = q - alpha[i] * y[i];
			}

			if (m_has_A0) {
				#ifdef USE_PARDISO
					for (int c = 0; c < 3; c++) {
						q.col(c) = m_pardiso_solver.solve(q.col(c));
					}
				#else
					for (int c = 0; c < 3; c++) {
						q.col(c) = m_linear_solver.solve(q.col(c));
					}
				#endif
			} else {
				throw std::runtime_error("Error: A0 is required but wasn't supplied.");
			}

			// L-BFGS second - loop recursion
			for(int i = 0; i < iter; ++i){
				Scalar beta = rho[i] * (y[i].cwiseProduct(q)).sum();
				q = q + s[i] * (alpha[i] - beta);
			}

			// is there a descent
			Scalar dir = grad.cwiseProduct(q).sum();
			if (std::isnan(dir)) {
				std::cout << "Decrement is NaN. Exiting.\n";
				exit(0);
			}
			if (dir <= 0) {
				// std::cout << "Decrement is <= 0. It is " << dir << ". Exiting.\n";
				// exit(0);
			}
			if (dir <= 0 || std::isnan(dir)) {
				q = grad;
				max_iters -= m_k;
				m_k = 0;
				alpha_init = std::min(1.0, 1.0 / grad.template lpNorm<Eigen::Infinity>() );
			}

			Scalar rate = 1.0;
			if (m_tol > 1.e-12) {
				rate = this->linesearch(x, -q, problem, alpha_init);
			}
			if( rate <= 0 ){
				if( verbose > 0 ){ printf("LBFGS::minimize: Failure in linesearch\n"); }
				return SplitMinimizer<Scalar,DIM>::FAILURE;
			}

			x_last = x;
			x -= rate * q;
			// if( problem.converged(x_last,x,grad) ){ break; }

			problem.gradient(x,grad);

			MatX3 s_temp = x - x_old;
			MatX3 y_temp = grad - grad_old;

			// update the history
			if(m_k < M){
				s[m_k] = s_temp;
				y[m_k] = y_temp;
			}
			else {
				s.erase(s.begin());
				s.push_back(s_temp);
				y.erase(y.begin());
				y.push_back(y_temp);
			}

			if (dir > 0 && (dir < m_kappa * m_tol || dir < m_kappa * 1.e-12)) {
				m_k += 1;
				x_old = x;
				x_last = x;
				breaking_out = true;
				break;
			}

			Scalar denom = y_temp.cwiseProduct(y_temp).sum();
			if( std::abs(denom) <= 0 ){
				if( show_denom_warning ){
					printf("LBFGS::minimize Warning: Encountered a zero denominator\n");
				}
				breaking_out = true;
				break;
			}
			alpha_init = 1.0;

			m_k++;
		}
		m_k = std::min(M, m_k);

		if (breaking_out) {
			return global_iter;
		} else {
			return global_iter - 1;
		}

		return global_iter;

	} // end minimize
};

}
}

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include <iostream>

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_distance_pointlike_pointlike
#endif

#include <boost/test/included/unit_test.hpp>

#include "test_distance_common.hpp"

typedef bg::model::point<double,2,bg::cs::cartesian>  point_type;
typedef bg::model::multi_point<point_type>            multi_point_type;

namespace services = bg::strategy::distance::services;
typedef bg::default_distance_result<point_type>::type return_type;

typedef bg::strategy::distance::pythagoras<> point_point_strategy;

//===========================================================================

template <typename Strategy>
void test_distance_point_point(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/point distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<point_type, point_type> tester;

    tester::apply("point(1 1)",
                  "point(0 0)",
                  sqrt(2.0), 2, strategy);
    tester::apply("point(1 1)",
                  "point(1 1)",
                  0, 0, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_point_multipoint(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/multipoint distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<point_type, multi_point_type> tester;

    tester::apply("point(1 1)",
                  "multipoint(1 1,2 1,2 2,1 2)",
                  0, 0, strategy);
    tester::apply("point(1 1)",
                  "multipoint(2 2,2 3,3 2,3 3)",
                  sqrt(2.0), 2, strategy);
    tester::apply("point(3 0)",
                  "multipoint(2 2,2 4,4 2,4 4)",
                  sqrt(5.0), 5, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_multipoint_multipoint(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/multipoint distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            multi_point_type, multi_point_type
        > tester;

    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "multipoint(1 1,2 1,2 2,1 2)",
                  0, 0, strategy);
    tester::apply("multipoint(0 0,1 0,0 1,1 1)",
                  "multipoint(2 2,2 3,3 2,3 3)",
                  sqrt(2.0), 2, strategy);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_more_empty_input_pointlike_pointlike(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "testing on empty inputs... " << std::flush;
#endif
    bg::model::multi_point<Point> multipoint_empty;

    Point point = from_wkt<Point>("point(0 0)");

    // 1st geometry is empty
    test_empty_input(multipoint_empty, point, strategy);

    // 2nd geometry is empty
    test_empty_input(point, multipoint_empty, strategy);

    // both geometries are empty
    test_empty_input(multipoint_empty, multipoint_empty, strategy);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << "done!" << std::endl;
#endif
}

//===========================================================================

BOOST_AUTO_TEST_CASE( test_all_point_point )
{
    test_distance_point_point(point_point_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_point_multipoint )
{
    test_distance_point_multipoint(point_point_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_multipoint_multipoint )
{
    test_distance_multipoint_multipoint(point_point_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_empty_input_pointlike_pointlike )
{
    test_more_empty_input_pointlike_pointlike
        <
            point_type
        >(point_point_strategy());
}


#include <iostream>
#include "CSVParser.hpp"
#include "LeastSquaresFit.hpp"
#include <vector>
#include <math.h>
#include "gnuplot-iostream.h"
#include <boost/program_options.hpp>
#include <chrono>
#include "Plot.hpp"

void usage() {
    std::cout << "Example usage:\n"
        << "make_plot --filename census_2000_all_places_sample_dimensions.csv --header SE_T001_001 --header_row_index 1 --first_data_row_index 2\n" << std::endl;
}

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

    // program arg variables
    std::string     filename;
    std::string     X_header;
    std::string     Y_header;
	index_type		num_segments;
    index_type header_row_index;
    index_type  first_data_row_index;

    // ********* start of boost::program_options code ****************
    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()
        ("help", "Print help messages")
        ("filename", po::value<std::string>(&filename)->required(),
            "CSV filename including file extension")
        ("X_header", po::value<std::string>(&X_header)->required(),
            "unique header of column")
        ("Y_header", po::value<std::string>(&Y_header)->required(),
            "unique header of column")
        ("num_segs", po::value<index_type>(&num_segments)->required(),
            "number of segments in analysis")
        ("header_row_index", po::value<index_type>(&header_row_index)->required(),
            "index of row header is located on")
        ("first_data_row_index", po::value<index_type>(&first_data_row_index)->required(),
            "index of first row of actual data");

    po::variables_map vm;
    try{
        po::store(po::parse_command_line(argc, argv, desc), vm);
        if(argc == 1) {
            usage();
            std::cout << desc << std::endl;
            return 0;
        }

        if (vm.count("help") )
        {
            std::cout << "Progressive Analytics Application" << std::endl
                      << desc << std::endl;
            return 0;
        }

      po::notify(vm); // throws on error, so do after help in case
                      // there are any problems
    }
    catch(po::error& e)
    {
      std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
      std::cerr << desc << std::endl;
      return 1;
    }
    // ********* end of boost::program_options code ****************

	// Create CSVParser
		constexpr unsigned char DELIM {','};
	std::shared_ptr<CSVParser> csvp;
	try {
		csvp = std::make_shared<CSVParser> (makeCSVParser(filename, DELIM));
	} catch(const std::runtime_error &e) {
		std::cout << e.what() << std::endl;
		return 2;
	}
	// Create LeastSquaresFit object
	using vec_cols_tX = std::vector<ColumnData<float_data_type>>;
	using vec_cols_tY = std::vector<ColumnData<float_data_type>>;

	auto Xs {csvp->makeSegments<float_data_type>(X_header,
		header_row_index, first_data_row_index, num_segments)};
	auto Ys {csvp->makeSegments<float_data_type>(Y_header,
		header_row_index, first_data_row_index, num_segments)};
	auto lsf {makeLeastSquaresFit<vec_cols_tX, vec_cols_tY>(Xs, Ys)};

	// for each segment, process ColumnData into a summary
	// and combine with previous summary, then plot summary info

	auto cur_seg {lsf.getNextSegment()};
	auto summary_aggregated {create_summary(cur_seg)};

	SummaryPlotter plotter{};
	std::string settings {"set style fill solid 0.1\n"};
	plotter.set_settings(settings);

	plotter(summary_aggregated); // plotting first segment's summary

	std::cout << "press enter to plot summary #2" << std::endl;
	std::cin.get();

	// for each iteration, get another summary of a segment of data,
	// then aggregate it with summary_aggregate
	for(int i {1}; i < num_segments; ++i) {
		auto cur_summary {create_summary(lsf.getNextSegment())};
		summary_aggregated = create_summary(summary_aggregated, cur_summary);
		plotter(cur_summary);
		std::cout << "press enter to plot summary #" << (i + 2) << std::endl;
		std::cin.get();

	}
	return 0;
}


//===-- Units ---------------------------------------------------*- C++ -*-===//
///
/// \file
/// \brief Thread
/// \sa <https://boost.org/doc/libs/master/libs/units/>
///
/// \version 2019-12-06
/// \since 2019-12-05
/// \authors zhengrr
/// \copyright Unlicense
///
//===----------------------------------------------------------------------===//

#include <boost/units/systems/information.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/io.hpp>
#include <gtest/gtest.h>

using namespace std;

namespace units = ::boost::units;
namespace information = ::boost::units::information;
namespace si = ::boost::units::si;

//
// 十分简单的需求：
// 能量存储为焦耳值，如何取得对应的卡路里值？
//

//
// 复合单位的字符串输出
//

namespace boost::units {
std::string name_string(const si::velocity &) { return "meters per second"; }
std::string symbol_string(const si::velocity &) { return "m/s"; }
}

namespace rrboost {
TEST(units, output_for_composite_units)
{
    const units::quantity<si::length> length {15.0 * si::meter};
    const units::quantity<si::time> time {15.0 * si::second};
    const units::quantity<si::velocity> velocity = length / time;
    cout << velocity << endl;
}
}

//
// 自动缩放单位（输出）
//

namespace rrboost {
TEST(units, automatically_scaled_units)
{
    const units::quantity<si::length> length {1234 * si::meter};
    cout << length << '\n';
    cout << units::engineering_prefix << length << units::no_prefix << '\n';

    const units::quantity<information::info> info {50000 * information::byte};
    cout << info << '\n';
    cout << units::binary_prefix << info << units::no_prefix << '\n';
}
}


/*
 * baseMeasure.hpp
 *
 *  Created on: Feb 1, 2013
 *      Author: jstraub
 */

#ifndef BASEMEASURE_HPP_
#define BASEMEASURE_HPP_

#include "probabilityHelpers.hpp"

#include <boost/math/special_functions/gamma.hpp>
#include <armadillo>

#include <stddef.h>
#include <stdint.h>
#include <typeinfo>

using namespace std;
using namespace arma;

/*
 * templated on the unit
 */
template<class U>
class BaseMeasure
{
public:
  BaseMeasure()
    : mRowDim(0)
  {
    //cout<<"Creating "<<typeid(this).name()<<endl;
  };

  BaseMeasure(const BaseMeasure<U>& other)
    : mRowDim(0)
  { };

  virtual ~BaseMeasure()
  {};

  virtual double predictiveProb(const Col<U>& x_q, const Mat<U>& x_given) const
  {
    cerr<<"BaseMeasure:: Something gone wrong with virtual functions"<<endl;
    exit(0);
    return 0.0;
  };
  virtual double predictiveProb(const Col<U>& x_q) const
  {
    cerr<<"BaseMeasure:: Something gone wrong with virtual functions"<<endl;
    exit(0);
    return 0.0;
  };

  virtual BaseMeasure<U>* getCopy() const
  {
    cerr<<"BaseMeasure:: getCopy()"<<endl;
    exit(0);
    return new BaseMeasure<U>(*this);
  };

  virtual Row<double> asRow() const
  {
    cerr<<"BaseMeasure:: asRow()"<<endl;
    exit(0); return Row<double>();
  };

  uint32_t rowDim() const
  {
    return mRowDim;
  };

  virtual void fromRow(const Row<double>& r)
  {
    cerr<<"BaseMeasure:: fromRow()"<<endl;
    exit(0);
  };

  virtual BaseMeasure<U>* mode() const
  {
    cerr<<"BaseMeasure:: mode()"<<endl;
    exit(0);};

  virtual double Elog(const Col<U>& x) const
  {
    cerr<<"BaseMeasure:: Elog()"<<endl;
    exit(0);};

  virtual void posteriorHDP_var(const Col<double>& zeta, const Mat<double>& phi, uint32_t D, const Mat<U>& x)
  {
    cerr<<"BaseMeasure:: posteriorHDP_var()"<<endl;
    exit(0);};

  virtual void posterior(const Mat<U>& x)
  {
    cerr<<"BaseMeasure:: posterior()"<<endl;
    exit(0);};

  virtual double logP(const Col<U>& x) const
  {
    cerr<<"BaseMeasure:: logP()"<<endl;
    exit(0);}

protected:
  uint32_t mRowDim;

};

/*
 * Multinomial distribution
 */
class Mult : public BaseMeasure<uint32_t>
{
  public:

    Mult(const Mult& mult)
      : mP(mult.mP)
    {
      mRowDim = mP.n_cols;
    };

    Mult(const Row<double>& p)
      : mP(p)
    {
      mRowDim = mP.n_cols;
    };

  virtual BaseMeasure<uint32_t>* getCopy() const
  {
    return new Mult(*this);
  };

  virtual Row<double> asRow() const
  {
    return mP;
  };

  virtual void fromRow(const Row<double>& r)
  {
    mP = r;
    mRowDim = r.n_cols;
  };

  virtual double logP(const Col<uint32_t>& x) const
  {
    return log(mP[x(0)]);
  };

  Row<double> mP;
private:
};

/*
 * Mutlivariate Gaussian distribution
 */
class Gauss : public BaseMeasure<double>
{
  public:
    Gauss(const Gauss& gauss)
      : mMu(gauss.mMu), mSig(gauss.mSig)
    {
      uint32_t d=mMu.n_rows;
      mRowDim = d*d+d;
    };

    Gauss(const Col<double>& mu, const Mat<double>& sig)
      : mMu(mu), mSig(sig)
    {
      uint32_t d=mMu.n_rows;
      mRowDim = d*d+d;
    };

  virtual BaseMeasure<double>* getCopy() const
  {
    return new Gauss(*this);
  };

  /*
   * puts all parameters, mu and Sig into one vector
   * (0 to d^2-1) Sig
   * (d^2 to d^2+d-1) mu
   */
  virtual Row<double> asRow() const
  {
    uint32_t d = mMu.n_elem;
    Row<double> row(d*d+d);
    for (uint32_t i=0; i<d; ++i)
      for (uint32_t j=0; j<d; ++j)
       row(j+i*d) = mSig(j,i); // column major
    for (uint32_t i=0; i<d; ++i)
      row(d*d+i) = mMu(i);
    return row;
  };

  virtual void fromRow(const Row<double>& row)
  {
    uint32_t d = mMu.n_elem; // we already have a Vtheta from the init;
    for (uint32_t i=0; i<d; ++i)
      for (uint32_t j=0; j<d; ++j)
        mSig(j,i) = row(j+i*d); // column major
    for (uint32_t i=0; i<d; ++i)
      mMu(i) = row(d*d+i);
  };

  virtual double logP(const Col<double>& x) const
  {
    return -0.5*(double(mSig.n_rows)*1.8378770664093453 + log(det(mSig)) +  as_scalar((x-mMu).t()*solve(mSig,x-mMu)));
  };

  Col<double> mMu;
  Mat<double> mSig;
private:
};

class Dir : public BaseMeasure<uint32_t>
{
public:
  Dir(const Row<double>& alphas)
  : mAlphas(alphas), mAlpha0(sum(alphas))
  {
    mRowDim = mAlphas.n_elem;
  };

  Dir(const Dir& dir)
    : mAlphas(dir.mAlphas), mAlpha0(sum(dir.mAlphas))
  {
    mRowDim = mAlphas.n_elem;
  };

  /*
   * return a copy of this object - this uses the concept of covariance
   */
  virtual Dir* getCopy() const
  {
    return new Dir(*this);
  };

  virtual Row<double> asRow() const
  {
    return mAlphas;
  };

  virtual void fromRow(const Row<double>& r)
  {
    mAlphas = r;
    mAlpha0 = sum(r);
  };

  virtual BaseMeasure<uint32_t>* mode() const
//  virtual BaseMeasure<uint32_t> mode() const
  {
    Row<double> mode(mAlphas.size());
    dirMode(mode,mAlphas);
    return new Mult(mode);
  };

  virtual double Elog(const Col<uint32_t>& x) const
  {
    // TODO: assumes that x is 1D word
    return digamma(mAlphas(x(0))) - digamma(mAlpha0);
  };

  /*
   * update parameters using observations x to form posterior for stochastic variational HDP
   */
  virtual void posteriorHDP_var(const Col<double>& zeta, const Mat<double>& phi, uint32_t D, const Mat<uint32_t>& x)
  {

    uint32_t N = x.n_cols;
    uint32_t T = zeta.n_rows;
    uint32_t Nw = mAlphas.n_elem;

    Row<double> lambda(Nw);
    lambda.zeros();
    for (uint32_t i=0; i<T; ++i)
    {
      Row<double> _lambda(Nw);
      _lambda.zeros();
      for (uint32_t n=0; n<N; ++n){
        _lambda(x(n)) += phi(n,i);
      }
      lambda += zeta(i) * _lambda;
    }
    mAlphas += D*lambda;
    //cout<<"lambda-nu="<<d_lambda[k].t()<<endl;
    //cout<<"lambda="<<d_lambda[k].t()<<endl;
  };

  virtual void posterior(const Mat<uint32_t>& x)
  {
    uint32_t N = x.n_cols;
    for (uint32_t i=0; i< N; ++i)
      mAlphas[x[i]] ++;
    mAlpha0= sum(mAlphas);
  };

  /*
   * used for gibbs sampling - seems to assume a categorical distribution
   */
  double predictiveProb(const Col<uint32_t>& x_q, const Mat<uint32_t>& x_given) const
  {
    // TODO: assumes x_q is 1D! (so actually just a uint32_t

    // compute posterior under x_given
    uint32_t k=x_q(0);
    Row<uint32_t> ids=(x_given.row(0)==k);
    uint32_t C_k=sum(ids);
    uint32_t L=x_given.n_cols;
    //    cout<<"k="<<k<<" C_k="<<C_k<<" L="<<L<<" alpha0="<<mAlpha0
    //      <<" alpha_k="<<mAlphas(k)
    //      <<" log(p)="<< log((C_k + mAlphas(k))/(L + mAlpha0))<<endl;
    //    cout<<x_q<<" -" <<x_given.t()<<endl;
    return log((C_k + mAlphas(k))/(L + mAlpha0));
  };
  double predictiveProb(const Col<uint32_t>& x_q) const
  {
    const uint32_t k=x_q(0);
    return log(mAlphas(k)/mAlpha0);
  };

  Row<double> mAlphas;
  double mAlpha0;
private:
};

class NIW : public BaseMeasure<double>
{
public:
  // make a copy of vtheta and Delta (that should only be a copy of the header anyway
  NIW(colvec vtheta, double kappa, mat Delta, double nu)
  : mVtheta(vtheta), mKappa(kappa), mDelta(Delta), mNu(nu)
  {
//    cout<<"Creating "<<typeid(this).name()<<endl;

    mRowDim = mVtheta.n_elem;
    mRowDim = mRowDim*mRowDim + mRowDim +2;
  };

  NIW(const NIW& niw)
  : mVtheta(niw.mVtheta), mKappa(niw.mKappa), mDelta(niw.mDelta), mNu(niw.mNu)
  {
    mRowDim = mVtheta.n_elem;
    mRowDim = mRowDim*mRowDim + mRowDim +2;
  };

  virtual NIW* getCopy() const
  {
    return new NIW(*this);
  };

  /*
   * puts all parameters, Vtheta, kappa, delta, and nu into one vector
   * (0 to d^2-1) Delta
   * (d^2 to d^2+d-1) Vtheta
   * (d^2+d) nu
   * (d^2+d+1) kappa
   */
  virtual Row<double> asRow() const
  {
    uint32_t d = mVtheta.n_elem;
    Row<double> row(d*d+d+2);

    for (uint32_t i=0; i<d; ++i)
      for (uint32_t j=0; j<d; ++j)
        row(j+i*d) = mDelta(j,i); // column major
    for (uint32_t i=0; i<d; ++i)
      row(d*d+i) = mVtheta(i);
    row(d*d+d) = mNu;
    row(d*d+d+1) = mKappa;
    return row;
  };

  virtual void fromRow(const Row<double>& row)
  {
    uint32_t d = mVtheta.n_elem; // we already have a Vtheta from the init;
    for (uint32_t i=0; i<d; ++i)
      for (uint32_t j=0; j<d; ++j)
        mDelta(j,i) = row(j+i*d); // column major
    for (uint32_t i=0; i<d; ++i)
      mVtheta(i) = row(d*d+i);
    mNu = row(d*d+d);
    mKappa = row(d*d+d+1);
  };

  virtual BaseMeasure<double>* mode() const
  {
    uint32_t d = mVtheta.n_elem; // we already have a Vtheta from the init;
    Mat<double> sig = mDelta/(mNu+d+2);
//    cout<<"mu: "<<mVtheta;
//    cout<<"sig:"<<sig;

    return new Gauss(mVtheta,sig);

//    mode.set_size(d*d+d);
//    //TODO: not sure about covariance...
//    for (uint32_t i=0; i<d; ++i)
//      for (uint32_t j=0; j<d; ++j)
//        mode(j+i*d) = mDelta(j,i)/(mNu+d+1); // column major
//    for (uint32_t i=0; i<d; ++i)
//      mode(d*d+i) = mVtheta(i); // sure about the mean
  };

  virtual double Elog(const Col<double>& x) const
  {
    //TODO: make it log from the beginning
    double d = mVtheta.n_elem; // we already have a Vtheta from the init;

//    double b = as_scalar((x-mVtheta).t()*solve(mDelta,x-mVtheta));
//    double logA=boost::math::lgamma((mNu+1.0)/2.0) - (d/2.0)*log((mNu-d+1.0)*datum::pi) - boost::math::lgamma((mNu-d+1.0)/2.0) - 0.5*(log(mKappa+1.0)-log(mKappa*(mNu-d+1.0))) - 0.5*log(det(mDelta));
//    double logC=log (1.0+(mKappa/(mKappa+1.0))*b)*((mNu+1.0)/2.0);

//    cout<<boost::math::tgamma((mNu+1.0)/2.0)<<" "<<sqrt(det(mDelta))<<" "<<boost::math::tgamma((mNu-d+1.0)/2.0)<<" "<<(mNu-d+1.0)<<endl;

//    cout<<"vtheta: "<<mVtheta;
//    cout<<"delta: "<<mDelta;
//    cout<<"kappa: "<<mKappa<<" nu="<<mNu<<endl;
    double sq = as_scalar((x+mVtheta).t()*solve(mDelta,x+mVtheta));

    double eLog= -0.5*d*log(datum::pi) -0.5*log(det(mDelta)) +0.5*digamma_mult(-0.5*mNu,uint32_t(d)) -0.5*(d/mKappa) -0.5*mNu*sq;

    if(!is_finite(mVtheta)|| !is_finite(eLog))
    {
      cout<<"vtheta: "<<mVtheta;
      cout<<"delta: "<<mDelta;
      cout<<"kappa: "<<mKappa<<" nu="<<mNu<<endl;
      cout<<"Elog="<<eLog<<" sq="<<sq<<" digamma_mult="<<digamma_mult(-0.5*mNu,uint32_t(d))<<endl;
      exit(0);
    }
    return eLog;
  };

  virtual void posteriorHDP_var(const Col<double>& zeta, const Mat<double>& phi, uint32_t D, const Mat<double>& x)
  {
    uint32_t N = x.n_cols;
    uint32_t T = zeta.n_rows;

//    cout<<" -----------------"<<endl;
//    cout<<"N="<<N<<" T="<<T<<endl;
    double counts=0.0;
    for (uint32_t i=0; i<T; ++i){
//      cout<<"counts= "<<counts<<" zeta="<<zeta(i)<<" sumphi="<<sum(phi.col(i))<<endl;
      counts += zeta(i)*sum(phi.col(i));
    }
    counts*=D;

    Col<double> x_hat(x.n_rows);
    x_hat.zeros();
    for (uint32_t i=0; i<T; ++i){
      Col<double> x_sum(x.n_rows);
      x_sum.zeros();
      for (uint32_t n=0; n<N; ++n){
        x_sum += phi(n,i)*x.col(n);
      }
      x_hat += zeta(i) * x_sum;
    }
    x_hat *= D/counts;

    Mat<double> S(mDelta);
    S.zeros();
    for (uint32_t i=0; i<T; ++i){
      Mat<double> S_sum(mDelta);
      S_sum.zeros();
      for (uint32_t n=0; n<N; ++n){
        S_sum += phi(n,i)*(x.col(n)-x_hat)*(x.col(n)-x_hat).t();
      }
      S += zeta(i) * S_sum;
    }

    // TODO: use zeta and Phi!!
//    uint32_t n = x.n_cols;
//    Col<double> x_hat=sum(x,1)/n;
//    Mat<double> S(mDelta)
//    S.zeros();
//    for (uint32_t i=0; i<n; ++i)
//      S += (x.col(i) - x_hat) * (x.col(i) - x_hat).t();

      cout<<" -----------------"<<endl;
      cout<<"vtheta(before): "<<mVtheta;
//    cout<<"delta: "<<mDelta;
//    cout<<"kappa: "<<mKappa<<" nu="<<mNu<<endl;
//    cout<<"S="<<S;
//    cout<<"x_hat="<<x_hat<< " sum/N="<<sum(x,1)/N;
//    cout<<"counts="<<counts<<endl;

    mDelta += D*S + (mKappa*counts)/(mKappa+counts)*(x_hat - mVtheta)*(x_hat - mVtheta).t();
    mVtheta = mKappa/(mKappa+counts)* mVtheta + counts/(mKappa+counts)*x_hat;
    mKappa += counts;
    mNu += counts;

//    if(!is_finite(mVtheta) || accu(mDelta<0.0)>0)
    {
//      cout<<"zeta="<<zeta.t();
//      cout<<"phi="<<phi;
//      cout<<"N="<<N<<" T="<<T<<endl;
//      cout<<"x="<<x;
      cout<<"S="<<S;
      cout<<"x_hat="<<x_hat<< " sum/N="<<sum(x,1)/N;
      cout<<"counts="<<counts<<endl;
      cout<<"-> vtheta: "<<mVtheta;
      cout<<"-> delta: "<<mDelta;
      cout<<"-> kappa: "<<mKappa<<" nu="<<mNu<<endl;
//      exit(0);
    }
  };

  virtual void posterior(const NIW& niw)
  {
    mDelta +=;
  };

  virtual void posterior(const Mat<double>& x)
  {
    uint32_t n = x.n_cols;
    Col<double> x_hat=sum(x,1)/n;
    Mat<double> S(mDelta);
    S.zeros();
    for (uint32_t i=0; i<n; ++i)
      S += (x.col(i) - x_hat) * (x.col(i) - x_hat).t();

    mDelta = mDelta + S + (mKappa*n)/(mKappa+n)*(x_hat - mVtheta)*(x_hat - mVtheta).t();
    mVtheta = mKappa/(mKappa+n)* mVtheta + n/(mKappa+n)*x_hat;
    mKappa += x.n_cols;
    mNu += x.n_cols;
  };

  double predictiveProb(const Col<double>& x_q, const Mat<double>& x_given) const
  {
    // compute posterior under x_given
    // M0: number of data points in the cluster
    // M1: sum over all data points in the cluster
    // M2: x*x.T over all data points in the cluster
    double M0=x_given.n_cols;
    colvec M1=sum(x_given,1);
    mat M2=x_given.t()*x_given;
    double kappa = mKappa+M0;
    double nu = mNu+M0;
    colvec vtheta = (mKappa*mVtheta + M1)/kappa;
    //    cout<<"vtheta="<<vtheta<<endl;
    //    cout<<"mVtheta="<<mVtheta<<endl;
    //    cout<<"M2="<<M2<<endl;
    //    cout<<"mDelta="<<mDelta<<endl;
    mat Delta = (mNu*mDelta + M2 + mKappa*(mVtheta*mVtheta.t()) - kappa*(vtheta*vtheta.t()))/nu;

    // compute the likelihood of x_q under the posterior
    // This can be approximated by using a moment matched gaussian
    // to the multivariate student-t distribution which arises in
    // when integrating over the parameters of the normal inverse
    // wishart
    mat C_matched=(((kappa+1.0)*nu)/(kappa*(nu-vtheta.n_rows-1.0)))*Delta;

    return logGaus(x_q, vtheta, C_matched);
  };

  double predictiveProb(const Col<double>& x_q) const
  {
    // compute the likelihood of x_q under the posterior
    // This can be approximated by using a moment matched gaussian
    // to the multivariate student-t distribution which arises in
    // when integrating over the parameters of the normal inverse
    // wishart
    mat C_matched=(((mKappa+1.0)*mNu)/(mKappa*(mNu-mVtheta.n_rows-1.0)))*mDelta;

    return logGaus(x_q, mVtheta, C_matched);
  };

  static double logGaus(const colvec& x, const colvec& mu, const mat& C)
  {
    //    cout<<"C"<<C<<endl;
    //    cout<<"mu"<<mu<<endl;
    //    cout<<"x"<<x<<endl;
    double detC=det(C);
    if(!is_finite(detC))
      return 0.0;
    mat CinvXMu=solve(C,x-mu);
    mat logXCX = (x-mu).t() * CinvXMu;
    //    cout<<"rows="<<C.n_rows<<" log(det(C))="<<log(det(C*0.001))-double(C.n_rows)*1000.0<<" logXC="<<logXCX(0)<<" p="<<-0.5*(double(C.n_rows)*1.8378770664093453 + log(det(C)) + logXCX(0))<<endl;

    return -0.5*(double(C.n_rows)*1.8378770664093453 + log(detC) + logXCX(0));
  };

  colvec mVtheta;
  double mKappa;
  mat mDelta;
  double mNu;
};

/*
 * Container for base measure pointers
 */
template <class U>
class DistriContainer
{
public:

  DistriContainer(uint32_t d=0)
    : mDistris(d,NULL)
  {};

  DistriContainer(const BaseMeasure<U>& a, uint32_t d)
    : mDistris(d,NULL)
  {
    for (uint32_t i=0; i<d; ++i)
      mDistris[i] = a.getCopy();
  };

  DistriContainer(const vector<BaseMeasure<U>* >& a)
    : mDistris(a.size(),NULL)
  {
    for (uint32_t i=0; i<a.size(); ++i)
      mDistris[i] = a[i]->getCopy();
  };

  DistriContainer(const DistriContainer<U>& a)
    : mDistris(a.size(),NULL)
  {
    for (uint32_t i=0; i<a.size(); ++i)
      mDistris[i] = a[i]->getCopy();
  };

  ~DistriContainer()
  {
    for (uint32_t i=0; i<mDistris.size(); ++i)
    {
      //cout<<"deleting "<<mDistris.size()<<" "<<i<<" :"<<mDistris[i]<<endl;
      delete mDistris[i];
    }
  };

  void init(const BaseMeasure<U>& a, uint32_t d)
  {
    for (uint32_t i=0; i<mDistris.size(); ++i)
      delete mDistris[i];

    mDistris.resize(d,NULL);
    for (uint32_t i=0; i<d; ++i)
      mDistris[i] = a.getCopy();
  };

  BaseMeasure<U>* operator[](const uint32_t i) const
  {
    assert(i<mDistris.size());
    return mDistris[i];
  };

  BaseMeasure<U>*& operator[](const uint32_t i)
  {
    assert(i<mDistris.size());
    return mDistris[i];
  };

  uint32_t size() const
  {
    return mDistris.size();
  };

  void resize(uint32_t i)
  {
    mDistris.resize(i,NULL);
  };

  void toMat( Mat<double>& mat) const
  {
    mat.set_size(this->size(), mDistris[0]->rowDim() );
    for (uint32_t i=0; i< this->size(); ++i)
      mat.row(i) = mDistris[i]->asRow();
  };

  Mat<double> toMat() const
  {
    Mat<double> mat;
    toMat(mat);
    return mat;
  };

private:
  vector<BaseMeasure<U>* > mDistris;
};

/*
 * Mixture of probabiliti distributions
 */
template <class U>
class Mixture
{
  public:
    Mixture()
      : mDistris()
    { };

    Mixture(const Mixture<U>& mix)
      : mDistris(mix.mDistris), mP(mix.mP)
    {};

    Mixture(const DistriContainer<U>& distris, const Row<double>& ps)
      : mDistris(distris), mP(ps)
    {};

    double logP(const Col<U>& x) const
    {
      double p=0.0 ;
      for (uint32_t i=0; i<mDistris.size(); ++i)
        p += mP[i]*exp(mDistris[i]->logP(x));
      return log(p);
    };

    DistriContainer<U> mDistris;
    Row<double> mP;

  private:
};

#endif /* BASEMEASURE_HPP_ */


#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <localizer/GVGmap.h>
#include <boost/lexical_cast.hpp>
#include <cmath>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Dense>
#include <vector>
#include <tf/transform_listener.h>

using namespace Eigen;
using namespace cv;
using namespace std;
#define _xdim 600.0
#define _ydim 600.0
#define _xdimI 650
#define _ydimI 650

#define margin 25
void handle_Odom(const nav_msgs::Odometry& odom);
void handle_Map(const localizer::GVGmap& map);

Mat image = Mat(_xdimI, _ydimI, CV_8UC3, cvScalar(255) );
vector<Point2f> Trajectory;

VectorXd X; // The State Vector
MatrixXd P; // The Covariance Matrix
int nL;
double minX,minY,maxX,maxY;
double Dim,_scaleX,_scaleY;
Vector3d R; // The State Vector
Matrix2d cov; // The Covariance Matrix
int cnt;
char timeS [80];

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

  std::time_t rawtime;
  std::tm* timeinfo;

  std::time(&rawtime);
  timeinfo = std::localtime(&rawtime);
  std::strftime(timeS,80,"%Y-%m-%d-%H-%M-%S",timeinfo);

  ros::init(argc, argv, "localizer_viz");
  ros::NodeHandle nh;
  ros::Subscriber pose_sub = nh.subscribe("/indoor/gvg/odom_combined", 1, &handle_Odom);
  ros::Subscriber map_sub  = nh.subscribe("/indoor/gvg/theMap", 1, &handle_Map);
  maxX=-1000;
  minX=1000;
  maxY=-1000;
  minY=1000;
  Dim=600;
  cnt=0;
  X=VectorXd(3);
  X<<0,0,0;
  P=MatrixXd::Zero(3,3);
  nL=0;
  cvNamedWindow("GVG Map");

  ros::spin();
  cvDestroyWindow("GVG Map");
  return 0;
}

Point2f scaleP(double x, double y) {
  double tmpX=margin+(_scaleX)*(x-minX);
  double tmpY=margin+_ydim-(_scaleY)*(y-minY);
  Point2f P(tmpX,tmpY);
  return(P);
}
Point2f scaleP(Point2f P) {
  return(scaleP((double)P.x,(double)P.y));
}

void drawRobot(double x, double y, double theta) {
  int thickness = 1;
  int lineType = 8;
  Point2f center=Point2f(x,y);
  Trajectory.push_back (center);
  for (std::vector<Point2f>::iterator it = Trajectory.begin() ; it != Trajectory.end(); ++it) {
    circle( image,
	    scaleP(*it),
	    1,
	    Scalar( 0, 0, 255 ),
	    thickness,
	    lineType );
  }
}
double pow2(double x) { return((x)*(x)); }

void ellipseC(double x, double y, Matrix2d cov, Scalar color=Scalar( 0, 0,250 )) {
  Point2f center=scaleP(x,y);

  EigenSolver<MatrixXd> es(cov);

  VectorXcd V1c=es.eigenvectors().col(0);
  Vector2d V1= Vector2d(std::real(V1c[0]),std::real(V1c[1])) ;
  VectorXcd V2c=es.eigenvectors().col(1);
  Vector2d V2= Vector2d(std::real(V2c[0]),std::real(V2c[1])) ;

  double l1=std::real(es.eigenvalues()[0]);
  double l2=std::real(es.eigenvalues()[1]);

  if((l1>0.0)&&(l2>0.0)) {
    double k=1.0;
    double halfaxis[2];
    double angle = -atan2(V1[1],V1[0]);
    if(l1<l2) {
      double tmp=l1; l1=l2; l2=tmp;
      angle  = -atan2(V2[1],V2[0]);
    }
    halfaxis[0]=(_scaleX)*sqrt(k*(l1))*3;
    halfaxis[1]=(_scaleY)*sqrt(k*(l2))*3;

    if((halfaxis[0]>2)&&(halfaxis[1]>2)) {
      int thickness = 1;
      int lineType = 8;

      ellipse( image,
	       center,
	       Size(halfaxis[0], halfaxis[1]),
	       angle*180.0/M_PI,
	       0,
	       360,
	       color,
	       thickness,
	       lineType );
      if (minX>x-sqrt(k*(l1))*3) minX=x-sqrt(k*(l1))*3;
      if (maxX<x+sqrt(k*(l1))*3) maxX=x+sqrt(k*(l1))*3;
      if (minY>y-sqrt(k*(l1))*3) minY=y-sqrt(k*(l1))*3;
      if (maxY<y+sqrt(k*(l1))*3) maxY=y+sqrt(k*(l1))*3;

    }
  }
  else
    cerr<<"l1 or l2 are less than 0 l1="<<l1<<" l2="<<l2<<endl;

}
void drawAll() {
  image =cvScalar(255,255,255);
  drawRobot(R(0),R(1),R(2));
  ellipseC(R(0),R(1),cov);
  for(unsigned int i=3; i<X.size(); i++) {
    double x=X(i); i++;
    double y=X(i); i++;
    circle( image, scaleP(x,y), 4, Scalar( 30,30,30 ), 1, 8);
    ellipseC(x,y,P.block(i-2,i-2,2,2),Scalar( 255,0,0 ));
  }

  cv::imshow("GVG Map",image);
  cv::waitKey(1);
}

void handle_Odom(const nav_msgs::Odometry &odom) {

  R(0)=odom.pose.pose.position.x;
  R(1)=odom.pose.pose.position.y;
  tf::Pose pose;
  tf::poseMsgToTF(odom.pose.pose, pose);
  R(2)=tf::getYaw(pose.getRotation());
  if (minX>R(0)) minX=R(0);
  if (maxX<R(0)) maxX=R(0);
  if (minY>R(1)) minY=R(1);
  if (maxY<R(1)) maxY=R(1);
  double dx=maxX-minX;
  double dy=maxY-minY;
  if (dx>dy) Dim=dx;
  else Dim=dy;
  if(Dim==0) Dim=_xdim;
  _scaleX=_xdim/Dim;
  _scaleY=_ydim/Dim;

  cov<<odom.pose.covariance[0], odom.pose.covariance[1],
    odom.pose.covariance[6], odom.pose.covariance[7];
  drawAll();
}

void handle_Map(const localizer::GVGmap& map) {

  if((unsigned)X.size()<map.state.size()) {
    X.conservativeResize(map.state.size());
  }
  nL=(map.state.size()-3)/3;
  int i=0;
  for(std::vector<double>::const_iterator it = map.state.begin();it != map.state.end(); ++it) {
    X(i)=*it;
    i++;
  }
  std::vector<double>::const_iterator it = map.cov.begin();
  unsigned int covSize=3*nL+3;
  if(P.cols()!=covSize) {
    P.conservativeResize(covSize,covSize);
  }
  for(unsigned int i=0; i<covSize; i++) {
    for(unsigned int j=0; j<covSize; j++) {
      P(i,j)=*it;
      it++;
    }
  }

  drawAll();

  char buffer [50];
  sprintf(buffer,"GVGMap%s-%04d.png",timeS,cnt);
  vector<int> compression_params;
  compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
  compression_params.push_back(9);

  try {
    imwrite(buffer, image, compression_params);
  }
  catch (runtime_error& ex) {
    fprintf(stderr, "Exception converting image to PNG format: %s\n", ex.what());
    return;
  }
  cnt++;
}


#include <frovedis.hpp>
#include <frovedis/ml/graph/graph.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

template <class T>
void call_sssp(const std::string& data_p,
               const std::string& out_p,
               bool if_prep,
               size_t source_vertex) {
  graph<T> gr;
  time_spent t(INFO);
  if(if_prep) gr = read_edgelist<T>(data_p);
  else {
    auto mat = make_crs_matrix_load<T>(data_p);
    gr = graph<T>(mat);
  }
  t.show("data loading time: ");
  auto res = gr.single_source_shortest_path(source_vertex);
  t.show("sssp computation time: ");
  res.save(out_p);
  res.debug_print(5);
}

int main(int argc, char* argv[]){
    frovedis::use_frovedis use(argc, argv);
    using namespace boost::program_options;

    options_description opt("option");
    opt.add_options()
        ("help,h", "produce help message")
        ("input,i" , value<std::string>(), "input data path containing either edgelist or adjacency matrix data")
        ("dtype,t" , value<std::string>(), "input data type (int, float or double) [default: int]")
        ("output,o" , value<std::string>(), "output data path to save cc results")
        ("source,s", value<size_t>(), "source vertex id [1 ~ nvert] (default: 1)")
        ("verbose", "set loglevel to DEBUG")
        ("verbose2", "set loglevel to TRACE")
        ("prepare,p" , "whether to generate the CRS matrix from original edgelist file ");

    variables_map argmap;
    store(command_line_parser(argc,argv).options(opt).allow_unregistered().
          run(), argmap);
    notify(argmap);

    bool if_prep = 0; // true if prepare data from raw dataset
    std::string data_p, out_p, dtype = "int";
    size_t source_vertex = 1;

    if(argmap.count("help")){
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("input")){
      data_p = argmap["input"].as<std::string>();
    } else {
      std::cerr << "input path is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("output")){
      out_p = argmap["output"].as<std::string>();
    } else {
      std::cerr << "output path is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("prepare")){
      if_prep = true;
    }
    if(argmap.count("dtype")){
      dtype = argmap["dtype"].as<std::string>();
    }
    if(argmap.count("source")){
       source_vertex = argmap["source"].as<size_t>();
    }
    if(argmap.count("verbose")){
      set_loglevel(DEBUG);
    }
    if(argmap.count("verbose2")){
      set_loglevel(TRACE);
    }

    try {
      if (dtype == "int") {
        call_sssp<int>(data_p, out_p, if_prep, source_vertex);
      }
      else if (dtype == "float") {
        call_sssp<float>(data_p, out_p, if_prep, source_vertex);
      }
      else if (dtype == "double") {
        call_sssp<double>(data_p, out_p, if_prep, source_vertex);
      }
      else {
        std::cerr << "Supported dtypes are only int, float and double!\n";
        std::cerr << opt << std::endl;
        exit(1);
      }
    }
    catch (std::exception& e) {
      std::cout << "exception caught: " << e.what() << std::endl;
    }
    return 0;
}


#include <boost/test/unit_test.hpp>

#include <madym/tests/mdm_test_utils.h>

#include <madym/t1_methods/mdm_T1FitterVFA.h>
#include <madym/t1_methods/mdm_T1FitterIR.h>
#include <madym/image_io/nifti/mdm_NiftiFormat.h>
#include <madym/image_io/xtr/mdm_XtrFormat.h>
#include <madym/mdm_Image3D.h>
#include <mdm_version.h>

namespace fs = boost::filesystem;

BOOST_AUTO_TEST_SUITE(test_mdm_tools)

BOOST_AUTO_TEST_CASE(test_madym_T1_VFA) {
	BOOST_TEST_MESSAGE("======= Testing tool: madym T1 VFA =======");

	//Generate some signals from sample FA, TR, T1 and S0 values
	double T1 = 1000;
	double M0 = 2000;
	double TR = 3.5;
	std::vector<double>	FAs = { 2, 10, 18 };
	const auto PI = acos(-1.0);

	std::string test_dir = mdm_test_utils::temp_dir();
	std::string FA_dir = test_dir + "/FAs/";
	fs::create_directories(FA_dir);

	//Compute signal for each FA and write out image
	std::vector<std::string> FA_names(3);
	for (int i_fa = 0; i_fa < 3; i_fa++)
	{
		//Create VFA signal image
		mdm_Image3D FA_img;
		FA_img.setDimensions(1, 1, 1);
		FA_img.setVoxelDims(1, 1, 1);
		FA_img.info().flipAngle.setValue(FAs[i_fa]);
		FA_img.info().TR.setValue(TR);
		FA_img.setVoxel(0, mdm_T1FitterVFA::T1toSignal(T1, M0, PI*FAs[i_fa]/180, TR));

		FA_names[i_fa] = FA_dir + "FA_" + std::to_string((int)FAs[i_fa]);

		mdm_NiftiFormat::writeImage3D(FA_names[i_fa], FA_img,
			mdm_ImageDatatypes::DT_FLOAT, mdm_XtrFormat::NEW_XTR, false);
	}

	//Call calculate_T1 to fit T1 and S0
	std::string T1_output_dir = test_dir + "/madym_T1/";
	std::stringstream cmd;
	cmd << mdm_test_utils::tools_exe_dir() << "madym_T1"
		<< " -T VFA "
		<< " --T1_vols " << FA_names[0] << "," << FA_names[1] << "," << FA_names[2]
		<< " -o " << T1_output_dir
		<< " --overwrite"
    << " --no_audit";

	BOOST_TEST_MESSAGE("Command to run: " + cmd.str());

	int error;
	try
	{
		error = std::system(cmd.str().c_str());
	}
	catch (...)
	{
		BOOST_CHECK_MESSAGE(false, "Running madym_T1 failed");
		return;
	}

	BOOST_CHECK_MESSAGE(!error, "Error returned from madym_T1 tool");

	//Load in the parameter img vols and extract the single voxel from each
	mdm_Image3D T1_fit = mdm_NiftiFormat::readImage3D(T1_output_dir + "T1", false);
	mdm_Image3D M0_fit = mdm_NiftiFormat::readImage3D(T1_output_dir + "M0", false);

	//Check the model parameters have fitted correctly
	double tol = 0.1;
	BOOST_TEST_MESSAGE("Testing fitted T1");
	BOOST_CHECK_CLOSE(T1_fit.voxel(0), T1, tol);
	BOOST_TEST_MESSAGE("Testing fitted M0");
	BOOST_CHECK_CLOSE(M0_fit.voxel(0), M0, tol);

	//Tidy up
	fs::remove_all(FA_dir);
	fs::remove_all(T1_output_dir);
}

BOOST_AUTO_TEST_CASE(test_madym_T1_VFA_B1) {
  BOOST_TEST_MESSAGE("======= Testing tool: madym T1 VFA with B1 correction =======");

  //Generate some signals from sample FA, TR, T1 and S0 values
  double T1 = 1000;
  double M0 = 2000;
  double B1 = 120;
  double B1_scaling = 100;
  double TR = 3.5;
  std::vector<double>	FAs = { 2, 10, 18 };
  const auto PI = acos(-1.0);

  std::string test_dir = mdm_test_utils::temp_dir();
  std::string FA_dir = test_dir + "/FAs/";
  fs::create_directories(FA_dir);

  //Compute signal for each FA and write out image
  std::vector<std::string> FA_names(3);
  for (int i_fa = 0; i_fa < 3; i_fa++)
  {
    //Create VFA signal image
    mdm_Image3D FA_img;
    FA_img.setDimensions(1, 1, 1);
    FA_img.setVoxelDims(1, 1, 1);
    FA_img.info().flipAngle.setValue(FAs[i_fa]);
    FA_img.info().TR.setValue(TR);
    auto effective_FA = FAs[i_fa] * B1 / B1_scaling;
    FA_img.setVoxel(0, mdm_T1FitterVFA::T1toSignal(T1, M0, PI*effective_FA / 180, TR));

    FA_names[i_fa] = FA_dir + "FA_" + std::to_string((int)FAs[i_fa]);

    mdm_NiftiFormat::writeImage3D(FA_names[i_fa], FA_img,
      mdm_ImageDatatypes::DT_FLOAT, mdm_XtrFormat::NEW_XTR, false);
  }

  std::string B1_name = FA_dir + "B1";
  mdm_Image3D B1_img;
  B1_img.setDimensions(1, 1, 1);
  B1_img.setVoxelDims(1, 1, 1);
  B1_img.setVoxel(0, B1);
  mdm_NiftiFormat::writeImage3D(B1_name, B1_img,
    mdm_ImageDatatypes::DT_FLOAT, mdm_XtrFormat::NEW_XTR, false);

  //Call calculate_T1 to fit T1 and S0
  std::string T1_output_dir = test_dir + "/madym_T1/";
  std::stringstream cmd;
  cmd << mdm_test_utils::tools_exe_dir() << "madym_T1"
    << " -T VFA_B1 "
    << " --T1_vols " << FA_names[0] << "," << FA_names[1] << "," << FA_names[2]
    << " --B1 " << B1_name
    << " --B1_scaling " << B1_scaling
    << " -o " << T1_output_dir
    << " --overwrite"
    << " --no_audit";

  BOOST_TEST_MESSAGE("Command to run: " + cmd.str());

  int error;
  try
  {
    error = std::system(cmd.str().c_str());
  }
  catch (...)
  {
    BOOST_CHECK_MESSAGE(false, "Running madym_T1 failed");
    return;
  }

  BOOST_CHECK_MESSAGE(!error, "Error returned from madym_T1 tool");

  //Load in the parameter img vols and extract the single voxel from each
  mdm_Image3D T1_fit = mdm_NiftiFormat::readImage3D(T1_output_dir + "T1", false);
  mdm_Image3D M0_fit = mdm_NiftiFormat::readImage3D(T1_output_dir + "M0", false);

  //Check the model parameters have fitted correctly
  double tol = 0.1;
  BOOST_TEST_MESSAGE("Testing fitted T1");
  BOOST_CHECK_CLOSE(T1_fit.voxel(0), T1, tol);
  BOOST_TEST_MESSAGE("Testing fitted M0");
  BOOST_CHECK_CLOSE(M0_fit.voxel(0), M0, tol);

  //Tidy up
  fs::remove_all(FA_dir);
  fs::remove_all(T1_output_dir);
}

BOOST_AUTO_TEST_CASE(test_madym_T1_IR) {
  BOOST_TEST_MESSAGE("======= Testing tool: madym T1 IR =======");

  //Generate some signals from sample FA, TR, T1 and S0 values
  std::vector<double> TIs = { 50, 300, 800, 1000, 2000, 4000 };
  int nTIs = (int)TIs.size();
  double T1 = 1500;
  double M0 = 1000;
  double TR = 1e5;

  std::string test_dir = mdm_test_utils::temp_dir();
  std::string IR_dir = test_dir + "/IRs/";
  fs::create_directories(IR_dir);

  //Compute signal for each IR and write out image
  std::vector<std::string> IR_names(nTIs);
  for (int i_ti = 0; i_ti < nTIs; i_ti++)
  {
    //Create inversion recovery signal image
    mdm_Image3D IR_img;
    IR_img.setDimensions(1, 1, 1);
    IR_img.setVoxelDims(1, 1, 1);
    IR_img.info().TI.setValue(TIs[i_ti]);
    IR_img.info().TR.setValue(TR);
    IR_img.setVoxel(0, mdm_T1FitterIR::T1toSignal(T1, M0, TIs[i_ti], TR));

    IR_names[i_ti] = IR_dir + "IR_" + std::to_string((int)TIs[i_ti]);

    mdm_NiftiFormat::writeImage3D(IR_names[i_ti], IR_img,
      mdm_ImageDatatypes::DT_FLOAT, mdm_XtrFormat::NEW_XTR, false);
  }

  //Call calculate_T1 to fit T1 and S0
  std::string T1_output_dir = test_dir + "/madym_T1/";
  std::stringstream cmd;
  cmd << mdm_test_utils::tools_exe_dir() << "madym_T1"
    << " -T IR "
    << " --T1_vols " << IR_names[0];
  for (int i_ti = 1; i_ti < nTIs; i_ti++)
    cmd << "," << IR_names[i_ti];

  cmd << " --TR " << TR
    << " -o " << T1_output_dir
    << " --overwrite"
    << " --no_audit";

  BOOST_TEST_MESSAGE("Command to run: " + cmd.str());

  int error;
  try
  {
    error = std::system(cmd.str().c_str());
  }
  catch (...)
  {
    BOOST_CHECK_MESSAGE(false, "Running madym_T1 failed");
    return;
  }

  BOOST_CHECK_MESSAGE(!error, "Error returned from madym_T1 tool");

  //Load in the parameter img vols and extract the single voxel from each
  mdm_Image3D T1_fit = mdm_NiftiFormat::readImage3D(T1_output_dir + "T1", false);
  mdm_Image3D M0_fit = mdm_NiftiFormat::readImage3D(T1_output_dir + "M0", false);

  //Check the model parameters have fitted correctly
  double tol = 0.1;
  BOOST_TEST_MESSAGE("Testing fitted T1");
  BOOST_CHECK_CLOSE(T1_fit.voxel(0), T1, tol);
  BOOST_TEST_MESSAGE("Testing fitted M0");
  BOOST_CHECK_CLOSE(M0_fit.voxel(0), M0, tol);

  // Repeat the test with inversion recovery

  //Tidy up
  fs::remove_all(IR_dir);
  fs::remove_all(T1_output_dir);
}

BOOST_AUTO_TEST_SUITE_END() //


/*
 * ValidatorStage.cpp
 *
 *  Created on: 2015/07/22
 *      Author: kryozahiro
 */

#include "ValidatorStage.h"

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include "cpputil/GenericIo.h"

#include "../solver/Evaluator.h"
using namespace std;
using namespace cpputil;
namespace ac = boost::accumulators;

ValidatorStage::ValidatorStage(const boost::property_tree::ptree& config, const boost::property_tree::ptree& validatorStageTree, std::shared_ptr<Game>& game) : game(game) {
	times = validatorStageTree.get<int>("Times");
}

void ValidatorStage::operator()(std::vector<std::shared_ptr<Solution>>& solutions, std::mt19937_64& randomEngine) {
	cerr << "Validation:" << endl;
	cerr << "times = " << times << endl;
	cerr << "run\tfitness" << endl;
	validate(solutions);
	cerr << endl;
}

void ValidatorStage::validate(std::vector<std::shared_ptr<Solution>>& solutions) {
	Evaluator evaluator(game, "Validator", pair<int, int>(0, times));

	//各評価位置における評価値の統計情報
	vector<ac::accumulator_set<double, ac::stats<ac::tag::min, ac::tag::max, ac::tag::mean, ac::tag::variance>>> stats(game->getProgramSize().first);
	vector<int> minIndex(evaluator.getProgramSize().first);

	for (int i = 0; i < times; ++i) {
		//プログラムの選択
		vector<Program*> result(evaluator.getProgramSize().first);
		for (unsigned int k = 0; k < result.size(); ++k) {
			result[k] = &*(solutions[k]->getProgram());
		}

		//評価の実行
		vector<double> fitness = evaluator(result);
		evaluator.nextSetting();
		cerr << i << "\t" << fitness << endl;

		//統計情報の収集
		for (unsigned int k = 0; k < fitness.size(); ++k) {
			stats[k](fitness[k]);
			if (ac::min(stats[k]) == fitness[k]) {
				minIndex[k] = i;
			}
		}
	}

	//統計情報の表示
	cerr << "(bestIndex best worst mean sd) of each position:" << endl;
	for (unsigned int i = 0; i < stats.size(); ++i) {
		cout << minIndex[i] << "\t" << ac::min(stats[i]) << "\t" << ac::max(stats[i]) << "\t" << ac::mean(stats[i]) << "\t" << sqrt((double)ac::variance(stats[i])) << "\t";
	}
	cout << endl;
}


//  Copyright (c) 2018-2019 Cem Bassoy
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer and Google in producing this work
//  which started as a Google Summer of Code project.
//

#include <boost/test/unit_test.hpp>
#include <boost/numeric/ublas/tensor/strides.hpp>
#include <boost/numeric/ublas/tensor/extents.hpp>

//BOOST_AUTO_TEST_SUITE(test_strides, * boost::unit_test::depends_on("test_extents"));

BOOST_AUTO_TEST_SUITE(test_strides)

using test_types = std::tuple<boost::numeric::ublas::first_order, boost::numeric::ublas::last_order>;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_strides_ctor, value, test_types)
{
	using namespace boost::numeric;

	using extents_type  = ublas::basic_extents<unsigned>;
	using strides_type = ublas::strides<value>;

	strides_type         s0{};
	BOOST_CHECK        ( s0.empty());
	BOOST_CHECK_EQUAL  ( s0.size(), 0);

	strides_type        s1{extents_type{1,1}};
	BOOST_CHECK       (!s1.empty());
	BOOST_CHECK_EQUAL ( s1.size(), 2);

	strides_type        s2{extents_type{1,2}};
	BOOST_CHECK       (!s2.empty());
	BOOST_CHECK_EQUAL ( s2.size(), 2);

	strides_type        s3{extents_type{2,1}};
	BOOST_CHECK       (!s3.empty());
	BOOST_CHECK_EQUAL ( s3.size(), 2);

	strides_type        s4{extents_type{2,3}};
	BOOST_CHECK       (!s4.empty());
	BOOST_CHECK_EQUAL ( s4.size(), 2);

	strides_type        s5{extents_type{2,3,1}};
	BOOST_CHECK       (!s5.empty());
	BOOST_CHECK_EQUAL ( s5.size(), 3);

	strides_type        s6{extents_type{1,2,3}};
	BOOST_CHECK       (!s6.empty());
	BOOST_CHECK_EQUAL ( s6.size(), 3);

	strides_type        s7{extents_type{4,2,3}};
	BOOST_CHECK       (!s7.empty());
	BOOST_CHECK_EQUAL ( s7.size(), 3);
}

BOOST_AUTO_TEST_CASE( test_strides_ctor_access_first_order)
{
	using namespace boost::numeric;

	using extents_type  = ublas::basic_extents<unsigned>;
	using strides_type =  ublas::strides<ublas::first_order>;

	strides_type         s1{extents_type{1,1}};
	BOOST_REQUIRE_EQUAL( s1.size(),2);
	BOOST_CHECK_EQUAL  ( s1[0], 1);
	BOOST_CHECK_EQUAL  ( s1[1], 1);

	strides_type          s2{extents_type{1,2}};
	BOOST_REQUIRE_EQUAL ( s2.size(),2);
	BOOST_CHECK_EQUAL   ( s2[0], 1);
	BOOST_CHECK_EQUAL   ( s2[1], 1);

	strides_type          s3{extents_type{2,1}};
	BOOST_REQUIRE_EQUAL ( s3.size(),2);
	BOOST_CHECK_EQUAL   ( s3[0], 1);
	BOOST_CHECK_EQUAL   ( s3[1], 1);

	strides_type          s4{extents_type{2,3}};
	BOOST_REQUIRE_EQUAL ( s4.size(),2);
	BOOST_CHECK_EQUAL   ( s4[0], 1);
	BOOST_CHECK_EQUAL   ( s4[1], 2);

	strides_type          s5{extents_type{2,3,1}};
	BOOST_REQUIRE_EQUAL ( s5.size(),3);
	BOOST_CHECK_EQUAL   ( s5[0], 1);
	BOOST_CHECK_EQUAL   ( s5[1], 2);
	BOOST_CHECK_EQUAL   ( s5[2], 6);

	strides_type          s6{extents_type{1,2,3}};
	BOOST_REQUIRE_EQUAL ( s6.size(),3);
	BOOST_CHECK_EQUAL   ( s6[0], 1);
	BOOST_CHECK_EQUAL   ( s6[1], 1);
	BOOST_CHECK_EQUAL   ( s6[2], 2);

	strides_type          s7{extents_type{2,1,3}};
	BOOST_REQUIRE_EQUAL ( s7.size(),3);
	BOOST_CHECK_EQUAL   ( s7[0], 1);
	BOOST_CHECK_EQUAL   ( s7[1], 2);
	BOOST_CHECK_EQUAL   ( s7[2], 2);

	strides_type          s8{extents_type{4,2,3}};
	BOOST_REQUIRE_EQUAL ( s8.size(),3);
	BOOST_CHECK_EQUAL   ( s8[0], 1);
	BOOST_CHECK_EQUAL   ( s8[1], 4);
	BOOST_CHECK_EQUAL   ( s8[2], 8);
}

BOOST_AUTO_TEST_CASE( test_strides_ctor_access_last_order)
{
	using namespace boost::numeric;

	using extents_type  = ublas::basic_extents<unsigned>;
	using strides_type =  ublas::strides<ublas::last_order>;

	strides_type         s1{extents_type{1,1}};
	BOOST_REQUIRE_EQUAL( s1.size(),2);
	BOOST_CHECK_EQUAL  ( s1[0], 1);
	BOOST_CHECK_EQUAL  ( s1[1], 1);

	strides_type          s2{extents_type{1,2}};
	BOOST_REQUIRE_EQUAL ( s2.size(),2);
	BOOST_CHECK_EQUAL   ( s2[0], 1);
	BOOST_CHECK_EQUAL   ( s2[1], 1);

	strides_type          s3{extents_type{2,1}};
	BOOST_REQUIRE_EQUAL ( s3.size(),2);
	BOOST_CHECK_EQUAL   ( s3[0], 1);
	BOOST_CHECK_EQUAL   ( s3[1], 1);

	strides_type          s4{extents_type{2,3}};
	BOOST_REQUIRE_EQUAL ( s4.size(),2);
	BOOST_CHECK_EQUAL   ( s4[0], 3);
	BOOST_CHECK_EQUAL   ( s4[1], 1);

	strides_type          s5{extents_type{2,3,1}};
	BOOST_REQUIRE_EQUAL ( s5.size(),3);
	BOOST_CHECK_EQUAL   ( s5[0], 3);
	BOOST_CHECK_EQUAL   ( s5[1], 1);
	BOOST_CHECK_EQUAL   ( s5[2], 1);

	strides_type          s6{extents_type{1,2,3}};
	BOOST_REQUIRE_EQUAL ( s6.size(),3);
	BOOST_CHECK_EQUAL   ( s6[0], 6);
	BOOST_CHECK_EQUAL   ( s6[1], 3);
	BOOST_CHECK_EQUAL   ( s6[2], 1);

	strides_type          s7{extents_type{2,1,3}};
	BOOST_REQUIRE_EQUAL ( s7.size(),3);
	BOOST_CHECK_EQUAL   ( s7[0], 3);
	BOOST_CHECK_EQUAL   ( s7[1], 3);
	BOOST_CHECK_EQUAL   ( s7[2], 1);

	strides_type          s8{extents_type{4,2,3}};
	BOOST_REQUIRE_EQUAL ( s8.size(),3);
	BOOST_CHECK_EQUAL   ( s8[0], 6);
	BOOST_CHECK_EQUAL   ( s8[1], 3);
	BOOST_CHECK_EQUAL   ( s8[2], 1);
}

BOOST_AUTO_TEST_SUITE_END()


//  ratio_fwd.hpp  ---------------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*

This code was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.4 Compile-time rational arithmetic [ratio], of the C++ committee working
paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

// The way overflow is managed for ratio_less is taken from llvm/libcxx/include/ratio

#ifndef BOOST_RATIO_RATIO_FWD_HPP
#define BOOST_RATIO_RATIO_FWD_HPP

#include <boost/cstdint.hpp>
#ifdef INTMAX_C
#define BOOST_RATIO_INTMAX_C(a) INTMAX_C(a)
#else
#define BOOST_RATIO_INTMAX_C(a) a##LL
#endif

namespace boost
{

//----------------------------------------------------------------------------//
//                                                                            //
//              20.6 Compile-time rational arithmetic [ratio]                 //
//                                                                            //
//----------------------------------------------------------------------------//

// ratio
template <boost::intmax_t N, boost::intmax_t D = 1> class ratio;

// ratio arithmetic
template <class R1, class R2> struct ratio_add;
template <class R1, class R2> struct ratio_subtract;
template <class R1, class R2> struct ratio_multiply;
template <class R1, class R2> struct ratio_divide;

// ratio comparison
template <class R1, class R2> struct ratio_equal;
template <class R1, class R2> struct ratio_not_equal;
template <class R1, class R2> struct ratio_less;
template <class R1, class R2> struct ratio_less_equal;
template <class R1, class R2> struct ratio_greater;
template <class R1, class R2> struct ratio_greater_equal;

// convenience SI typedefs
typedef ratio<BOOST_RATIO_INTMAX_C(1), BOOST_RATIO_INTMAX_C(1000000000000000000)> atto;
typedef ratio<BOOST_RATIO_INTMAX_C(1),    BOOST_RATIO_INTMAX_C(1000000000000000)> femto;
typedef ratio<BOOST_RATIO_INTMAX_C(1),       BOOST_RATIO_INTMAX_C(1000000000000)> pico;
typedef ratio<BOOST_RATIO_INTMAX_C(1),          BOOST_RATIO_INTMAX_C(1000000000)> nano;
typedef ratio<BOOST_RATIO_INTMAX_C(1),             BOOST_RATIO_INTMAX_C(1000000)> micro;
typedef ratio<BOOST_RATIO_INTMAX_C(1),                BOOST_RATIO_INTMAX_C(1000)> milli;
typedef ratio<BOOST_RATIO_INTMAX_C(1),                 BOOST_RATIO_INTMAX_C(100)> centi;
typedef ratio<BOOST_RATIO_INTMAX_C(1),                  BOOST_RATIO_INTMAX_C(10)> deci;
typedef ratio<                 BOOST_RATIO_INTMAX_C(10), BOOST_RATIO_INTMAX_C(1)> deca;
typedef ratio<                BOOST_RATIO_INTMAX_C(100), BOOST_RATIO_INTMAX_C(1)> hecto;
typedef ratio<               BOOST_RATIO_INTMAX_C(1000), BOOST_RATIO_INTMAX_C(1)> kilo;
typedef ratio<            BOOST_RATIO_INTMAX_C(1000000), BOOST_RATIO_INTMAX_C(1)> mega;
typedef ratio<         BOOST_RATIO_INTMAX_C(1000000000), BOOST_RATIO_INTMAX_C(1)> giga;
typedef ratio<      BOOST_RATIO_INTMAX_C(1000000000000), BOOST_RATIO_INTMAX_C(1)> tera;
typedef ratio<   BOOST_RATIO_INTMAX_C(1000000000000000), BOOST_RATIO_INTMAX_C(1)> peta;
typedef ratio<BOOST_RATIO_INTMAX_C(1000000000000000000), BOOST_RATIO_INTMAX_C(1)> exa;

}  // namespace boost

#endif  // BOOST_RATIO_HPP


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_FIX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FIX_HPP_INCLUDED

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

 /*!

    @ingroup group-arithmetic
    Function object implementing fix capabilities

    Computes the truncation toward @ref Zero of its parameter.

    This is a convenient alias of @ref trunc
  **/
  const boost::dispatch::functor<tag::fix_> fix = {};
} }
#endif

#include <boost/simd/function/scalar/trunc.hpp>
#include <boost/simd/function/simd/fix.hpp>

#endif


#include "../../read_vector.hxx"
#include "../../Block_Vector.hxx"

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>

void read_primal_objective_c(const boost::filesystem::path &sdp_directory,
                             const std::vector<size_t> &block_indices,
                             const El::Grid &grid,
                             Block_Vector &primal_objective_c)
{
  primal_objective_c.blocks.reserve(block_indices.size());
  for(auto &block_index : block_indices)
    {
      const boost::filesystem::path primal_path(
        sdp_directory / ("primal_objective_c." + std::to_string(block_index)));
      boost::filesystem::ifstream primal_stream(primal_path);
      if(!primal_stream.good())
        {
          throw std::runtime_error("Could not open '" + primal_path.string()
                                   + "'");
        }

      std::vector<El::BigFloat> temp;
      read_vector(primal_stream, temp);
      primal_objective_c.blocks.emplace_back(temp.size(), 1, grid);
      auto &block(primal_objective_c.blocks.back());
      size_t local_height(block.LocalHeight());
      if(block.GlobalCol(0) == 0)
        {
          for(size_t row = 0; row < local_height; ++row)
            {
              size_t global_row(block.GlobalRow(row));
              block.SetLocal(row, 0, temp.at(global_row));
            }
        }
    }
}