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

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// 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)

#ifndef BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_CARTESIAN_UNIFORM_POINT_DISTRIBUTION_BOX_HPP
#define BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_CARTESIAN_UNIFORM_POINT_DISTRIBUTION_BOX_HPP

#include <random>
#include <iterator>

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/util/for_each_coordinate.hpp>

#include <boost/geometry/extensions/random/strategies/uniform_point_distribution.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace uniform_point_distribution {

namespace detail
{

template
<
    typename Point,
    typename Box,
    typename Generator,
    bool isIntegral =
        boost::is_integral<typename coordinate_type<Point>::type>::type::value
>
struct interval_sample {};

template<typename Point, typename Box, typename Generator>
struct interval_sample<Point, Box, Generator, true>
{
    Box const& b;
    Generator& gen;
    inline interval_sample(Box const& b, Generator& gen):b(b),gen(gen) {}
    template <typename PointDst, std::size_t Index>
    inline void apply(PointDst& point_dst) const
    {
        std::uniform_int_distribution<typename coordinate_type<Point>::type>
            dist(get<min_corner,Index>(b),get<max_corner,Index>(b));
        set<Index>(point_dst,dist(gen));
    }
};

template<typename Point, typename Box, typename Generator>
struct interval_sample<Point, Box, Generator, false>
{
    Box const& b;
    Generator& gen;
    inline interval_sample(Box const& b, Generator& gen):b(b),gen(gen) {}

    template <typename PointDst, std::size_t Index>
    inline void apply(PointDst& point_dst) const
    {
        std::uniform_real_distribution<typename coordinate_type<Point>::type>
            dist(get<min_corner,Index>(b),get<max_corner,Index>(b));
        set<Index>(point_dst,dist(gen));
    }
};

}

template
<
    typename Point,
    typename DomainGeometry,
    int Dim
>
struct interval_product_distribution
{
    interval_product_distribution(DomainGeometry const& g) {}
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                interval_product_distribution const& r_strategy) const
    {
        return boost::geometry::equals(l_domain.domain(), r_domain.domain());
    }
    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        Point r;
        for_each_coordinate(r,
        	detail::interval_sample<Point, DomainGeometry, Gen>(d, g));
        return r;
    }
    void reset(DomainGeometry const&) {};
};

namespace services {

template
<
    typename Point,
    typename DomainGeometry,
    int Dim
>
struct default_strategy
<
    Point,
    DomainGeometry,
    box_tag,
    single_tag, //There are no MultiBoxes right now
    Dim,
    cartesian_tag
> : public interval_product_distribution<Point, DomainGeometry, Dim> {
    typedef interval_product_distribution<Point, DomainGeometry, Dim> base;
    using base::base;
};

} // namespace services

}} // namespace strategy::uniform_point_distribution

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_CARTESIAN_UNIFORM_POINT_DISTRIBUTION_BOX_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2007 Marco Bianchetti
Copyright (C) 2007 Giorgio Facchinetti
Copyright (C) 2006 Chiara Fornarola
Copyright (C) 2005 StatPro Italia srl
Copyright (C) 2013 Peter Caspers
Copyright (C) 2015 CompatibL

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

// Based on shortratemodels.cpp file from Quantlib/test-suite.

#ifndef cl_adjoint_shortratemodels_impl_hpp
#define cl_adjoint_shortratemodels_impl_hpp
#pragma once

#include <ql/quantlib.hpp>
#include "utilities.hpp"
#include "adjointshortratemodelstest.hpp"
#include "adjointtestutilities.hpp"
#include "adjointtestbase.hpp"
#include <boost/shared_ptr.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

#define OUTPUT_FOLDER_NAME "AdjointShortRateModels"

namespace
{
    struct CalibrationData
    {
        Integer start_;
        Integer length_;
        Real volatility_;
    };

    struct VolatilityDependence
    {
        static std::deque<std::string > get_columns()
        {
            static std::deque<std::string > columns =
            {
                "Volatility", "ModelSigma"
            };

            return columns;
        }

        template <typename stream_type>
        friend inline stream_type&
            operator << (stream_type& stm, VolatilityDependence& v)
        {
                stm << v.inputVolatility_
                    << ";" << v.modelSigma_ << std::endl;

                return stm;
            }

        Real inputVolatility_;
        Real modelSigma_;
    };

    struct ModelData
    {
        ModelData()
        : today_(15, February, 2002)
        , settlement_(19, February, 2002)
        , termStructure_(flatRate(settlement_, 0.04875825, Actual365Fixed()))
        , model_(new HullWhite(termStructure_))
        , index_(new Euribor6M(termStructure_))
        , engine_(new JamshidianSwaptionEngine(model_))
        , swaptions_()
        , data_()
        {
            Settings::instance().evaluationDate() = today_;
        }

        void calibrationData(std::vector<cl::tape_double>& vol, std::vector<CalibrationData>& data)
        {
            Integer pos = 0;
            Integer size = vol.size();
            data.clear();
            for (std::vector<cl::tape_double>::iterator it = vol.begin(); it != vol.end(); it++, pos++)
            {
                data.push_back(CalibrationData { pos + 1, size - pos, *it });
            }
        }

        std::string getPath(Size calibrationType)
        {
            if (calibrationType == 1)
                return "\\TestCachedHullWhite";
            else
                return "\\TestCachedHullWhiteFixedReversion";
        }

        // Calibration type 1: Testing Hull-White calibration against cached values using swaptions with start delay
        // type 2: Testing Hull-White calibration with fixed reversion against cached values.
        Real calibrate(std::vector<Real>& volatility
                       , Size calibrationType)
        {
            std::vector<CalibrationData> data;
            calibrationData(volatility, data);
            Size sizeof_indep = data.size();
            swaptions_.clear();
            for (Size i = 0; i < sizeof_indep; i++)
            {
                boost::shared_ptr<Quote> volatil(new SimpleQuote(data[i].volatility_));
                boost::shared_ptr<CalibrationHelper> helper(
                    new SwaptionHelper(Period(data[i].start_, Years),
                    Period(data[i].length_, Years),
                    Handle<Quote>(volatil),
                    index_,
                    Period(1, Years), Thirty360(),
                    Actual360(), termStructure_));
                helper->setPricingEngine(engine_);
                swaptions_.push_back(helper);
            }

            // Set up the optimization problem
            // Real simplexLambda = 0.1;
            // Simplex optimizationMethod(simplexLambda);
            LevenbergMarquardt optimizationMethod(1.0e-8, 1.0e-8, 1.0e-8);
            EndCriteria endCriteria(1000, 100, 1e-6, 1e-8, 1e-8);

            // Optimize.
            switch (calibrationType)
            {

                case 1:
                    model_->calibrate(swaptions_, optimizationMethod, endCriteria);
                    break;
                case 2:
                    model_->calibrate(swaptions_, optimizationMethod, endCriteria, Constraint(), std::vector<Real>(),
                                     HullWhite::FixedReversion());
                    // The difference is in the choice of  HullWhite::FixedReversion() to calibrate the model below.
            }

            return  model_->sigma();
        }

        void finiteDiff(std::vector<Real>& vol
                          , Size calibrationType
                          , double h
                          , std::vector<Real>& sfFinite)
        {
            std::vector<Real>::iterator it;
            std::vector<Real>::iterator it_fin;
            for (it = vol.begin(), it_fin = sfFinite.begin(); it != vol.end(); it++, it_fin++)
            {
                *it -= h;
                calibrate(vol, calibrationType);
                cl::tape_double stepbackward = model_->sigma();
                *it += 2 * h;
                calibrate(vol, calibrationType);
                cl::tape_double stepforward = model_->sigma();
                *it_fin = (stepforward - stepbackward) / (2 * h);
                *it -= h;
            }
        }

        // Common data.
        Date today_;
        Date settlement_;
        Handle<YieldTermStructure> termStructure_;
        boost::shared_ptr<HullWhite> model_;
        boost::shared_ptr<IborIndex> index_;
        boost::shared_ptr<PricingEngine> engine_;
        std::vector<boost::shared_ptr<CalibrationHelper>> swaptions_;
        std::vector<CalibrationData> data_;

    };

    struct CachedHullWhiteTestData
        : public ModelData
    {
        struct Test
        : public cl::AdjointTest<Test>
        {
            Test(Size size, CachedHullWhiteTestData* data)
            : size_(size)
            , data_(data)
            , volatility_(size)
            , iterNumFactor_(1)
            {
                setLogger(&data_->outPerform_);

                for (Size i = 0; i < size; i++)
                    volatility_[i] = 0.1148 - 0.004*i;
            }

            Size indepVarNumber() { return size_;}

            Size depVarNumber() { return 1; }

            Size minPerfIteration() { return iterNumFactor_; }

            void recordTape()
            {
                cl::Independent(volatility_);
                calculateModelSigma();
                f_ = std::make_unique<cl::tape_function<double>>(volatility_, modelSigma_);
            }

            // Calculates total calibration error.
            void  calculateModelSigma()
            {
                modelSigma_.push_back(data_->calibrate(volatility_, data_->calibrationType_));
            }

            // Calculates derivatives using finite difference method.
            void calcAnalytical()
            {
                // Shift for finite diff. method.
                double h = 1.0e-3;
                analyticalResults_.resize(size_);
                data_->finiteDiff(volatility_, data_->calibrationType_, h, analyticalResults_);
            }

            double relativeTol() const { return 1e-2; }

            double absTol() const { return 1e-2; }

            Size size_;
            Size iterNumFactor_;
            CachedHullWhiteTestData* data_;
            std::vector<cl::tape_double> volatility_;
            std::vector<cl::tape_double> modelSigma_;
        };

        CachedHullWhiteTestData(Size calibrationType)
            : ModelData()
            , calibrationType_(calibrationType)

            , outPerform_(OUTPUT_FOLDER_NAME + getPath(calibrationType)
            , { { "filename", "AdjointPerformance" }
              , { "not_clear", "Not" }
              , { "line_box_width", "-5" }
              , { "cleanlog", "true" }
              , { "title", "Model sigma differentiation performance with respect to calibration implied swaption volatilities" }
              , { "xlabel", "Size of  calibration  volatilities vector" }
              , { "ylabel", "Time (s)" }
              , { "smooth", "12" } })

            , outAdjoint_(OUTPUT_FOLDER_NAME + getPath(calibrationType)
            , { { "filename", "Adjoint" }
              , { "not_clear", "Not" }
              , { "cleanlog", "false" }
              , { "title", "Adjoint differentiation performance with respect to calibration implied swaption volatilities" }
              , { "xlabel", "Size of  calibration  volatilities vector" }
              , { "ylabel", "Time (s)" }
              , { "smooth", "12" } })

            , outSize_(OUTPUT_FOLDER_NAME + getPath(calibrationType)
            , { { "filename", "TapeSize" }
              , { "not_clear", "Not" }
              , { "cleanlog", "false" }
              , { "title", "Tape size dependence on  size of  calibration  volatilities vector" }
              , { "xlabel", "Size of  calibration  volatilities vector" }
              , { "ylabel", "Memory (MB)" }
              , { "smooth", "12" } })

            , out_(OUTPUT_FOLDER_NAME + getPath(calibrationType) + "//output"
            , { { "filename", "SigmaonVolatil" }
              , { "not_clear", "Not" }
              , { "title", "Model sigma on calibration swaption implied volatilty dependence" }
              , { "xlabel", "Volatility" }
              , { "ylabel", "Model Sigma" }
              , { "cleanlog", "false" } })

#if defined CL_GRAPH_GEN
            , pointNo_(20)
            , iterNo_(20)
            , step_(1)
#else
            , pointNo_(1)
            , iterNo_(1)
            , step_(1)
#endif
        {
        }

        bool makeOutput()
        {
            bool ok = true;
            if (pointNo_ > 0)
            {
                ok &= recordDependencePlot();
            }
            ok &= cl::recordPerformance(*this, iterNo_, step_);
            return ok;
        }

        std::shared_ptr<Test> getTest(size_t size)
        {
            return std::make_shared<Test>(size + 4, this);
        }

        // Makes plots for strike sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<VolatilityDependence> outData(pointNo_);
            auto test = getTest(pointNo_);
            for (Size i = 0; i < pointNo_; i++)
            {
                Real vol = 0.06 + 0.0003*i*5;
                test->volatility_[0] = vol;
                outData[i] = { vol, test->data_->calibrate(test->volatility_, test->data_->calibrationType_) };
            }
            out_ << outData;

            // Reset volatility_[0] to initial value.
            test->volatility_[0] = 0.1148;

            return true;
        }

        Size pointNo_;
        Size iterNo_;
        Size step_;

        Size calibrationType_;

        cl::tape_empty_test_output outPerform_;
        cl::tape_empty_test_output outAdjoint_;
        cl::tape_empty_test_output outSize_;
        cl::tape_empty_test_output out_;
    };
    typedef CachedHullWhiteTestData::Test CachedHullWhiteTest;

    struct FuturesConvexityBiasTest
        : public cl::AdjointTest<FuturesConvexityBiasTest>
    {
        FuturesConvexityBiasTest()
        : iterNumFactor_(1)
        , futureQuote_(94.0)
        , a_(0.03)
        , sigma_(0.015)
        , t_(5.0)
        , T_(5.25)
        , parameters_(3)
        , calculatedFunction_(1)
        {
            parameters_ = { a_, sigma_, futureQuote_ };
        }

        Size indepVarNumber() { return 3; }

        Size depVarNumber() { return 1; }

        Size minPerfIteration() { return iterNumFactor_; }

        void recordTape()
        {
            cl::Independent(parameters_);
            calculateFunction();
            f_ = std::make_unique<cl::tape_function<double>>(parameters_, calculatedFunction_);
        }

        void calculateFunction()
        {
            calculatedFunction_[0] = (100.0 - parameters_[2]) / 100.0 -
                HullWhite::convexityBias(parameters_[2], t_, T_, parameters_[1], parameters_[0]);
        }

        // Calculates derivatives using finite difference method.
        void calcAnalytical()
        {
            double h = 1e-4;
            Size size = indepVarNumber();
            analyticalResults_.resize(size);
            for (Size i = 0; i < size; i++)
            {
                parameters_[i] -= h;
                Real stepbackward = (100.0 - parameters_[2]) / 100.0 -
                    HullWhite::convexityBias(parameters_[2], t_, T_, parameters_[1], parameters_[0]);
                parameters_[i] += 2 * h;
                Real stepforward = (100.0 - parameters_[2]) / 100.0 -
                    HullWhite::convexityBias(parameters_[2], t_, T_, parameters_[1], parameters_[0]);
                analyticalResults_[i] = (stepforward - stepbackward) / (2 * h);
                parameters_[i] -= h;
            }

        }

        double relativeTol() const { return 1e-2; }

        double absTol() const { return 1e-4; }

        Size iterNumFactor_;

        Real futureQuote_;
        Real a_;
        Real sigma_;
        Time t_;
        Time T_;

        std::vector<cl::tape_double> parameters_;
        std::vector<cl::tape_double> calculatedFunction_;
    };
}

#endif


//==================================================================================================
/*!
  @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_ABS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ABS_HPP_INCLUDED

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

 /*!
  @ingroup group-arithmetic
    Function object implementing abs capabilities

    Computes the absolute value of its parameter.

    @par Semantic

    For any value @c x of type @c T,

    @code
    T r = abs(x);
    @endcode

    is equivalent to:

    @code
    T r = x < T(0) ? -x : x;
    @endcode

    @par Note:

    - Take care that for signed integers the absolute value of @ref Valmin is
    @ref Valmin (thus negative!). This is a side effect of the 2-complement
    representation of integers. To avoid this, you can use the abss
    saturated functor or convert the input parameter to a larger type
    before taking the absolute value.

    - Also abs is a very current function name and sometimes a C macro version can be
    an unwanted concurrent of simd::abs, you can just prefix abs or
    use the alias modulus or fabs instead to circumvent this problem.

    @par Alias

    modulus, fabs

    @par Decorators

    std_ for floating entries

    @see  abss, sqr_abs, sqrs

  **/
  const boost::dispatch::functor<tag::abs_> abs = {};
} }
#endif

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

#endif


// main.cpp
// Used to write exploratory/behavioural tests for CSVParser

#include "CSVParser.hpp"
#include "LeastSquaresFit.hpp"
#include <iostream>
#include <memory> // shared pointers
#include <unistd.h> // usleep
#include <boost/program_options.hpp>
#include <chrono>

void usage() {
	std::cout << "Example usage:\n"
		<< "CSVParser_tester --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 	header;
	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")
		("header", po::value<std::string>(&header)->required(),
			"unique header of column")
		("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 ****************

	constexpr unsigned char DELIM {','};
	std::shared_ptr<CSVParser> t1;
	try {
		 t1 = std::make_shared<CSVParser>(makeCSVParser(filename, DELIM));
	} catch(const std::runtime_error &e) {
		std::cout << e.what() << std::endl;
		return 2;
	}
	ColumnData<int_data_type> results;

	// Set up values to perform a one-hit analysis
	{
		index_type num_segments {1};
		std::vector<ColumnData<int_data_type>> result_cds;
		try{
			result_cds = t1->makeSegments<int_data_type>(
				header, header_row_index, first_data_row_index, num_segments);
		}catch (std::runtime_error &e) {
			std::cout << e.what() << std::endl;
			return 3;
		}
		t1->runAnalysisSegment(result_cds.at(0));

		std::cout << "result of one-hit analysis: " << result_cds.at(0).data_summary_actual << '\n' << std::endl;
	}

	// Set up values to perform a progressive analysis
	// Note: This only creates ColumnData segments, it
	// does not perform any calculations
	{
		index_type num_segments {5};
		std::vector<ColumnData<int_data_type>> result_cds;
		try{
			result_cds = t1->makeSegments<int_data_type>(
				header, header_row_index, first_data_row_index, num_segments);
		}catch (std::runtime_error &e) {
			std::cout << e.what() << std::endl;
			return 3;
		}

		std::cout << "******************************************" << std::endl;
		std::cout << "printing contents of progressive analysis:" << std::endl;
		std::cout << "******************************************" << std::endl;

		int_data_type sum{};
		for(auto &cur_res : result_cds) {
			t1->runAnalysisSegment(cur_res);
			std::cout << cur_res << '\n' << std::endl;
			sum += cur_res.data_summary_actual;
		}
		std::cout << "Final sum: " << sum << std::endl;
	}

	// Performing a progressive analysis using LeastSquaresFit class
	std::string X_header {"SE_T003_001"};
	std::string Y_header {"SE_T013_001"};

	auto Xs = t1->makeSegments<float_data_type>(X_header, header_row_index, first_data_row_index, 5);
	auto Ys = t1->makeSegments<float_data_type>(Y_header, header_row_index, first_data_row_index, 5);
	std::cout << "Xs size: " << Xs.size() << std::endl;
	std::cout << "Ys size: " << Ys.size() << std::endl;
	std::cout << "X_bar: " << calcAvg(Xs) << std::endl;
	std::cout << "Y_bar: " << calcAvg(Ys) << std::endl;
	std::cout << "\n" << std::endl;

	using vec_cols_float = std::vector<ColumnData<float_data_type>>;
	LeastSquaresFit<vec_cols_float, vec_cols_float> lsf =
		makeLeastSquaresFit<vec_cols_float, vec_cols_float>(Xs, Ys);

	// Display results/projections after each segment is processed
	std::cout << lsf << "\n\n" << std::endl;
	constexpr int_data_type usecs {2000000}; // microseconds
	std::cout << "Displaying projections during progressive analysis:" << std::endl;
	while(lsf.calcNextProjection()) {
		auto start = std::chrono::system_clock::now();
		std::cout << '\r' << "a: " << lsf.getProja() << " b: " << lsf.getProjb();
		auto end = std::chrono::system_clock::now();
		auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);

		std::cout << " elapsed: " << elapsed.count() << " ns" << std::flush;
		usleep(usecs);
	}
	std::cout << "\nProgram closing" << std::endl;
	return 0;
}


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_FEATURES_MATCHING_HARRIS_CORNER_DETECTOR_HPP
#define PIC_FEATURES_MATCHING_HARRIS_CORNER_DETECTOR_HPP

#include "../util/vec.hpp"
#include "../util/std_util.hpp"
#include "../image.hpp"
#include "../filtering/filter_luminance.hpp"
#include "../filtering/filter_gaussian_2d.hpp"
#include "../filtering/filter_gradient_harris_opt.hpp"
#include "../filtering/filter_max.hpp"
#include "../features_matching/general_corner_detector.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief The HarrisCornerDetector class
 */
class HarrisCornerDetector: public GeneralCornerDetector
{
protected:
    Image *I_grad;
    Image *I_grad_flt;
    Image *ret;

    //Harris Corners detector parameters
    float sigma, threshold;
    int radius;

    //previous values
    int width, height;

    /**
     * @brief release
     */
    void release()
    {
        lum = delete_s(lum);
        I_grad = delete_s(I_grad);
        I_grad_flt = delete_s(I_grad_flt);
        ret = delete_s(ret);
    }

    /**
     * @brief setNULL
     */
    void setNULL()
    {
        width = -1;
        height = -1;
        lum = NULL;
        I_grad = NULL;
        I_grad_flt = NULL;
        ret = NULL;
    }

public:

    /**
     * @brief HarrisCornerDetector
     * @param sigma
     * @param radius
     * @param threshold
     */
    HarrisCornerDetector(float sigma = 1.0f, int radius = 3, float threshold = 0.001f) : GeneralCornerDetector()
    {
        setNULL();
        update(sigma, radius, threshold);
    }

    ~HarrisCornerDetector()
    {
        release();
    }

    /**
     * @brief update
     * @param sigma
     * @param radius
     * @param threshold
     */
    void update(float sigma = 1.0f, int radius = 3, float threshold = 0.001f)
    {
        this->sigma = sigma > 0.0f ? sigma : 1.0f;
        this->radius = radius > 0 ? radius : 1;
        this->threshold = threshold > 0.0f ? threshold : 0.001f;
    }

    /**
     * @brief execute
     * @param img
     * @param corners
     */
    void execute(Image *img, std::vector< Eigen::Vector2f > *corners)
    {
        if(img == NULL || corners == NULL) {
            return;
        }

        if((img->width != width) || (img->height != height)) {
            width = img->width;
            height = img->height;

            release();
        }

        if(img->channels == 1) {
            lum = img->clone();
        } else {
            lum = FilterLuminance::execute(img, lum, LT_CIE_LUMINANCE);
        }

        float minL, maxL;
        lum->getMinVal(NULL, &minL);
        lum->getMaxVal(NULL, &maxL);

        float delta = maxL - minL;

        *lum -= minL;
        *lum *= delta;

        corners->clear();

        std::vector< Eigen::Vector3f > corners_w_quality;

        //compute gradients
        I_grad = FilterGradientHarrisOPT::execute(lum, I_grad, 0);

        float eps = 2.2204e-16f;

        //filter gradient values
        FilterGaussian2D flt(sigma);
        I_grad_flt = flt.Process(Single(I_grad), I_grad_flt);

        if(ret == NULL) {
            ret = lum->allocateSimilarOne();
        }

        //ret = (Ix2.*Iy2 - Ixy.^2)./(Ix2 + Iy2 + eps);
        for(int i = 0; i < height; i++) {
            for(int j = 0; j < width; j++) {
                float *data_ret = (*ret)(j, i);

                float *I_grad_val = (*I_grad_flt)(j, i);

                float x2 = I_grad_val[0];
                float y2 = I_grad_val[1];
                float xy = I_grad_val[2];

                data_ret[0] =  (x2 * y2 - xy * xy) / (x2 + y2 + eps);
            }
        }

        //non-maximal supression
        lum = FilterMax::execute(ret, lum, radius * 2 + 1);
        Image* ret_flt = lum;

        float w = 1.0f;

        if(threshold < 0.0f) { //the best i-th points
            int bestPoints = int(-threshold);

            float *tmp = ret->sort();

            int n = ret->size();
            threshold = tmp[n - 1 - bestPoints];

            delete_vec_s(tmp);
        }

        for(int i = 0; i < height; i++) {
            float i_f = float(i);
            float cx, cy, ax, ay, bx, by, x, y;

            for(int j = 0; j < width; j++) {

                float R = (*ret)(j, i)[0];
                float R_flt = (*ret_flt)(j, i)[0];

                if((R == R_flt) && (R > threshold)) {
                    float Rr = (*ret)(j, i + 1)[0];
                    float Rl = (*ret)(j, i - 1)[0];
                    float Ru = (*ret)(j + 1, i)[0];
                    float Rd = (*ret)(j - 1, i)[0];

                    cx = R;
                    ax = (Rl + Rr) / 2.0f - cx;
                    bx = ax + cx - Rl;

                    if(ax != 0.0f) {
                        x = -w * bx / (2.0f * ax);
                    } else {
                        x = 0.0f;
                    }

                    cy = R;
                    ay = (Rd + Ru) / 2.0f - cy;
                    by = ay + cy - Rd;

                    if(ay != 0.0f) {
                        y = -w * by / (2.0f * ay);
                    } else {
                        y = 0.0f;
                    }

                    corners_w_quality.push_back(Eigen::Vector3f(float(j) + x, i_f + y, R));
                }
            }
        }

        sortCornersAndTransfer(&corners_w_quality, corners);
    }
};

#endif

} // end namespace pic

#endif /* PIC_FEATURES_MATCHING_HARRIS_CORNER_DETECTOR_HPP */


#ifndef NEURAL_NET_HPP_
#define NEURAL_NET_HPP_

#include <armadillo>
#include <config.h>
#include <string>
#include "util.hpp"
#include "mnist.hpp"

#include <algorithm>
#include <random>
#include <vector>
#include <numeric>

// Apply sigmoid to all elements
template<typename T>
void sigmoid(T&& out) {
    out.transform([](typename T::elem_type val) {
        return (1.0 / (1.0 + exp (-1.0 * val)));
    });
}

/**
 * Simple two-layer neural network for the MNIST data set
 *
 * \code
 * auto train_net = neural_net<float>(train_data, 0.1);
 * \endcode
 */
template<typename T = float>
struct neural_net {
    using elem_t = T;
    using mat_t = arma::Mat<elem_t>;
    using vec_t = arma::Col<elem_t>;
    using subview_t = arma::subview<elem_t>;
    using mnist = mnist<elem_t>;

    const mnist input;    //< Input data
    const elem_t lambda;  //< Regularization parameter
    mat_t theta1;         //< Weights for layer 1
    mat_t theta2;         //< Weights for layer 2
    mat_t a1;             //< Input activations
    mat_t a2;             //< Output activations from layer 1
    mat_t a3;             //< Output activations from layer 2
    mat_t yy;             //< One-hot vector for labels
    mat_t d_theta1;       //< Gradient for layer 1 weights
    mat_t d_theta2;       //< Gradient for layer 2 weights
    mat_t delta3;         //< Gradient from output to labels
    mat_t delta2;         //< Gradient from layer 2 to layer 1 activations

    /**
     * Constructor
     *
     * @param input_   MNIST inut data
     * @param lambda_  lambda value for regularization. Set to 0 for no
     *                 regularization
     */
    neural_net(const mnist& input_, elem_t lambda_ = 1)
    : input(input_),
      lambda(lambda_) {
        using rowvec = arma::Row<elem_t>;
        using arma::zeros;
        using arma::ones;
        using arma::randu;

        elem_t epsilon = 0.12;
        theta1 = (randu<mat_t>(64, input.images.n_cols + 1) * 2 - 1) * epsilon;
        theta2 = (randu<mat_t>(10, theta1.n_rows + 1) * 2 - 1) * epsilon;

        // Convert the column vector of y labels to a matrix where each
        // row has a 1 in the column specified by the label.
        yy = zeros<mat_t>(input.labels.n_rows, 11);
        int row = 0;
        input.labels.for_each([&](const elem_t& element) {
            yy(row, element) = 1.0;
            yy(row, 0) = 1.0;
            row++;
        });

        // Allocate space for each layer of activations. Add an additional
        // column for the bias neuron
        a1 = ones<mat_t>(input.images.n_rows, input.images.n_cols + 1);
        a2 = ones<mat_t>(input.images.n_rows, theta1.n_rows + 1);
        a3 = ones<mat_t>(input.images.n_rows, theta2.n_rows + 1);

        // Load the images onto first activation layer, but do not overwrite
        // the bias neuron
        no_bias(a1) = input.images;

        //shuffle_a1_yy();
    }

    /**
     * Use the neural network to predict outcomes
     *
     * @return The percentage of correct predictions

     */
    elem_t predict(void) const {
        feed_forward();

        // Find neuron with maximum confidence, this is our predicted label
        arma::ucolvec predictions = index_max(no_bias(a3), 1);
        predictions = predictions + 1;

        // Display percentage of correct labels
        return elem_t(sum(predictions == input.labels)) / input.labels.n_rows;
    }

    /**
     * Cost function for the neural net
     *
     * @return the cost
     */
    elem_t cost() const {
        // Cost, without regularization
        elem_t cost = sum(sum(
            -1 * no_bias(yy) % log(no_bias(a3)) -
                (1 - no_bias(yy)) % log(1 - no_bias(a3)), 1))
            / input.images.n_rows;

        // Cost, with regularization
        if (std::abs(lambda) > 0) {
            // Square each element. This next operation makes a copy
            mat_t theta1_sq = square(no_bias(theta1));
            mat_t theta2_sq = square(no_bias(theta2));

            // Sum up all elements in each layer
            elem_t reg = (sum(sum(theta1_sq)) + sum(sum(theta2_sq)));

            // Normalize
            reg *= lambda / (2 * input.images.n_rows);

            // Add in regularization term
            cost += reg;
        }

        return cost;
    }

    /**
     * Calculate the gradient for the weights
     */
    void gradient() {
        elem_t m = a1.n_rows;

        // Delta from labeled data to current set of predictions
        delta3 = a3 - yy;
        d_theta2 = (no_bias(delta3).t() * a2) / m;

        // Delta from output layer to hidden layer
        delta2 = no_bias(delta3) * theta2 % (a2 % (1 - a2));

        // Delta from output layer to hidden layer without bias
        d_theta1 = (no_bias(delta2).t() * a1) / m;

        // Regularization
        if (std::abs(lambda) > 0) {
            no_bias(d_theta1) += (lambda / m) * no_bias(theta1);
            no_bias(d_theta2) += (lambda / m) * no_bias(theta2);
        }
    }

    /**
     * A single Forward propogation step for the neural net
     */
    void feed_forward() const {
        // Pass input thru weights to second layer, do not disturb the bias
        // neuron
        no_bias(a2) = a1 * theta1.t();
        sigmoid(no_bias(a2));

        // Pass hidden layer to output layer
        no_bias(a3) = a2 * theta2.t();
        sigmoid(no_bias(a3));
    }

    /**
     * Train the neural network. Pass in a number of steps and a progress
     * function.
     *
     * \code
     * net.train(1000, [&](size_t i, size_t max_itr) -> void {
     *     if (i == 0 || i % 100 == 0 || i == max_itr - 1) {
     *         std::cout << "\r " << i << " j = " << train_net.cost()
     *                   << std::flush;
     *     }
     * });
     * \endcode
     *
     * @param steps    The number of steps to train
     * @param progress Function called after each step.
     */
    void train(size_t steps, std::function<void(size_t, size_t)> progress) {
        for (size_t i = 0; i < steps; i++) {
            feed_forward();
            // Back prop
            gradient();
            theta1 -= d_theta1;
            theta2 -= d_theta2;
            progress(i, steps);
        }
    }

    /**
     * Save the weights to two files
     */
    void save() const {
        std::string theta1_fn = data_dir + "/theta1.bin";
        std::string theta2_fn = data_dir + "/theta2.bin";
        theta1.save(theta1_fn);
        theta2.save(theta2_fn);
    }

    /**
     * Shuffle the rows of the input and label matrices
     */
    void shuffle_a1_yy() {
        using std::iota;
        using std::vector;
        using std::shuffle;
        using std::begin;
        using std::end;

        vector<size_t> indices(a1.n_rows);
        iota(begin(indices), end(indices), 0);

        auto rng = std::default_random_engine {};
        shuffle(begin(indices), end(indices), rng);

        mat_t a1_shfl(a1.n_rows, a1.n_cols);
        mat_t yy_shfl(yy.n_rows, yy.n_cols);

        for (size_t dst_idx = 0; dst_idx < indices.size(); ++dst_idx) {
            size_t src_idx = indices[dst_idx];
            a1_shfl.row(dst_idx) = a1.row(src_idx);
            yy_shfl.row(dst_idx) = yy.row(src_idx);
        }

        a1 = a1_shfl;
        yy = yy_shfl;
    }
};

#endif /* end of include guard: NEURAL_NET_HPP_ */


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

  @copyright 2015 NumScale SAS
  @copyright 2015 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_ARCH_COMMON_SCALAR_FUNCTION_REMAINDER_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_REMAINDER_HPP_INCLUDED

#include <boost/simd/function/div.hpp>
#include <boost/simd/function/idiv.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/selsub.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
// As the result can be negative the functor is not defined for unsigned
// The drem function is just an alias for the same thing.
// The remainder() function computes the remainder of dividing x by y.  The
// entries
// integer.  If the boost::simd::absolute value of x-n*y is 0.5, n is chosen
// return value is x-n*y, where n is the value x / y, rounded to the nearest
// to be even.
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( remainder_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::signed_<A0> >
                          , bd::generic_< bd::signed_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return selsub(is_nez(a1),a0,
                    simd::multiplies(idiv(a0, a1, round2even), a1));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( remainder_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return a0-div(a0, a1, round2even)*a1;
    }
  };
} } }

#endif


#include <fc/uint128.hpp>
#include <fc/io/raw_fwd.hpp>
#include <fc/variant.hpp>
#include <fc/crypto/bigint.hpp>
#include <boost/multiprecision/cpp_int.hpp>

#include <stdexcept>
#include "byteswap.hpp"

namespace fc
{
    typedef boost::multiprecision::uint128_t  m128;

    template <typename T>
    static void divide(const T &numerator, const T &denominator, T &quotient, T &remainder)
    {
      static const int bits = sizeof(T) * 8;//CHAR_BIT;

      if(denominator == 0) {
        throw std::domain_error("divide by zero");
      } else {
        T n      = numerator;
        T d      = denominator;
        T x      = 1;
        T answer = 0;

        while((n >= d) && (((d >> (bits - 1)) & 1) == 0)) {
          x <<= 1;
          d <<= 1;
        }

        while(x != 0) {
          if(n >= d) {
            n -= d;
            answer |= x;
          }

          x >>= 1;
          d >>= 1;
        }

        quotient = answer;
        remainder = n;
      }
    }

    uint128::uint128(const std::string &sz)
    :hi(0), lo(0)
    {
      // do we have at least one character?
      if(!sz.empty()) {
        // make some reasonable assumptions
        int radix = 10;
        bool minus = false;

        std::string::const_iterator i = sz.begin();

        // check for minus sign, i suppose technically this should only apply
        // to base 10, but who says that -0x1 should be invalid?
        if(*i == '-') {
          ++i;
          minus = true;
        }

        // check if there is radix changing prefix (0 or 0x)
        if(i != sz.end()) {
          if(*i == '0') {
            radix = 8;
            ++i;
            if(i != sz.end()) {
              if(*i == 'x') {
                radix = 16;
                ++i;
              }
            }
          }

          while(i != sz.end()) {
            unsigned int n = 0;
            const char ch = *i;

            if(ch >= 'A' && ch <= 'Z') {
              if(((ch - 'A') + 10) < radix) {
                n = (ch - 'A') + 10;
              } else {
                break;
              }
            } else if(ch >= 'a' && ch <= 'z') {
              if(((ch - 'a') + 10) < radix) {
                n = (ch - 'a') + 10;
              } else {
                break;
              }
            } else if(ch >= '0' && ch <= '9') {
              if((ch - '0') < radix) {
                n = (ch - '0');
              } else {
                break;
              }
            } else {
              /* completely invalid character */
              break;
            }

            (*this) *= radix;
            (*this) += n;

            ++i;
          }
        }

        // if this was a negative number, do that two's compliment madness :-P
        if(minus) {
          *this = -*this;
        }
      }
    }

    uint128::operator bigint()const
    {
       auto tmp  = uint128( bswap_64( hi ), bswap_64( lo ) );
       bigint bi( (char*)&tmp, sizeof(tmp) );
       return bi;
    }
    uint128::uint128( const fc::bigint& bi )
    {
       *this = uint128( std::string(bi) ); // TODO: optimize this...
    }

    uint128::operator std::string ()const
    {
      if(*this == 0) { return "0"; }

      // at worst it will be size digits (base 2) so make our buffer
      // that plus room for null terminator
      static char sz [128 + 1];
       sz[sizeof(sz) - 1] = '\0';

      uint128 ii(*this);
      int i = 128 - 1;

      while (ii != 0 && i) {

      uint128 remainder;
      divide(ii, uint128(10), ii, remainder);
          sz [--i] = "0123456789abcdefghijklmnopqrstuvwxyz"[remainder.to_integer()];
      }

      return &sz[i];
    }

    uint128& uint128::operator<<=(const uint128& rhs)
    {
        if(rhs >= 128)
        {
          hi = 0;
          lo = 0;
        }
        else
        {
          unsigned int n = rhs.to_integer();
          const unsigned int halfsize = 128 / 2;

            if(n >= halfsize){
                n -= halfsize;
                hi = lo;
                lo = 0;
            }

            if(n != 0) {
            // shift high half
                hi <<= n;

            const uint64_t mask(~(uint64_t(-1) >> n));

            // and add them to high half
                hi |= (lo & mask) >> (halfsize - n);

            // and finally shift also low half
                lo <<= n;
            }
       }

       return *this;
    }

    uint128 & uint128::operator>>=(const uint128& rhs)
    {
       if(rhs >= 128)
       {
         hi = 0;
         lo = 0;
       }
       else
       {
         unsigned int n = rhs.to_integer();
         const unsigned int halfsize = 128 / 2;

           if(n >= halfsize) {
               n -= halfsize;
               lo = hi;
               hi = 0;
           }

           if(n != 0) {
           // shift low half
               lo >>= n;

           // get lower N bits of high half
           const uint64_t mask(~(uint64_t(-1) << n));

           // and add them to low qword
               lo |= (hi & mask) << (halfsize - n);

           // and finally shift also high half
               hi >>= n;
           }
      }
      return *this;
   }

    uint128& uint128::operator/=(const uint128 &b)
    {
        auto self = (m128(hi) << 64) + m128(lo);
        auto other = (m128(b.hi) << 64) + m128(b.lo);
        self /= other;
        hi = static_cast<uint64_t>(self >> 64);
        lo = static_cast<uint64_t>((self << 64 ) >> 64);

        /*
        uint128 remainder;
        divide(*this, b, *this, remainder ); //, *this);
        if( tmp.hi != hi || tmp.lo != lo ) {
           std::cerr << tmp.hi << "  " << hi <<"\n";
           std::cerr << tmp.lo << "  " << lo << "\n";
           exit(1);
        }
        */

        /*
        const auto&  b128 = std::reinterpret_cast<const m128&>(b);
        auto&     this128 = std::reinterpret_cast<m128&>(*this);
        this128 /= b128;
        */
        return *this;
    }

    uint128& uint128::operator%=(const uint128 &b)
    {
        uint128 quotient;
        divide(*this, b, quotient, *this);
        return *this;
    }

    uint128& uint128::operator*=(const uint128 &b)
    {
        uint64_t a0 = (uint32_t) (this->lo        );
        uint64_t a1 = (uint32_t) (this->lo >> 0x20);
        uint64_t a2 = (uint32_t) (this->hi        );
        uint64_t a3 = (uint32_t) (this->hi >> 0x20);

        uint64_t b0 = (uint32_t) (b.lo        );
        uint64_t b1 = (uint32_t) (b.lo >> 0x20);
        uint64_t b2 = (uint32_t) (b.hi        );
        uint64_t b3 = (uint32_t) (b.hi >> 0x20);

        // (a0 + (a1 << 0x20) + (a2 << 0x40) + (a3 << 0x60)) *
        // (b0 + (b1 << 0x20) + (b2 << 0x40) + (b3 << 0x60)) =
        //  a0 * b0
        //
        // (a1 * b0 + a0 * b1) << 0x20
        // (a2 * b0 + a1 * b1 + a0 * b2) << 0x40
        // (a3 * b0 + a2 * b1 + a1 * b2 + a0 * b3) << 0x60
        //
        // all other cross terms are << 0x80 or higher, thus do not appear in result

        this->hi = 0;
        this->lo = a3*b0;
        (*this) += a2*b1;
        (*this) += a1*b2;
        (*this) += a0*b3;
        (*this) <<= 0x20;
        (*this) += a2*b0;
        (*this) += a1*b1;
        (*this) += a0*b2;
        (*this) <<= 0x20;
        (*this) += a1*b0;
        (*this) += a0*b1;
        (*this) <<= 0x20;
        (*this) += a0*b0;

        return *this;
   }

   void uint128::full_product( const uint128& a, const uint128& b, uint128& result_hi, uint128& result_lo )
   {
       //   (ah * 2**64 + al) * (bh * 2**64 + bl)
       // = (ah * bh * 2**128 + al * bh * 2**64 + ah * bl * 2**64 + al * bl
       // =  P * 2**128 + (Q + R) * 2**64 + S
       // = Ph * 2**192 + Pl * 2**128
       // + Qh * 2**128 + Ql * 2**64
       // + Rh * 2**128 + Rl * 2**64
       // + Sh * 2**64  + Sl
       //

       uint64_t ah = a.hi;
       uint64_t al = a.lo;
       uint64_t bh = b.hi;
       uint64_t bl = b.lo;

       uint128 s = al;
       s *= bl;
       uint128 r = ah;
       r *= bl;
       uint128 q = al;
       q *= bh;
       uint128 p = ah;
       p *= bh;

       uint64_t sl = s.lo;
       uint64_t sh = s.hi;
       uint64_t rl = r.lo;
       uint64_t rh = r.hi;
       uint64_t ql = q.lo;
       uint64_t qh = q.hi;
       uint64_t pl = p.lo;
       uint64_t ph = p.hi;

       uint64_t y[4];    // final result
       y[0] = sl;

       uint128_t acc = sh;
       acc += ql;
       acc += rl;
       y[1] = acc.lo;
       acc = acc.hi;
       acc += qh;
       acc += rh;
       acc += pl;
       y[2] = acc.lo;
       y[3] = acc.hi + ph;

       result_hi = uint128( y[3], y[2] );
       result_lo = uint128( y[1], y[0] );

       return;
   }

   static uint8_t _popcount_64( uint64_t x )
   {
      static const uint64_t m[] = {
         0x5555555555555555ULL,
         0x3333333333333333ULL,
         0x0F0F0F0F0F0F0F0FULL,
         0x00FF00FF00FF00FFULL,
         0x0000FFFF0000FFFFULL,
         0x00000000FFFFFFFFULL
      };
      // TODO future optimization:  replace slow, portable version
      // with fast, non-portable __builtin_popcountll intrinsic
      // (when available)

      for( int i=0, w=1; i<6; i++, w+=w )
      {
         x = (x & m[i]) + ((x >> w) & m[i]);
      }
      return uint8_t(x);
   }

   uint8_t uint128::popcount()const
   {
      return _popcount_64( lo ) + _popcount_64( hi );
   }

   void to_variant( const uint128& var,  variant& vo )  { vo = std::string(var);         }
   void from_variant( const variant& var,  uint128& vo ){ vo = uint128(var.as_string()); }

} // namespace fc

/*
 * Portions of the above code were adapted from the work of Evan Teran.
 *
 * Copyright (c) 2008
 * Evan Teran
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose and without fee is hereby granted, provided
 * that the above copyright notice appears in all copies and that both the
 * copyright notice and this permission notice appear in supporting
 * documentation, and that the same name not be used in advertising or
 * publicity pertaining to distribution of the software without specific,
 * written prior permission. We make no representations about the
 * suitability this software for any purpose. It is provided "as is"
 * without express or implied warranty.
 */


//==================================================================================================
/*!
  @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_SINC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SINC_HPP_INCLUDED

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

 /*!
  @ingroup group-trigonometric
    Function object implementing sinc capabilities

    Computes the sinus cardinal  value of its parameter that is sin(x)/x.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = sinc(x);
    @endcode

    is similar to:

    @code
    T r = x ? sin(x)/x : One;
    @endcode

    @see sin, sincpi, sinhc

  **/
  const boost::dispatch::functor<tag::sinc_> sinc = {};
} }
#endif

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

#endif


///////////////////////////////////////////////////////////////
//  Copyright 2019 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include "../performance_test.hpp"
#if defined(TEST_CPP_INT)
#include <boost/multiprecision/cpp_int.hpp>
#endif

void test10()
{
#ifdef TEST_CPP_INT
   test<boost::multiprecision::number<boost::multiprecision::cpp_int_backend<256, 256, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void>, boost::multiprecision::et_off> >("cpp_int(fixed)", 256);
#endif
}


/*
   Intel Copyright (C) ....
*/

#include "sparse_solver.h"
#include <Eigen/PardisoSupport>

template<typename T> void test_pardiso_T()
{
  PardisoLLT < SparseMatrix<T, RowMajor>, Lower> pardiso_llt_lower;
  PardisoLLT < SparseMatrix<T, RowMajor>, Upper> pardiso_llt_upper;
  PardisoLDLT < SparseMatrix<T, RowMajor>, Lower> pardiso_ldlt_lower;
  PardisoLDLT < SparseMatrix<T, RowMajor>, Upper> pardiso_ldlt_upper;
  PardisoLU  < SparseMatrix<T, RowMajor> > pardiso_lu;

  check_sparse_spd_solving(pardiso_llt_lower);
  check_sparse_spd_solving(pardiso_llt_upper);
  check_sparse_spd_solving(pardiso_ldlt_lower);
  check_sparse_spd_solving(pardiso_ldlt_upper);
  check_sparse_square_solving(pardiso_lu);
}

EIGEN_DECLARE_TEST(pardiso_support)
{
  CALL_SUBTEST_1(test_pardiso_T<float>());
  CALL_SUBTEST_2(test_pardiso_T<double>());
  CALL_SUBTEST_3(test_pardiso_T< std::complex<float> >());
  CALL_SUBTEST_4(test_pardiso_T< std::complex<double> >());
}


//==================================================================================================
/*!
  @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_CONSTANT_MLOGEPS2_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_MLOGEPS2_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate \f$-\log(eps^2)\f$ value

    @par Semantic:

    @code
    T r = Mlogeps2<T>();
    @endcode

    is similar to:

    @code
    T r = -log(sqr(Eps<T>()));
    @endcode

    @return The Mlogeps2 constant for the proper type
  **/
  template<typename T> T Mlogeps2();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant mlogeps2.

      @return The Mlogeps2 constant for the proper type
    **/
    const boost::dispatch::functor<tag::mlogeps2_> mlogeps2 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/mlogeps2.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


//==================================================================================================
/*!
  @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_CSC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CSC_HPP_INCLUDED

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

  /*!
    @ingroup group-trigonometric
    This function object returns the cosecante of the input in radian : \f$1/\sin(x)\f$.

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

    @par Note

      As most other trigonometric function csc can be called
      with a second optional parameter  which is a tag on
      speed and accuracy (see @ref cos for further details)

    @see cscd, cscpi,

    @par Example:

      @snippet csc.cpp csc

    @par Possible output:

      @snippet csc.txt csc

  **/
  IEEEValue csc(IEEEValue const& x);
} }
#endif

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

#endif


//==================================================================================================
/*!
  @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_ARCH_COMMON_SIMD_FUNCTION_REMROUND_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_REMROUND_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/div.hpp>
#include <boost/simd/function/round.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/if_minus.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(rem_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::round_
                          , bs::pack_<bd::int_<A0>, X>
                          , bs::pack_<bd::int_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(bd::functor<bs::tag::round_> const&
                                     , const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        return if_minus(is_nez(a1), a0, div(round,a0,a1)*a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(rem_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::round_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(bd::functor<bs::tag::round_> const&
                                     , const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        auto z = is_nez(a1);
        return if_else(logical_and(z, is_eqz(a0)),  a0,
                       if_nan_else(logical_or(is_invalid(a1),is_invalid(a0)) ,
                                   if_nan_else(is_eqz(a1),
                                               if_minus(z, a0, div(round, a0,a1)*a1)
                                              )
                                  )
                      );
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(rem_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::fast_tag
                          , bs::tag::round_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(const fast_tag &
                                     , bd::functor<bs::tag::round_> const&
                                     , const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        return fnms(div(round, a0,a1), a1, a0);
      }
   };
} } }

#endif


/*
 * File:   Grid.cpp
 * Author: Weiming Hu <weiming@psu.edu>
 *
 * Created on April 3, 2021, 11:15 AM
 */

#include <cmath>
#include <sstream>
#include <boost/numeric/conversion/cast.hpp>

#include "Txt.h"
#include "Grid.h"

using namespace std;

/********
 * Grid *
*********/

Grid::Grid() {
    nan_count_ = 0;
}

Grid::Grid(const Grid & orig) {
    *this = orig;
}

Grid::Grid(const string & file) {
    setup(file);
}

Grid::~Grid() {
}

void
Grid::setup(const string & file) {

    // Read matrix from the file
    Txt::readMatrix(file, grid_);

    // Set up the mapping from station keys to the row/column numbers
    row_cols_.clear();
    nan_count_ = 0;

    size_t num_rows = nrows();
    size_t num_cols = ncols();

    for (size_t r = 0; r < num_rows; ++r) {
        for (size_t c = 0; c < num_cols; ++c) {
            double key = grid_(r, c);

            if (std::isnan(key)) {
                nan_count_++;

            } else {
                double integral, fractional;
                fractional = modf(key, &integral);

                if (fractional != 0) {
                    throw runtime_error("Station keys must be non-negative integer!");
                }

                row_cols_[boost::numeric_cast<size_t>(key)] = {r, c};
            }
        }
    }

    if (row_cols_.size() + nan_count_ != num_rows * num_cols) {
        stringstream ss;
        ss << "Duplicated station keys found. Expect " << num_rows << "x" << num_cols
            << ", but got " << row_cols_.size() << " and " << nan_count_ << " nan";
        throw runtime_error(ss.str());
    }

    return;
}

void
Grid::getRectangle(size_t station_key, Matrix & mask,
        size_t width, size_t height, bool same_padding) const {

    if (width == 0 || height == 0) throw runtime_error("Invalid width or height");
    if (remainder(width - 1, 2) != 0) throw runtime_error("Width needs to be an odd number");
    if (remainder(height - 1, 2) != 0) throw runtime_error("Height needs to be an odd number");

    // Initialization
    mask.resize(height, width);

    // Get the location of the center grid
    RowCol row_col = (*this)[station_key];
    long center_row = boost::numeric_cast<long>(row_col.first);
    long center_col = boost::numeric_cast<long>(row_col.second);

    // Get the spatial extent of the mask
    long left = center_col - (width - 1) / 2;
    long top = center_row - (height - 1) / 2;

    size_t num_rows = nrows();
    size_t num_cols = ncols();

    for (size_t r = 0; r < mask.size1(); ++r) {
        long grid_r = top + r;

        if (same_padding) {
            if (grid_r < 0) grid_r = 0;
            else if (grid_r >= num_rows) grid_r = num_rows - 1;
        }

        if (grid_r < 0 || grid_r >= num_rows) {
            for (size_t c = 0; c < mask.size2(); ++c) {
                mask(r, c) = NAN;
            }

        } else {
            for (size_t c = 0; c < mask.size2(); ++c) {
                long grid_c = left + c;

                if (same_padding) {
                    if (grid_c < 0) grid_c = 0;
                    else if (grid_c >= num_cols) grid_c = num_cols - 1;
                }

                if (grid_c < 0 || grid_c >= num_cols) {
                    mask(r, c) = NAN;
                } else {
                    double v = (*this)(grid_r, grid_c);

                    if (same_padding && std::isnan(v)) {
                        throw runtime_error("NAN is not allowed when using same padding");
                    } else {
                        mask(r, c) = v;
                    }
                }
            }
        }
    }

    return;
}

size_t
Grid::nrows() const {
    return grid_.size1();
}

size_t
Grid::ncols() const {
    return grid_.size2();
}

size_t
Grid::nkeys() const {
    return row_cols_.size();
}

string
Grid::summary() const {
    stringstream ss;
    ss << nrows() << " rows, " << ncols() << " columns, " << nkeys()
        << " station keys, and " << nan_count_ << " unassigned grids (nan)";
    return ss.str();
}

string
Grid::detail() const {
    stringstream ss;
    ss << grid_;
    return ss.str();
}

double
Grid::getMaxKey() const {
    if (row_cols_.size() == 0) throw runtime_error("Grid empty! No max to find.");

    double max_val = 0;

    for (auto & it:row_cols_) {
        if (it.first > max_val) max_val = it.first;
    }

    return max_val;
}

ostream &
operator<<(ostream & os, const Grid & obj) {
    return os << obj.grid_;
}

Grid &
Grid::operator=(const Grid & rhs) {
    if (this != &rhs) {
        nan_count_ = rhs.nan_count_;
        grid_ = rhs.grid_;
        row_cols_ = rhs.row_cols_;
    }
    return *this;
}

RowCol
Grid::operator[](size_t station_key) const {
    return row_cols_.at(station_key);
}

double
Grid::operator()(size_t row, size_t col) const {
    return grid_(row, col);
}


//==============================================================================
//         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/ieee/include/functions/ulpdist.hpp>

#include <boost/dispatch/functor/meta/call.hpp>

#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>

#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/valmin.hpp>
#include <nt2/include/constants/valmax.hpp>

NT2_TEST_CASE_TPL ( ulpdist_real,  BOOST_SIMD_REAL_TYPES)
{
  using nt2::ulpdist;
  using nt2::tag::ulpdist_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<ulpdist_(T,T)>::type
                  , T
                  );

#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(ulpdist(nt2::Inf<T>(), nt2::Inf<T>()), nt2::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(nt2::Minf<T>(), nt2::Minf<T>()), nt2::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(nt2::Nan<T>(), nt2::Nan<T>()), nt2::Zero<T>());
#endif

  NT2_TEST_EQUAL(ulpdist(nt2::Mone<T>(), nt2::Mone<T>()), nt2::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(nt2::One<T>(), nt2::One<T>()), nt2::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(nt2::Zero<T>(), nt2::Zero<T>()), nt2::Zero<T>());

  NT2_TEST_EQUAL( ulpdist(nt2::One<T>(), nt2::One<T>()+nt2::Eps<T>())
                , T(0.5)
                );

  NT2_TEST_EQUAL( ulpdist(nt2::One<T>(), nt2::One<T>()-nt2::Eps<T>())
                , T(0.5)
                );

  NT2_TEST_EQUAL( ulpdist(nt2::One<T>(), nt2::One<T>()-nt2::Eps<T>()/2)
                , T(0.25)
                );
}

NT2_TEST_CASE_TPL ( ulpdist_signed_integral,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using nt2::ulpdist;
  using nt2::tag::ulpdist_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<ulpdist_(T,T)>::type
                  , T
                  );

  NT2_TEST_EQUAL(ulpdist(nt2::Mone<T>(), nt2::Mone<T>()), nt2::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(nt2::One<T>(), nt2::One<T>()), nt2::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(nt2::Zero<T>(), nt2::Zero<T>()), nt2::Zero<T>());

  NT2_TEST_EQUAL( ulpdist(nt2::Zero<T>(), nt2::Valmin<T>())
                , nt2::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(nt2::Valmin<T>(), nt2::Zero<T>())
                , nt2::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(nt2::Zero<T>(), nt2::Valmax<T>())
                , nt2::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(nt2::Valmax<T>(), nt2::Zero<T>())
                , nt2::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(nt2::Valmin<T>(), nt2::Valmax<T>())
                , nt2::Valmax<T>()
                );
}

NT2_TEST_CASE_TPL ( ulpdist_unsigned_integral,  BOOST_SIMD_UNSIGNED_TYPES)
{
  using nt2::ulpdist;
  using nt2::tag::ulpdist_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<ulpdist_(T,T)>::type
                  , T
                  );

  NT2_TEST_EQUAL(ulpdist(nt2::Mone<T>(), nt2::Mone<T>()), nt2::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(nt2::One<T>(), nt2::One<T>()), nt2::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(nt2::Zero<T>(), nt2::Zero<T>()), nt2::Zero<T>());

  NT2_TEST_EQUAL( ulpdist(nt2::Zero<T>(), nt2::Valmin<T>())
                , nt2::Zero<T>()
                );

  NT2_TEST_EQUAL( ulpdist(nt2::Valmin<T>(), nt2::Zero<T>())
                , nt2::Zero<T>()
                );

  NT2_TEST_EQUAL( ulpdist(nt2::Zero<T>(), nt2::Valmax<T>())
                , nt2::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(nt2::Valmax<T>(), nt2::Zero<T>())
                , nt2::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(nt2::Valmin<T>(), nt2::Valmax<T>())
                , nt2::Valmax<T>()
                );
}


#include "Scene.h"
#include "Intersection.h"
#include "Ray.h"
#include "Surface.h"
#include "SurfaceList.h"

#include <Eigen/Dense>

std::unique_ptr<Intersection> Scene::intersect(const Ray& ray) const {
    return surfaces.intersect(ray);
}

Color Scene::shade(const Ray& ray, const Intersection& hit, int depth, bool shadows, bool kd) const {
    auto p = ray.evaluate(hit.t);
    auto n = hit.normal;

    return (hit.hit)->shade(ray, p, n, light, *this, depth, shadows, kd);
}


/**
 * \file se3_localization.cpp
 *
 *  Created on: Dec 12, 2018
 *     \author: jsola
 *
 *  ---------------------------------------------------------
 *  This file is:
 *  (c) 2018 Joan Sola @ IRI-CSIC, Barcelona, Catalonia
 *
 *  This file is part of `manif`, a C++ template-only library
 *  for Lie theory targeted at estimation for robotics.
 *  Manif is:
 *  (c) 2018 Jeremie Deray @ IRI-UPC, Barcelona
 *  ---------------------------------------------------------
 *
 *  ---------------------------------------------------------
 *  Demonstration example:
 *
 *  3D Robot localization based on fixed beacons.
 *
 *  See se2_localization.cpp for the 2D equivalent.
 *  See se3_sam.cpp for a more advanced example performing smoothing and mapping.
 *  ---------------------------------------------------------
 *
 *  This demo corresponds to the 3D version of the application
 *  in chapter V, section A, in the paper Sola-18,
 *  [https://arxiv.org/abs/1812.01537].
 *
 *  The following is an abstract of the content of the paper.
 *  Please consult the paper for better reference.
 *
 *
 *  We consider a robot in 3D space surrounded by a small
 *  number of punctual landmarks or _beacons_.
 *  The robot receives control actions in the form of axial
 *  and angular velocities, and is able to measure the location
 *  of the beacons w.r.t its own reference frame.
 *
 *  The robot pose X is in SE(3) and the beacon positions b_k in R^3,
 *
 *      X = |  R   t |              // position and orientation
 *          |  0   1 |
 *
 *      b_k = (bx_k, by_k, bz_k)    // lmk coordinates in world frame
 *
 *  The control signal u is a twist in se(3) comprising longitudinal
 *  velocity vx and angular velocity wz, with no other velocity
 *  components, integrated over the sampling time dt.
 *
 *      u = (vx*dt, 0, 0, 0, 0, w*dt)
 *
 *  The control is corrupted by additive Gaussian noise u_noise,
 *  with covariance
 *
 *    Q = diagonal(sigma_x^2, sigma_y^2, sigma_z^2, sigma_roll^2, sigma_pitch^2, sigma_yaw^2).
 *
 *  This noise accounts for possible lateral and rotational slippage
 *  through a non-zero values of sigma_y, sigma_z, sigma_roll and sigma_pitch.
 *
 *  At the arrival of a control u, the robot pose is updated
 *  with X <-- X * Exp(u) = X + u.
 *
 *  Landmark measurements are of the range and bearing type,
 *  though they are put in Cartesian form for simplicity.
 *  Their noise n is zero mean Gaussian, and is specified
 *  with a covariances matrix R.
 *  We notice the rigid motion action y = h(X,b) = X^-1 * b
 *  (see appendix D),
 *
 *      y_k = (brx_k, bry_k, brz_k)    // lmk coordinates in robot frame
 *
 *  We consider the beacons b_k situated at known positions.
 *  We define the pose to estimate as X in SE(3).
 *  The estimation error dx and its covariance P are expressed
 *  in the tangent space at X.
 *
 *  All these variables are summarized again as follows
 *
 *    X   : robot pose, SE(3)
 *    u   : robot control, (v*dt; 0; 0; 0; 0; w*dt) in se(3)
 *    Q   : control perturbation covariance
 *    b_k : k-th landmark position, R^3
 *    y   : Cartesian landmark measurement in robot frame, R^3
 *    R   : covariance of the measurement noise
 *
 *  The motion and measurement models are
 *
 *    X_(t+1) = f(X_t, u) = X_t * Exp ( w )     // motion equation
 *    y_k     = h(X, b_k) = X^-1 * b_k          // measurement equation
 *
 *  The algorithm below comprises first a simulator to
 *  produce measurements, then uses these measurements
 *  to estimate the state, using a Lie-based error-state Kalman filter.
 *
 *  This file has plain code with only one main() function.
 *  There are no function calls other than those involving `manif`.
 *
 *  Printing simulated state and estimated state together
 *  with an unfiltered state (i.e. without Kalman corrections)
 *  allows for evaluating the quality of the estimates.
 */

#include "manif/SE3.h"

#include <Eigen/Dense>

#include <vector>

#include <iostream>
#include <iomanip>

using std::cout;
using std::endl;

using namespace Eigen;

typedef Array<double, 3, 1> Array3d;
typedef Array<double, 6, 1> Array6d;
typedef Matrix<double, 6, 1> Vector6d;
typedef Matrix<double, 6, 6> Matrix6d;

int main()
{
    // START CONFIGURATION
    //
    //
    const int NUMBER_OF_LMKS_TO_MEASURE = 5;

    // Define the robot pose element and its covariance
    manif::SE3d X, X_simulation, X_unfiltered;
    Matrix6d    P;

    X_simulation.setIdentity();
    X.setIdentity();
    X_unfiltered.setIdentity();
    P.setZero();

    // Define a control vector and its noise and covariance
    manif::SE3Tangentd  u_simu, u_est, u_unfilt;
    Vector6d            u_nom, u_noisy, u_noise;
    Array6d             u_sigmas;
    Matrix6d            U;

    u_nom    << 0.1, 0.0, 0.0, 0.0, 0.0, 0.05;
    u_sigmas << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
    U        = (u_sigmas * u_sigmas).matrix().asDiagonal();

    // Declare the Jacobians of the motion wrt robot and control
    manif::SE3d::Jacobian J_x, J_u;

    // Define five landmarks in R^3
    Vector3d b0, b1, b2, b3, b4, b;
    b0 << 2.0,  0.0,  0.0;
    b1 << 3.0, -1.0, -1.0;
    b2 << 2.0, -1.0,  1.0;
    b3 << 2.0,  1.0,  1.0;
    b4 << 2.0,  1.0, -1.0;
    std::vector<Vector3d> landmarks;
    landmarks.push_back(b0);
    landmarks.push_back(b1);
    landmarks.push_back(b2);
    landmarks.push_back(b3);
    landmarks.push_back(b4);

    // Define the beacon's measurements
    Vector3d                y, y_noise;
    Array3d                 y_sigmas;
    Matrix3d                R;
    std::vector<Vector3d>   measurements(landmarks.size());

    y_sigmas << 0.01, 0.01, 0.01;
    R        = (y_sigmas * y_sigmas).matrix().asDiagonal();

    // Declare the Jacobian of the measurements wrt the robot pose
    Matrix<double, 3, 6>    H;      // H = J_e_x

    // Declare some temporaries
    Vector3d                e, z;   // expectation, innovation
    Matrix3d                E, Z;   // covariances of the above
    Matrix<double, 6, 3>    K;      // Kalman gain
    manif::SE3Tangentd      dx;     // optimal update step, or error-state
    manif::SE3d::Jacobian   J_xi_x; // Jacobian is typedef Matrix
    Matrix<double, 3, 6>    J_e_xi; // Jacobian

    //
    //
    // CONFIGURATION DONE

    // DEBUG
    cout << std::fixed   << std::setprecision(3) << std::showpos << endl;
    cout << "X STATE     :    X      Y      Z    TH_x   TH_y   TH_z " << endl;
    cout << "-------------------------------------------------------" << endl;
    cout << "X initial   : " << X_simulation.log().coeffs().transpose() << endl;
    cout << "-------------------------------------------------------" << endl;
    // END DEBUG

    // START TEMPORAL LOOP
    //
    //

    // Make 10 steps. Measure up to three landmarks each time.
    for (int t = 0; t < 10; t++)
    {
        //// I. Simulation ###############################################################################

        /// simulate noise
        u_noise = u_sigmas * Array6d::Random();             // control noise
        u_noisy = u_nom + u_noise;                          // noisy control

        u_simu   = u_nom;
        u_est    = u_noisy;
        u_unfilt = u_noisy;

        /// first we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
        X_simulation = X_simulation + u_simu;               // overloaded X.rplus(u) = X * exp(u)

        /// then we measure all landmarks - - - - - - - - - - - - - - - - - - - -
        for (int i = 0; i < landmarks.size(); i++)
        {
            b = landmarks[i];                               // lmk coordinates in world frame

            /// simulate noise
            y_noise = y_sigmas * Array3d::Random();         // measurement noise

            y = X_simulation.inverse().act(b);              // landmark measurement, before adding noise
            y = y + y_noise;                                // landmark measurement, noisy
            measurements[i] = y;                            // store for the estimator just below
        }

        //// II. Estimation ###############################################################################

        /// First we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -

        X = X.plus(u_est, J_x, J_u);                        // X * exp(u), with Jacobians

        P = J_x * P * J_x.transpose() + J_u * U * J_u.transpose();

        /// Then we correct using the measurements of each lmk - - - - - - - - -
        for (int i = 0; i < NUMBER_OF_LMKS_TO_MEASURE; i++)
        {
            // landmark
            b = landmarks[i];                               // lmk coordinates in world frame

           // measurement
            y = measurements[i];                            // lmk measurement, noisy

            // expectation
            e = X.inverse(J_xi_x).act(b, J_e_xi);           // note: e = R.tr * ( b - t ), for X = (R,t).
            H = J_e_xi * J_xi_x;                            // note: H = J_e_x = J_e_xi * J_xi_x
            E = H * P * H.transpose();

            // innovation
            z = y - e;
            Z = E + R;

            // Kalman gain
            K = P * H.transpose() * Z.inverse();            // K = P * H.tr * ( H * P * H.tr + R).inv

            // Correction step
            dx = K * z;                                     // dx is in the tangent space at X

            // Update
            X = X + dx;                                     // overloaded X.rplus(dx) = X * exp(dx)
            P = P - K * Z * K.transpose();
        }

        //// III. Unfiltered ##############################################################################

        // move also an unfiltered version for comparison purposes
        X_unfiltered = X_unfiltered + u_unfilt;

        //// IV. Results ##############################################################################

        // DEBUG
        cout << "X simulated : " << X_simulation.log().coeffs().transpose() << endl;
        cout << "X estimated : " << X.log().coeffs().transpose() << endl;
        cout << "X unfilterd : " << X_unfiltered.log().coeffs().transpose() << endl;
        cout << "-------------------------------------------------------" << endl;
        // END DEBUG

    }

    //
    //
    // END OF TEMPORAL LOOP. DONE.

    return 0;
}


#ifndef FUN_HPP__
#define FUN_HPP__

#include <string>
#include <deque>
#include <functional>
#include <boost/assert.hpp>

//#include <stdlib.h>
namespace biggles {
    /// \brief x^2
    template<class DTYPE> DTYPE square(const DTYPE& x) { return x*x; }
    template<class DTYPE> DTYPE cubed(const DTYPE& x) { return x*x*x; }
    template<class DTYPE> DTYPE abs_diff(const DTYPE& x, const DTYPE& y) {
        return x>y ? x-y : y-x;
    }

    std::string local_time(const std::string& fmt = std::string("%y%m%d_%T"));

    std::string count2str(size_t cnt);
    std::string float2str(float num);
    std::string time2str(float seconds);

    /// \brief a class to hold a fixed number of values
    ///
    /// if a new value is inserted into the full container the last (and oldest) value is removed
    template <class DATATYPE>
    class fqueue {
        typedef typename std::deque<DATATYPE> container_t;
        container_t data_;
        size_t max_size_;
    public:
        explicit fqueue(size_t max_size) : max_size_(max_size) { BOOST_ASSERT(max_size_ > 0); }
        typedef typename container_t::const_iterator const_iterator;
        const_iterator begin() const { return data_.begin(); }
        const_iterator end() const { return data_.end(); }
        void push(const DATATYPE& value) {
            if (max_size_ == data_.size()) data_.pop_back();
            data_.push_front(value);
        }
        bool is_full() const { return max_size_ <= data_.size(); }
        size_t size() const { return data_.size(); }
        DATATYPE last() const { return data_.front(); }
        DATATYPE first() const { return data_.back(); }
    };

    /// \brief this is copied form cplusplus.com and it not required in C++11
    template <class ForwardIterator>
    bool is_sorted (ForwardIterator first, ForwardIterator last) {
      if (first==last) return true;
      ForwardIterator next = first;
      while (++next!=last) {
        if (*next < *first)
          return false;
        ++first;
      }
      return true;
    }

    template<class ITERATOR>
    bool any_weight(ITERATOR b, ITERATOR e) {
        typedef typename ITERATOR::value_type value_type;
        while (b != e) {
            if (*b++ > value_type(0))
                return true;
        }
        return false;
    }

    /// \brief (sum(\em fun(*\em it)) for \em it in [\em b, \em e)) + init
    template<class ITERATOR, class FUN>
    typename FUN::result_type fun_sum(ITERATOR b, ITERATOR e, const FUN& fun, typename FUN::result_type init) {
        while (b != e) init += fun(*b++);
        return init;
    }

    template<class INPUT_ITER, class OUTPUT_ITER, class FUN>
    void fun_partial_sum(INPUT_ITER b, INPUT_ITER e, OUTPUT_ITER ob, const FUN& fun, typename FUN::result_type init) {
        while (b != e) *ob++ = init += fun(*b++);
    }

    /// \brief wrapper of unary function \em FUN for std::partial_sum
    template <class FUN> class fun_sum_operator :
        public std::binary_function<typename FUN::result_type, typename FUN::argument_type, typename FUN::result_type>
    {
        typedef typename FUN::result_type result_type;
        typedef typename FUN::argument_type argument_type;
        const FUN& fun_;
    public:
        fun_sum_operator(const FUN& fun) : fun_(fun) {}
        result_type operator()(const result_type& prev, const argument_type& arg) const { return prev + fun_(arg); }
    }; // fun_sum_operator

template<class EIGEN_MATRIX> bool is_symmetric(const EIGEN_MATRIX &matrix) {
    return matrix == matrix.transpose();
}

template<class EIGEN_MATRIX> float measure_asymmetry(const EIGEN_MATRIX &matrix) {
    return (matrix - matrix.transpose()).determinant();
}

template<class EIGEN_MATRIX> EIGEN_MATRIX enforce_symmetry(const EIGEN_MATRIX &matrix) {
    return (matrix + matrix.transpose())/2.0;
}

} // namespace biggles

#endif


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

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

/**
 * \file     univariate_distribution_estimator_impl.cpp
 * \author   Collin Johnson
 *
 * Definition of implementations of the UnivariateDistributionEstimator for the subclasses of UnivariateDistribution:
 *
 *   - UnivariateGaussianDistribution
 *   - GammaDistribution
 *   - BetaDistribution
 *   - ExponentialDistribution
 *   - TruncatedGaussianDistribution
 */

#include "math/univariate_distribution_estimator_impl.h"
#include "math/beta_distribution.h"
#include "math/discrete_gaussian.h"
#include "math/exponential_distribution.h"
#include "math/gamma_distribution.h"
#include "math/statistics.h"
#include "math/truncated_gaussian_distribution.h"
#include "math/univariate_gaussian.h"
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <iostream>

namespace vulcan
{
namespace math
{

using DistPtr = std::unique_ptr<UnivariateDistribution>;

double log_likelihood_gamma_func(double k, double sumXi, double sumLogXi, std::size_t n)
{
    return (k - 1) * sumXi - n * k - n * k * std::log(sumXi / (k * n)) - n * std::log(boost::math::tgamma(k));
}

double log_likelihood_gamma_deriv(double k, double sumXi, double sumLogXi, std::size_t n)
{
    return n * (std::log(k) - boost::math::digamma(k) - std::log(sumXi / n)) + sumLogXi;
}

DistPtr
  UnivariateGaussianDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                                const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new UnivariateGaussianDistribution(estimate(dataBegin, dataEnd))};
}

UnivariateGaussianDistribution
  UnivariateGaussianDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                    const std::vector<double>::const_iterator dataEnd) const
{
    double var = variance(dataBegin, dataEnd);

    if (var == 0.0) {
        std::cerr << "WARNING: UnivariateGaussianDistributionEstimator: Variance in the data was 0. Setting variance "
                     "to 1e-4.\n";
        var = 1e-4;
    }

    return UnivariateGaussianDistribution(mean(dataBegin, dataEnd), var);
}

DistPtr
  DiscreteGaussianDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                              const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new DiscreteGaussianDistribution(estimate(dataBegin, dataEnd))};
}

DiscreteGaussianDistribution
  DiscreteGaussianDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                  const std::vector<double>::const_iterator dataEnd) const
{
    double var = variance(dataBegin, dataEnd);

    if (var == 0.0) {
        std::cerr
          << "WARNING: DiscreteGaussianDistributionEstimator: Variance in the data was 0. Setting variance to 1e-4.\n";
        var = 1e-4;
    }

    return DiscreteGaussianDistribution(mean(dataBegin, dataEnd), var);
}

DistPtr GammaDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                         const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new GammaDistribution(estimate(dataBegin, dataEnd))};
}

GammaDistribution GammaDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                       const std::vector<double>::const_iterator dataEnd) const
{
    std::vector<double> filtered(dataBegin, dataEnd);
    auto validDataEnd = std::remove_if(filtered.begin(), filtered.end(), [](double x) {
        return x <= 0.0;
    });

    int numValidData = std::distance(filtered.begin(), validDataEnd);
    assert(numValidData);

    using namespace std::placeholders;

    double sumXi = std::accumulate(filtered.begin(), validDataEnd, 0.0);
    double sumLogXi = 0.0;
    for (auto val : boost::make_iterator_range(filtered.begin(), validDataEnd)) {
        sumLogXi += std::log(val);
    }

    double s = std::log(sumXi / numValidData) - sumLogXi / numValidData;
    double k0 = (3.0 - s + std::sqrt(std::pow(s - 3.0, 2.0) + 24.0 * s)) / (12.0 * s);

    //     NewtonRaphsonErrorFunc<double> newton(std::bind(log_likelihood_func,  _1, sumXi, sumLogXi, filtered.size()),
    //                                           std::bind(log_likelihood_deriv, _1, sumXi, sumLogXi, filtered.size()));
    //
    //     double k     = find_single_root(newton, k0, 1e-5);
    double k = k0;
    double theta = sumXi / (numValidData * k);
    return GammaDistribution(k, theta);
}

DistPtr BetaDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                        const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new BetaDistribution(estimate(dataBegin, dataEnd))};
}

BetaDistribution BetaDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                     const std::vector<double>::const_iterator dataEnd) const
{
    double mean = math::mean(dataBegin, dataEnd);
    double variance = math::variance(dataBegin, dataEnd);

    if (variance == 0.0) {
        std::cerr << "WARNING: BetaDistributionEstimator: Variance of data was 0. Setting to 1e-4.\n";
        variance = 1e-4;
    }

    if (variance < mean * (1 - mean) && (variance > 0.0)) {
        double alpha = mean * ((mean * (1.0 - mean) / variance) - 1.0);
        double beta = (1.0 - mean) * ((mean * (1.0 - mean) / variance) - 1.0);
        return BetaDistribution(alpha, beta);
    } else {
        std::cerr << "ERROR: BetaDistributionEstimator: Could not use method-of-moments to find parameters. Mean:"
                  << mean << " Variance:" << variance << '\n'
                  << " Variance should be less than " << (mean * (1 - mean)) << '\n';
        return BetaDistribution();
    }
}

ExponentialDistributionEstimator::ExponentialDistributionEstimator(double maxValue) : max_(maxValue)
{
}

DistPtr ExponentialDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                               const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new ExponentialDistribution(estimate(dataBegin, dataEnd))};
}

ExponentialDistribution
  ExponentialDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                             const std::vector<double>::const_iterator dataEnd) const
{
    double mean = math::mean(dataBegin, dataEnd);
    if (mean > 0.0) {
        return ExponentialDistribution(1.0 / mean, max_);
    } else {
        std::cerr << "ERROR: ExponentialDistributionEstimator: Invalid mean for the data:" << mean
                  << " Must be greater than 0.\n";
        return ExponentialDistribution(1.0, max_);
    }
}

TruncatedGaussianDistributionEstimator::TruncatedGaussianDistributionEstimator(double lower, double upper)
: lower_(lower)
, upper_(upper)
{
}

DistPtr
  TruncatedGaussianDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                               const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new TruncatedGaussianDistribution(estimate(dataBegin, dataEnd))};
}

TruncatedGaussianDistribution
  TruncatedGaussianDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                   const std::vector<double>::const_iterator dataEnd) const
{
    double var = variance(dataBegin, dataEnd);

    if (var == 0.0) {
        std::cerr
          << "WARNING: TruncatedGaussianDistributionEstimator: Variance in the data was 0. Setting variance to 1e-4.\n";
        var = 1e-4;
    }

    return TruncatedGaussianDistribution(mean(dataBegin, dataEnd), var, lower_, upper_);
}

}   // namespace math
}   // namespace vulcan


/*
 * Copyright (c) 2021 Nobuyuki Umetani
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <random>
#include <vector>
#include <cstdlib>
#include <Eigen/Core>
#if defined(_WIN32) // windows
#  define NOMINMAX   // to remove min,max macro
#  include <windows.h>  // should put before glfw3.h
#endif
#define GL_SILENCE_DEPRECATION
#include <GLFW/glfw3.h>

#include "delfem2/mshprimitive.h"
#include "delfem2/eigen/ls_dense.h"
#include "delfem2/eigen/ls_sparse.h"
#include "delfem2/femsolidlinear.h"
#include "delfem2/lsitrsol.h"
#include "delfem2/mshuni.h"
#include "delfem2/femutil.h"
#include "delfem2/glfw/viewer3.h"
#include "delfem2/glfw/util.h"
#include "delfem2/opengl/old/funcs.h"
#include "delfem2/opengl/old/mshuni.h"

namespace dfm2 = delfem2;

// --------------------------------------------------------------

void Simulation_Mat3(
    std::vector<double> &aDisp,
    delfem2::CMatrixSparseBlock<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> &mA,
    //
    const std::vector<double> &aXYZ0,
    const std::vector<unsigned int> &aHex,
    const std::vector<int> &aBCFlag,
    //
    double mass,
    double myu,
    double lambda,
    const double gravity[3]) {
  const unsigned int np = aXYZ0.size() / 3;
  const unsigned int nDoF = np * 3;
  mA.setZero();
  {
    double ddW[8][8][3][3];
    {
      const double gravity_zero[3] = {0, 0, 0};
      double aP0[8][3], aU[8][3];
      delfem2::FetchData<8, 3>(aP0, aHex.data(), aXYZ0.data());
      delfem2::FetchData<8, 3>(aU, aHex.data(), aDisp.data());
      double dW[8][3];
      std::fill_n(&dW[0][0], 8 * 3, 0.0);
      std::fill_n(&ddW[0][0][0][0], 8 * 8 * 3 * 3, 0.0);
      delfem2::elemMatRes_LinearSolidGravity3_Static_Q1(
          myu, lambda,
          0, gravity_zero,
          aP0, aU, ddW, dW);
    }
    std::vector<unsigned int> tmp_buffer;
    for (unsigned int ih = 0; ih < aHex.size() / 8; ++ih) {
      const unsigned int *aIP = aHex.data() + ih * 8;
      delfem2::Merge<8, 8, 3, 3, double>(mA, aIP, aIP, ddW, tmp_buffer);
    }
  }
  Eigen::VectorXd vec_b(nDoF);
  vec_b.setZero();
  for (unsigned int ip = 0; ip < np; ++ip) {
    vec_b[ip * 3 + 0] += mass * gravity[0];
    vec_b[ip * 3 + 1] += mass * gravity[1];
    vec_b[ip * 3 + 2] += mass * gravity[2];
  }
  { // comput rhs vectors
    const Eigen::VectorXd &vd = Eigen::Map<const Eigen::VectorXd>(aDisp.data(), nDoF);
    AddMatVec(vec_b, 1.0, -1.0, mA, vd);
    std::cout << "energy" << vec_b.dot(vd) << std::endl;
  }
  SetFixedBC_Dia(mA, aBCFlag.data(), 1.f);
  SetFixedBC_Col(mA, aBCFlag.data());
  SetFixedBC_Row(mA, aBCFlag.data());
  delfem2::setZero_Flag(vec_b, aBCFlag, 0);
  // --------------------------------
  Eigen::VectorXd vec_x(vec_b.size());
  {
    double conv_ratio = 1.0e-6;
    int iteration = 1000;
    const std::size_t n = vec_b.size();
    Eigen::VectorXd tmp0(n), tmp1(n);
    std::vector<double> aConv = delfem2::Solve_CG(
        vec_b, vec_x, tmp0, tmp1,
        conv_ratio, iteration, mA);
    std::cout << aConv.size() << std::endl;
  }
  // ------------------------------
  dfm2::XPlusAY(
      aDisp,
      aBCFlag, 1.0, vec_x);
}

void Simulation_Mat4(
    std::vector<double> &aDisp,
    delfem2::CMatrixSparseBlock<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>, 3> &mA,
    //
    const std::vector<double> &aXYZ0,
    const std::vector<unsigned int> &aHex,
    const std::vector<int> &aBCFlag,
    //
    double mass,
    double myu,
    double lambda,
    const double gravity[3]) {
  const unsigned int np = aXYZ0.size() / 3;
  mA.setZero();
  {
    double ddW[8][8][3][3];
    {
      const double gravity_zero[3] = {0, 0, 0};
      double aP0[8][3], aU[8][3];
      delfem2::FetchData<8, 3>(aP0, aHex.data(), aXYZ0.data());
      delfem2::FetchData<8, 3>(aU, aHex.data(), aDisp.data());
      double dW[8][3];
      std::fill_n(&dW[0][0], 8 * 3, 0.0);
      std::fill_n(&ddW[0][0][0][0], 8 * 8 * 3 * 3, 0.0);
      delfem2::elemMatRes_LinearSolidGravity3_Static_Q1(
          myu, lambda,
          0, gravity_zero,
          aP0, aU, ddW, dW);
    }
    std::vector<unsigned int> tmp_buffer;
    for (unsigned int ih = 0; ih < aHex.size() / 8; ++ih) {
      const unsigned int *aIP = aHex.data() + ih * 8;
      delfem2::Merge<8, 8, 3, 3, double>(mA, aIP, aIP, ddW, tmp_buffer);
    }
  }
  Eigen::Matrix<double, -1, 4, Eigen::RowMajor> vec_b(np, 4);
  vec_b.setZero();
  for (unsigned int ip = 0; ip < np; ++ip) {
    vec_b(ip, 0) += mass * gravity[0];
    vec_b(ip, 1) += mass * gravity[1];
    vec_b(ip, 2) += mass * gravity[2];
  }
  { // comput rhs vectors
    Eigen::Matrix<double, -1, 4, Eigen::RowMajor> vd(np, 4);
    for (unsigned int ip = 0; ip < np; ++ip) {
      vd(ip, 0) = aDisp[ip * 3 + 0];
      vd(ip, 1) = aDisp[ip * 3 + 1];
      vd(ip, 2) = aDisp[ip * 3 + 2];
      vd(ip, 3) = 0.0;
    }
    AddMatVec(vec_b, 1.0, -1.0, mA, vd);
    std::cout << "energy" << delfem2::Dot(vec_b, vd) << std::endl;
  }
  SetFixedBC_Dia(mA, aBCFlag.data(), 1.f);
  SetFixedBC_Col(mA, aBCFlag.data());
  SetFixedBC_Row(mA, aBCFlag.data());
  delfem2::setZero_Flag(vec_b, np, aBCFlag, 0);
  // --------------------------------
  Eigen::Matrix<double, -1, 4, Eigen::RowMajor> vec_x(np, 4);
  {
    double conv_ratio = 1.0e-6;
    int iteration = 1000;
    Eigen::Matrix<double, -1, 4, Eigen::RowMajor> tmp0(np, 4), tmp1(np, 4);
    std::vector<double> aConv = delfem2::Solve_CG(
        vec_b, vec_x, tmp0, tmp1,
        conv_ratio, iteration, mA);
    std::cout << aConv.size() << std::endl;
  }
  // ------------------------------
  dfm2::XPlusAY(
      aDisp,
      np, aBCFlag, 1.0, vec_x);
}

int main() {
  std::vector<double> aXYZ0;
  std::vector<unsigned int> aHex;
  dfm2::MeshHex3_Grid(
      aXYZ0, aHex,
      20, 10, 10, 0.1);
  std::vector<double> aMass(aXYZ0.size() / 3);

  delfem2::CMatrixSparseBlock<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> A3;
  delfem2::CMatrixSparseBlock<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>, 3> A4;
  {
    const unsigned int np = aXYZ0.size() / 3;
    std::vector<unsigned int> psup_ind, psup;
    dfm2::JArray_PSuP_MeshElem(
        psup_ind, psup,
        aHex.data(), aHex.size() / 8, 8,
        aXYZ0.size() / 3);
    A3.Initialize(np);
    A3.SetPattern(psup_ind.data(), psup_ind.size(), psup.data(), psup.size());
    A4.Initialize(np);
    A4.SetPattern(psup_ind.data(), psup_ind.size(), psup.data(), psup.size());
  }

  std::vector<double> aDisp(aXYZ0.size(), 0.0);
  std::vector<int> aBCFlag(aXYZ0.size(), 0.0); // 0: free, 1: fix BC
  {
    for (unsigned int ip = 0; ip < aXYZ0.size() / 3; ++ip) {
      double x0 = aXYZ0[ip * 3 + 0];
      if (x0 > 1.0e-10) { continue; }
      aBCFlag[ip * 3 + 0] = 1;
      aBCFlag[ip * 3 + 1] = 1;
      aBCFlag[ip * 3 + 2] = 1;
    }
  }
  const double mass = 0.5;
  const double gravity[3] = {0, 0, -10};
  aDisp.assign(aXYZ0.size(), 0.0);
  for (unsigned int i = 0; i < aDisp.size(); ++i) {
    if (aBCFlag[i] != 0) { continue; }
    aDisp[i] = (i % 10) * 1.0e-4;
  }
  Simulation_Mat3(
      aDisp, A3,
      aXYZ0, aHex, aBCFlag,
      mass, 1.0e+5, 1.e+5, gravity);

  aDisp.assign(aXYZ0.size(), 0.0);
  for (unsigned int i = 0; i < aDisp.size(); ++i) {
    if (aBCFlag[i] != 0) { continue; }
    aDisp[i] = (i % 10) * 1.0e-4;
  }
  Simulation_Mat4(
      aDisp, A4,
      aXYZ0, aHex, aBCFlag,
      mass, 1.0e+5, 1.e+5, gravity);

  // ----------------------
  delfem2::glfw::CViewer3 viewer(1.5);
//  viewer.camera.camera_rot_mode = delfem2::CCam3_OnAxisZplusLookOrigin<double>::CAMERA_ROT_MODE::ZTOP;
//  viewer.camera.theta = 0.1;
//  viewer.camera.psi = 0.1;
  delfem2::glfw::InitGLOld();
  viewer.InitGL();

  delfem2::opengl::setSomeLighting();
  while (!glfwWindowShouldClose(viewer.window)) {
    // -----
    viewer.DrawBegin_oldGL();
    ::glDisable(GL_LIGHTING);
    ::glColor3d(0, 0, 0);
    delfem2::opengl::DrawMeshHex3D_EdgeDisp(
        aXYZ0.data(), aXYZ0.size() / 3,
        aHex.data(), aHex.size() / 8,
        aDisp.data());
    //
    ::glEnable(GL_LIGHTING);
//    dfm2::opengl::DrawMeshHex3D_FaceNorm(aXYZ0.data(), aHex.data(), aHex.size() / 8);
    delfem2::opengl::DrawHex3D_FaceNormDisp(aXYZ0, aHex, aDisp);
    viewer.SwapBuffers();
    glfwPollEvents();
  }
  glfwDestroyWindow(viewer.window);
  glfwTerminate();
  exit(EXIT_SUCCESS);
}