#pragma once
#include <vector>
#include <fstream>
#include "settings.hpp"
#if USE_EIGEN == 1
#include <Eigen/Dense>
#endif

namespace nn
{
	enum LoadSettings
	{
		kVectorize = 0x1,
		kNormalize = 0x2
	};

	using MatrixType = Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>;
	std::vector<MatrixType> loadMNISTImages(const std::string& filename, LoadSettings settings, uint32_t maxImages = 0xFFFFFFFF)
	{
		std::ifstream in(filename, std::ifstream::binary);
		if (in)
		{
			in.seekg(0, in.end);
			size_t fileLen = in.tellg();
			in.seekg(0, in.beg);
			std::vector<unsigned char> images(fileLen);
			in.read(reinterpret_cast<char*>(images.data()), images.size());
			for (size_t i = 0; i < 16; i += 4)
			{
				std::swap(images[i], images[i + 3]);
				std::swap(images[i + 1], images[i + 2]);
			}
			uint32_t magic, datasetSize, imageWidth, imageHeight;
			magic = *reinterpret_cast<uint32_t*>(images.data());
			if (magic != 0x00000803)
				throw std::runtime_error("Bad magic number");
			datasetSize = *reinterpret_cast<uint32_t*>(images.data() + 4);
			if (datasetSize > maxImages)
				datasetSize = maxImages;
			imageWidth = *reinterpret_cast<uint32_t*>(images.data() + 8);
			imageHeight = *reinterpret_cast<uint32_t*>(images.data() + 12);
			std::vector<MatrixType> result;
			size_t position = 16;
			const auto vectorizedSize = imageWidth * imageHeight;
			for (uint32_t i = 0u; i < datasetSize; ++i)
			{
				MatrixType image(vectorizedSize, 1);
					for (size_t j = 0; j < vectorizedSize; ++j)
						image(j, 0) = real(images[position++]);
				if (settings & LoadSettings::kNormalize)
					image /= 255.0;

				if (settings & LoadSettings::kVectorize)
					result.push_back(image);
				else
					result.push_back(Eigen::Map<MatrixType>(image.data(), imageHeight, imageWidth));
			}
			return result;
		}
		else
			throw std::runtime_error("Can't open MNIST images dataset");
	}

	std::vector<uint8_t> loadMNISTLabels(const std::string& filename, uint32_t maxLabels = 0xFFFFFFFF)
	{
		std::ifstream in(filename, std::ifstream::binary);
		if (in)
		{
			in.seekg(0, in.end);
			size_t fileLen = in.tellg();
			in.seekg(0, in.beg);
			std::vector<unsigned char> images(fileLen);
			in.read(reinterpret_cast<char*>(images.data()), images.size());
			for (size_t i = 0; i < 8; i += 4)
			{
				std::swap(images[i], images[i + 3]);
				std::swap(images[i + 1], images[i + 2]);
			}
			uint32_t magic, datasetSize;
			magic = *reinterpret_cast<uint32_t*>(images.data());
			if (magic != 0x00000801)
				throw std::runtime_error("Bad magic number");
			datasetSize = *reinterpret_cast<uint32_t*>(images.data() + 4);
			if (datasetSize > maxLabels)
				datasetSize = maxLabels;
			std::vector<uint8_t> result(images.cbegin() + 8, images.cbegin() + 8 + datasetSize);
			return result;
		}
		else
			throw std::runtime_error("Can't open MNIST images dataset");
	}
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief unit.cpp

\detailed
Test unit algebra.

Output:
@verbatim

//[unit_output
L             = m
L+L           = m
L-L           = m
L/L           = dimensionless
meter*meter   = m^2
M*(L/T)*(L/T) = m^2 kg s^-2
M*(L/T)^2     = m^2 kg s^-2
L^3           = m^3
L^(3/2)       = m^(3/2)
2vM           = kg^(1/2)
(3/2)vM       = kg^(2/3)
//]

@endverbatim
**/

#include <iostream>

#include "test_system.hpp"

#include <boost/units/pow.hpp>

int main()
{
    using namespace boost::units;
    using namespace boost::units::test;

    //[unit_snippet_1
    const length                    L;
    const mass                      M;
    // needs to be namespace-qualified because of global time definition
    const boost::units::test::time  T;
    const energy                    E;
    //]

    std::cout << "L             = " << L << std::endl
              << "L+L           = " << L+L << std::endl
              << "L-L           = " << L-L << std::endl
              << "L/L           = " << L/L << std::endl
              << "meter*meter   = " << meter*meter << std::endl
              << "M*(L/T)*(L/T) = " << M*(L/T)*(L/T) << std::endl
              << "M*(L/T)^2     = " << M*pow<2>(L/T) << std::endl
              << "L^3           = " << pow<3>(L) << std::endl
              << "L^(3/2)       = " << pow<static_rational<3,2> >(L)
              << std::endl
              << "2vM           = " << root<2>(M) << std::endl
              << "(3/2)vM       = " << root<static_rational<3,2> >(M)
              << std::endl;

    return 0;
}


//  french.cpp  ----------------------------------------------------------//

//  Copyright 2010 Howard Hinnant
//  Copyright 2011 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

// Adapted to Boost from the original Hawards's code

#include <iostream>
//#include <boost/chrono/chrono_io.hpp>
#include <boost/chrono/floor.hpp>
#include <boost/chrono/round.hpp>
#include <boost/chrono/ceil.hpp>

int main()
{
  boost::chrono::milliseconds ms(2500);
  std::cout << __FILE__ << "[" << __LINE__ << "]" << std::endl;
  std::cout << boost::chrono::floor<boost::chrono::seconds>(ms).count()
      << " seconds\n";
  std::cout << __FILE__ << "[" << __LINE__ << "]" << std::endl;
  std::cout << boost::chrono::round<boost::chrono::seconds>(ms).count()
      << " seconds\n";
  std::cout << __FILE__ << "[" << __LINE__ << "]" << std::endl;
  std::cout << boost::chrono::ceil<boost::chrono::seconds>(ms).count()
      << " seconds\n";
  std::cout << __FILE__ << "[" << __LINE__ << "]" << std::endl;
  ms = boost::chrono::milliseconds(2516);
  std::cout << __FILE__ << "[" << __LINE__ << "]" << std::endl;
  typedef boost::chrono::duration<long, boost::ratio<1, 30> > frame_rate;
  std::cout << __FILE__ << "[" << __LINE__ << "]" << std::endl;
  std::cout << boost::chrono::floor<frame_rate>(ms).count()
      << " [1/30] seconds\n";
  std::cout << __FILE__ << "[" << __LINE__ << "]" << std::endl;
  std::cout << boost::chrono::round<frame_rate>(ms).count()
      << " [1/30] seconds\n";
  std::cout << __FILE__ << "[" << __LINE__ << "]" << std::endl;
  std::cout << boost::chrono::ceil<frame_rate>(ms).count()
      << " [1/30] seconds\n";
  std::cout << __FILE__ << "[" << __LINE__ << "]" << std::endl;

  return 0;
}


/*====================================================================
 * Simple pendulum example
 * Copyright (c) 2015 Matthew Millard
 * <matthew.millard@iwr.uni-heidelberg.de>
 *
 *///=================================================================

#include <string>
#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <rbdl/rbdl.h>
#include "csvtools.h"

#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
//using namespace std;
using namespace boost::numeric::odeint;

#ifndef RBDL_BUILD_ADDON_LUAMODEL
    #error "Error: RBDL addon LuaModel not enabled."
#endif

#include <rbdl/addons/luamodel/luamodel.h>
#include <rbdl/addons/luamodel/luatables.h>

using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;

//====================================================================
// Boost stuff
//====================================================================

typedef std::vector< double > state_type;

typedef runge_kutta_cash_karp54< state_type > error_stepper_type;
typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;

class rbdlToBoost {
    public:
        rbdlToBoost(Model& model,std::vector<ConstraintSet>& cs
                        ) : model(model),cs(cs) {
            q = VectorNd::Zero(model.dof_count);
            qd = VectorNd::Zero(model.dof_count);
            qdd = VectorNd::Zero(model.dof_count);
            tau = VectorNd::Zero(model.dof_count);

        }

        void operator() (const state_type &x,
                         state_type &dxdt,
                         const double t){

            //q
            int j = 0;
            for(unsigned int i=0; i<model.dof_count; i++){
                q[i] = (double)x[j];
                j++;
            }

            //qd
            for(unsigned int i=0; i<model.dof_count; i++){
                qd[i] = (double)x[j];
                j++;
            }
            //tau = for now. This could call a control
            //               function.
            for(unsigned int i=0; i<model.dof_count; i++){
                tau[i] = 0;
            }

            //2c. A special forward dynamics function needs to be called in
            //    order to compute qdd which simultaneously satisfies the
            //    constraint set and the equations of motion.
            ForwardDynamicsConstraintsDirect (model, q, qd, tau, cs[0], qdd);

            //populate dxdt
            j = 0;
            for(unsigned int i = 0; i < model.dof_count; i++){
                dxdt[j] = (double)qd[i];
                j++;
            }
            for(unsigned int i = 0; i < model.dof_count; i++){
                dxdt[j] = (double)qdd[i];
                j++;
            }

        }

    private:
        Model& model;
        std::vector< RigidBodyDynamics::ConstraintSet >& cs;
        VectorNd q, qd, qdd, tau;
};

struct pushBackStateAndTime
{
    std::vector< state_type >& states;
    std::vector< double >& times;

    pushBackStateAndTime( std::vector< state_type > &states ,
                              std::vector< double > &times )
    : states( states ) , times( times ) { }

    void operator()( const state_type &x , double t )
    {
        states.push_back( x );
        times.push_back( t );
    }
};

void f(const state_type &x, state_type &dxdt, const double t);

/* Problem Constants */
int main (int argc, char* argv[]) {
    rbdl_check_api_version (RBDL_API_VERSION);

    //problem specific constants
    int     nPts    = 100;
    double  t0      = 0;
    double  t1      = 3;

    //Integration settings
    double absTolVal   = 1e-6;
    double relTolVal   = 1e-6;

    VectorNd q, qd, qdd, tau;

    RigidBodyDynamics::Model model;
    string modelFile = "./../model/constrainedDoublePendulum.lua";

    //2a. Some extra work is needed to load in the model, and the
    //    constraint sets
    std::vector<std::string> constraintSetNames =
        Addons::LuaModelGetConstraintSetNames(modelFile.c_str());
    std::vector<RigidBodyDynamics::ConstraintSet> constraintSets;

    constraintSets.resize(constraintSetNames.size());

    if (! Addons::LuaModelReadFromFileWithConstraints(
                                      modelFile.c_str(),
                                      &model,
                                      constraintSets,
                                      constraintSetNames,
                                      false)            ){
        std::cerr     << "Error loading model" << std::endl;
        abort();
    }

    q       = VectorNd::Zero (model.dof_count);
    qd      = VectorNd::Zero (model.dof_count);
    qdd     = VectorNd::Zero (model.dof_count);
    tau     = VectorNd::Zero (model.dof_count);

    printf("DoF: %i\n",model.dof_count);

    printf("==============================\n");
    printf("1. Forward Dynamics \n");
    printf("==============================\n");

    // rx0 x1,y1,z1,rx1,ry1,rz1
    //  0,  1, 2, 3,  4,  5,  6,
    q[2] = 1.0;

    rbdlToBoost rbdlModel(model,constraintSets);
    state_type xState(model.dof_count*2);

    int j = 0;
    for(unsigned int i = 0; i< (model.dof_count); ++i){
        xState[j++] = q(i);
    }
    for(unsigned int i = 0; i< (model.dof_count); ++i){
        xState[j++] = qd(i);
    }

    double dt   = (t1-t0)/((double)nPts);

    double ke, pe = 0;

    std::vector<std::vector< double > > matrixData;
    std::vector<std::vector< double > > matrixPlotData;
    std::vector< double > rowData(model.dof_count+1);
    std::vector< double > rowPlotData(4);

    double a_x = 1.0 , a_dxdt = 1.0;
    controlled_stepper_type
    controlled_stepper(
        default_error_checker< double ,
                               range_algebra ,
                               default_operations >
        ( absTolVal , relTolVal , a_x , a_dxdt ) );

    double tp = 0;
    rowData[0] = 0;
    for(unsigned int z=0; z < model.dof_count; z++){
        rowData[z+1] = xState[model.dof_count + z];
    }
    matrixData.push_back(rowData);
    double t = 0;

    VectorNd constraintPosError = VectorNd::Zero(5);
    VectorNd constraintVelError = VectorNd::Zero(5);

    double constraintPosNorm, constraintVelNorm;

    printf("Columns\n");
    printf("      t,        ke,       pe,    ke+pe ,norm(cons_pos), norm(cons_vel)\n");
    for(int i = 0; i <= nPts; i++){

        t = t0 + dt*i;

        integrate_adaptive(
            controlled_stepper ,
            rbdlModel , xState , tp , t , (t-tp)/10000 );
        tp = t;

        j = 0;
        for(unsigned int k = 0; k < model.dof_count; ++k){
            q[k] = xState[j++];
        }
        for(unsigned int k = 0; k < model.dof_count; ++k){
            qd[k] = xState[j++];
        }

        //Ensure that the constraint set quantites are up to date
        ForwardDynamicsConstraintsDirect (model, q, qd, tau,
          constraintSets[0], qdd);

        //2d. The position-level and velocity level constraint set errors
        //    can be fished out of the vectors in the ConstraintSet struct
        //
        //    Note: In the coming months there will be some functions added so that you
        //          can easily access this information.
        //
        //    Homework: After reading Featherstone & Orin Sec. 3.4, go and have a look
        //              at the fields in the ConstraintSet struct in
        //              rbdl-orb/include/rbdl/Constraints.h
        //
        //              At the same time, open the doxygen for RBDL and read the sections
        //              on Constraints and ConstraintSets
        ConstraintSet &ci = constraintSets[0];
        constraintPosNorm = 0;
        constraintVelNorm = 0;
        for(int k=0; k<5;++k){
          constraintPosError(k) = ci.err(k);
          constraintVelError(k) = ci.errd(k);

          constraintPosNorm += ci.err(k)*ci.err(k);
          constraintVelNorm += ci.errd(k)*ci.errd(k);
        }
        constraintPosNorm = sqrt(constraintPosNorm);
        constraintVelNorm = sqrt(constraintVelNorm);

        pe = Utils::CalcPotentialEnergy(model, q, true);
        ke = Utils::CalcKineticEnergy(model, q, qd, true);

        printf("%f, %f, %f, %f, %f, %f\n",
                    t, ke,pe, ke+pe, constraintPosNorm, constraintVelNorm);

        rowData[0] = t;
        for(unsigned int z=0; z < model.dof_count; z++){
            rowData[z+1] = xState[z];
        }
        matrixData.push_back(rowData);

        rowPlotData[0] = t;
        rowPlotData[1] = constraintPosNorm;
        rowPlotData[2] = constraintVelNorm;
        rowPlotData[3] =(ke+pe);

        matrixPlotData.push_back(rowPlotData);
    }
    printf("Columns\n");
    printf("      t,        ke,       pe,    ke+pe ,norm(cons_pos), norm(cons_vel)\n");

    std::string header = "";
    std::string fname   = "../output/meshup.csv";
    printMatrixToFile(matrixData, header, fname);
    printf("Wrote: ../output/meshup.csv (meshup animation file)\n");

    fname   = "../output/simulationData.csv";
    header = "time,constraintPositionNorm,constraintVelocityNorm,systemEnergy,";
    printMatrixToFile(matrixPlotData, header, fname);
    printf("Wrote: ../output/simulationData.csv (error data)\n");

    return 0;

}


#include <iostream>
#include <armadillo>
#include <cmath>
#include <cstdlib>
#include <iomanip>
#include "iterator.hpp"

using namespace std;

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Не хватает аргументов. Нужно указать шаг n." << endl;
        return -1;
    }

    long long unsigned int n = static_cast<long long unsigned int>(atoi(argv[1]));
    arma::vec iter_values(2, arma::fill::zeros);

	iter_values = {0, 0};
	iter_values = Iterator(n, iter_values, 0.0);
	iter_values = Iterator(n, iter_values, 0.01);
	iter_values = Iterator(n, iter_values, 0.5);
	iter_values = Iterator(n, iter_values, 1);
	iter_values = Iterator(n, iter_values, 5);
	iter_values = Iterator(n, iter_values, 10);

    return 0;
}


//==================================================================================================
/*!
  @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_POW_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_POW_HPP_INCLUDED

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

  /*!
    @ingroup group-exponential
    This function object computes \f$x^y\f$

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

    @par Decorators
      - std_ decorator calls @c std:pow

      - raw_ decorator almost nearly uses the naive formula (\f$e^{y\log x}\f$)
         and so does not care for limits and leads to lower accuracy

    @par Example:

      @snippet pow.cpp pow

    @par Possible output:

      @snippet pow.txt pow

  **/
  ///@{
  IEEEValue pow(IEEEValue const& x, Value const& y);
  IntegerValue pow(IntegerValue const& x, IntegerValue const& y);
  ///@}
} }
#endif

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

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <algorithms/test_overlay.hpp>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/point.hpp>

#include <boost/geometry/algorithms/detail/partition.hpp>

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

#if defined(TEST_WITH_SVG)
# include <boost/geometry/extensions/io/svg/svg_mapper.hpp>
#endif

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

template <typename Box>
struct box_item
{
    int id;
    Box box;
    box_item(int i = 0, std::string const& wkt = "")
        : id(i)
    {
        if (! wkt.empty())
        {
            bg::read_wkt(wkt, box);
        }
    }
};

struct get_box
{
    template <typename Box, typename InputItem>
    static inline void apply(Box& total, InputItem const& item)
    {
        bg::expand(total, item.box);
    }
};

struct ovelaps_box
{
    template <typename Box, typename InputItem>
    static inline bool apply(Box const& box, InputItem const& item)
    {
        return ! bg::detail::disjoint::disjoint_box_box(box, item.box);
    }
};

template <typename Box>
struct box_visitor
{
    int count;
    typename bg::default_area_result<Box>::type area;

    box_visitor()
        : count(0)
        , area(0)
    {}

    template <typename Item>
    inline void apply(Item const& item1, Item const& item2)
    {
        if (bg::intersects(item1.box, item2.box))
        {
            Box b;
            bg::intersection(item1.box, item2.box, b);
            area += bg::area(b);
            count++;
        }
    }
};

template <typename Box>
void test_boxes(std::string const& wkt_box_list, double expected_area, int expected_count)
{
    std::vector<std::string> wkt_boxes;

    boost::split(wkt_boxes, wkt_box_list, boost::is_any_of(";"), boost::token_compress_on);

    typedef box_item<Box> sample;
    std::vector<sample> boxes;

    int index = 1;
    BOOST_FOREACH(std::string const& wkt, wkt_boxes)
    {
        boxes.push_back(sample(index++, wkt));
    }

    box_visitor<Box> visitor;
    bg::partition
        <
            Box, get_box, ovelaps_box
        >::apply(boxes, visitor, 1);

    BOOST_CHECK_CLOSE(visitor.area, expected_area, 0.001);
    BOOST_CHECK_EQUAL(visitor.count, expected_count);
}

struct point_item
{
    point_item()
        : id(0)
    {}

    int id;
    double x;
    double y;
};

BOOST_GEOMETRY_REGISTER_POINT_2D(point_item, double, cs::cartesian, x, y)

struct get_point
{
    template <typename Box, typename InputItem>
    static inline void apply(Box& total, InputItem const& item)
    {
        bg::expand(total, item);
    }
};

struct ovelaps_point
{
    template <typename Box, typename InputItem>
    static inline bool apply(Box const& box, InputItem const& item)
    {
        return ! bg::disjoint(item, box);
    }
};

struct point_visitor
{
    int count;

    point_visitor()
        : count(0)
    {}

    template <typename Item>
    inline void apply(Item const& item1, Item const& item2)
    {
        if (bg::equals(item1, item2))
        {
            count++;
        }
    }
};

void test_points(std::string const& wkt1, std::string const& wkt2, int expected_count)
{
    bg::model::multi_point<point_item> mp1, mp2;
    bg::read_wkt(wkt1, mp1);
    bg::read_wkt(wkt2, mp2);

    int id = 1;
    BOOST_FOREACH(point_item& p, mp1)
    { p.id = id++; }
    id = 1;
    BOOST_FOREACH(point_item& p, mp2)
    { p.id = id++; }

    point_visitor visitor;
    bg::partition
        <
            bg::model::box<point_item>, get_point, ovelaps_point
        >::apply(mp1, mp2, visitor, 1);

    BOOST_CHECK_EQUAL(visitor.count, expected_count);
}

template <typename P>
void test_all()
{
    typedef bg::model::box<P> box;

    test_boxes<box>(
        // 1           2             3               4             5             6             7
        "box(0 0,1 1); box(0 0,2 2); box(9 9,10 10); box(8 8,9 9); box(4 4,6 6); box(2 4,6 8); box(7 1,8 2)",
        5, // Area(Intersection(1,2)) + A(I(5,6))
        3);

    test_boxes<box>(
        "box(0 0,10 10); box(4 4,6 6); box(3 3,7 7)",
            4 + 16 + 4,  // A(I(1,2)) + A(I(1,3)) + A(I(2,3))
            3);

    test_boxes<box>(
        "box(0 2,10 3); box(3 1,4 5); box(7 1,8 5)",
            1 + 1,  // A(I(1,2)) + A(I(1,3))
            2);

    test_points("multipoint((1 1))", "multipoint((1 1))", 1);
    test_points("multipoint((0 0),(1 1),(7 3),(10 10))", "multipoint((1 1),(2 2),(7 3))", 2);

}

//------------------- higher volumes

template <typename SvgMapper>
struct svg_visitor
{
    SvgMapper& m_mapper;

    svg_visitor(SvgMapper& mapper)
        : m_mapper(mapper)
    {}

    template <typename Box>
    inline void apply(Box const& box, int level)
    {
        /*
        std::string color("rgb(64,64,64)");
        switch(level)
        {
            case 0 : color = "rgb(255,0,0)"; break;
            case 1 : color = "rgb(0,255,0)"; break;
            case 2 : color = "rgb(0,0,255)"; break;
            case 3 : color = "rgb(255,255,0)"; break;
            case 4 : color = "rgb(255,0,255)"; break;
            case 5 : color = "rgb(0,255,255)"; break;
            case 6 : color = "rgb(255,128,0)"; break;
            case 7 : color = "rgb(0,128,255)"; break;
        }
        std::ostringstream style;
        style << "fill:none;stroke-width:" << (5.0 - level / 2.0) << ";stroke:" << color << ";";
        m_mapper.map(box, style.str());
        */
        m_mapper.map(box, "fill:none;stroke-width:2;stroke:rgb(0,0,0);");

    }
};

template <typename Collection>
void fill_points(Collection& collection, int seed, int size, int count)
{
    typedef boost::minstd_rand base_generator_type;

    base_generator_type generator(seed);

    boost::uniform_int<> random_coordinate(0, size - 1);
    boost::variate_generator<base_generator_type&, boost::uniform_int<> >
        coordinate_generator(generator, random_coordinate);

    std::set<std::pair<int, int> > included;

    int n = 0;
    for (int i = 0; n < count && i < count*count; i++)
    {
        int x = coordinate_generator();
        int y = coordinate_generator();
        std::pair<int, int> pair = std::make_pair(x, y);
        if (included.find(pair) == included.end())
        {
            included.insert(pair);
            typename boost::range_value<Collection>::type item;
            item.x = x;
            item.y = y;
            collection.push_back(item);
            n++;
        }
    }
}

void test_many_points(int seed, int size, int count)
{
    bg::model::multi_point<point_item> mp1, mp2;

    fill_points(mp1, seed, size, count);
    fill_points(mp2, seed * 2, size, count);

    // Test equality in quadratic loop
    int expected_count = 0;
    BOOST_FOREACH(point_item const& item1, mp1)
    {
        BOOST_FOREACH(point_item const& item2, mp2)
        {
            if (bg::equals(item1, item2))
            {
                expected_count++;
            }
        }
    }

#if defined(TEST_WITH_SVG)
    std::ostringstream filename;
    filename << "partition" << seed << ".svg";
    std::ofstream svg(filename.str().c_str());

    bg::svg_mapper<point_item> mapper(svg, 800, 800);

    {
        point_item p;
        p.x = -1; p.y = -1; mapper.add(p);
        p.x = size + 1; p.y = size + 1; mapper.add(p);
    }

    typedef svg_visitor<bg::svg_mapper<point_item> > box_visitor_type;
    box_visitor_type box_visitor(mapper);
#else
    typedef bg::visit_no_policy box_visitor_type;
    box_visitor_type box_visitor;
#endif

    point_visitor visitor;
    bg::partition
        <
            bg::model::box<point_item>, get_point, ovelaps_point,
            box_visitor_type
        >::apply(mp1, mp2, visitor, 2, box_visitor);

    BOOST_CHECK_EQUAL(visitor.count, expected_count);

#if defined(TEST_WITH_SVG)
    BOOST_FOREACH(point_item const& item, mp1)
    {
        mapper.map(item, "fill:rgb(255,128,0);stroke:rgb(0,0,100);stroke-width:1", 8);
    }
    BOOST_FOREACH(point_item const& item, mp2)
    {
        mapper.map(item, "fill:rgb(0,128,255);stroke:rgb(0,0,100);stroke-width:1", 4);
    }
#endif
}

template <typename Collection>
void fill_boxes(Collection& collection, int seed, int size, int count)
{
    typedef boost::minstd_rand base_generator_type;

    base_generator_type generator(seed);

    boost::uniform_int<> random_coordinate(0, size * 10 - 1);
    boost::variate_generator<base_generator_type&, boost::uniform_int<> >
        coordinate_generator(generator, random_coordinate);

    int n = 0;
    for (int i = 0; n < count && i < count*count; i++)
    {
        int w = coordinate_generator() % 30;
        int h = coordinate_generator() % 30;
        if (w > 0 && h > 0)
        {
            int x = coordinate_generator();
            int y = coordinate_generator();
            if (x + w < size * 10 && y + h < size * 10)
            {
                typename boost::range_value<Collection>::type item(n+1);
                bg::assign_values(item.box, x / 10.0, y / 10.0, (x + w) / 10.0, (y + h) / 10.0);
                collection.push_back(item);
                n++;
            }
        }
    }
}

void test_many_boxes(int seed, int size, int count)
{
    typedef bg::model::box<point_item> box_type;
    std::vector<box_item<box_type> > boxes;

    fill_boxes(boxes, seed, size, count);

    // Test equality in quadratic loop
    int expected_count = 0;
    double expected_area = 0.0;
    BOOST_FOREACH(box_item<box_type> const& item1, boxes)
    {
        BOOST_FOREACH(box_item<box_type> const& item2, boxes)
        {
            if (item1.id < item2.id)
            {
                if (bg::intersects(item1.box, item2.box))
                {
                    box_type b;
                    bg::intersection(item1.box, item2.box, b);
                    expected_area += bg::area(b);
                    expected_count++;
                }
            }
        }
    }

#if defined(TEST_WITH_SVG)
    std::ostringstream filename;
    filename << "partition_box_" << seed << ".svg";
    std::ofstream svg(filename.str().c_str());

    bg::svg_mapper<point_item> mapper(svg, 800, 800);

    {
        point_item p;
        p.x = -1; p.y = -1; mapper.add(p);
        p.x = size + 1; p.y = size + 1; mapper.add(p);
    }

    BOOST_FOREACH(box_item<box_type> const& item, boxes)
    {
        mapper.map(item.box, "opacity:0.6;fill:rgb(50,50,210);stroke:rgb(0,0,0);stroke-width:1");
    }

    typedef svg_visitor<bg::svg_mapper<point_item> > partition_visitor_type;
    partition_visitor_type partition_visitor(mapper);

    box_visitor<box_type> visitor;
    bg::partition
        <
            box_type, get_box, ovelaps_box,
            partition_visitor_type
        >::apply(boxes, visitor, 2, partition_visitor);

    BOOST_CHECK_EQUAL(visitor.count, expected_count);
    BOOST_CHECK_CLOSE(visitor.area, expected_area, 0.001);

#endif
}

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

    test_many_points(12345, 20, 40);
    test_many_points(54321, 20, 60);
    test_many_points(67890, 20, 80);
    test_many_points(98765, 20, 100);
    for (int i = 1; i < 10; i++)
    {
        test_many_points(i, 30, i * 20);
    }

    test_many_boxes(12345, 20, 40);
    for (int i = 1; i < 10; i++)
    {
        test_many_boxes(i, 20, i * 10);
    }

    return 0;
}


#include <iostream>
#include <vector>
#include <random>
#include <limits>
#include <cmath>
#include <core/core.hpp>
#include "slic.h"
#include <unordered_set>
#include <fstream>
#include "image.h"
#include "raw.h"
#include "args.h"
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <dirent.h>
#include <omp.h>
#include <atomic>
#include <sstream>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <chrono>

struct MiddleburryCalib {
	float cam0[9];
	float cam1[9];
	float doffs;
	uint32_t w, h;
	uint32_t ndisp, vmin, vmax;
	float dyavg, dymax;
	float baseline;
};

MiddleburryCalib read_calib(const char *fn)
{
	MiddleburryCalib calib;
	std::ifstream is(fn);
	while (is) {
		std::string key, val;
		std::getline(is, key, '=');
		if (key == "cam0") {
			for (int i = 0; i < 9; ++i) {
				while (is.peek() < '0' || is.peek() > '9') is.get();
				is >> calib.cam0[i];
			}
		} else if (key == "cam1") {
			for (int i = 0; i < 9; ++i) {
				while (is.peek() < '0' || is.peek() > '9') is.get();
				is >> calib.cam1[i];
			}
		} else if (key == "doffs") {
			is >> calib.doffs;
		} else if (key == "width") {
			is >> calib.w;
		} else if (key == "height") {
			is >> calib.h;
		} else if (key == "ndisp") {
			is >> calib.ndisp;
		} else if (key == "vmin") {
			is >> calib.vmin;
		} else if (key == "vmax") {
			is >> calib.vmax;
		} else if (key == "dyavg") {
			is >> calib.dyavg;
		} else if (key == "dymax") {
			is >> calib.dymax;
		} else if (key == "baseline") {
			is >> calib.baseline;
		}
		is.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
	}
	return calib;
}

struct Plane {
	float a, b,/* c,*/ d;

	bool fit(const float *p0, const float *p1, const float *p2)
	{
		float a0[3], a1[3];
		for (int i = 0; i < 3; ++i) {
			a0[i] = p2[i] - p0[i];
			a1[i] = p1[i] - p0[i];
		}

		a = a0[1] * a1[2] - a0[2] * a1[1];
		b = a0[2] * a1[0] - a0[0] * a1[2];
		float c = a0[0] * a1[1] - a0[1] * a1[0];
		d = -(a * p2[0] + b * p2[1] + c * p2[2]);

		a /= -c;
		b /= -c;
		d /= -c;

		if (std::abs(a) < std::numeric_limits<float>::epsilon() && std::abs(b) < std::numeric_limits<float>::epsilon() && std::abs(c) < std::numeric_limits<float>::epsilon() && std::abs(d) < std::numeric_limits<float>::epsilon()) return false;
		return true;
	}

	float dist(const float *p) const
	{
		return std::abs(get_val_at(p[0], p[1]) - p[2]);
	}
	float get_val_at(float x, float y) const
	{
		return /*-(*/a * x + b * y + d/*) / c*/;
	}
};

void stereo(const MiddleburryCalib &calib, const image_f &im0g, const image_f &im1g, std::vector<float> &errors)
{
	image_f im1p(calib.w, calib.h);
	errors.clear();
	errors.resize(calib.w * calib.h * calib.ndisp, std::numeric_limits<float>::infinity());

	for (uint32_t i = 0; i < calib.ndisp; ++i) {
		for (uint32_t y = 0; y < calib.h; ++y) {
			for (uint32_t x = 0; x < calib.w; ++x) {
				float xx = x - i;
				float yy = y;
				im1p.at2d(x, y) = std::numeric_limits<float>::infinity();
				if (xx < 0 || yy < 0 || xx >= calib.w || yy >= calib.h) continue;

				im1p.at2d(x, y) = im1g.at_lin(xx, yy);
			}
		}
		for (uint32_t y = 0; y < calib.h; ++y) {
			for (uint32_t x = 0; x < calib.w; ++x) {
				float a[9], b[9];
				int n = 0;
				float avg_a = 0, avg_b = 0;
				for (int yy = y == 0; yy < 3 - (y == calib.h - 1); ++yy) {
					for (int xx = x == 0; xx < 3 - (x == calib.w - 1); ++xx) {
						avg_a += a[n] = im0g.at2d(x + xx - 1, y + yy - 1);
						avg_b += b[n] = im1p.at2d(x + xx - 1, y + yy - 1);
						++n;
					}
				}
				avg_a /= n;
				avg_b /= n;

				float var_a = 0, var_b = 0;
				for (int i = 0; i < n; ++i) {
					float aa = a[i] - avg_a;
					float bb = b[i] - avg_b;
					var_a += aa * aa;
					var_b += bb * bb;
				}
				var_a = std::sqrt(var_a / n);
				var_b = std::sqrt(var_b / n);

				float ncc = 0;
				for (int i = 0; i < n; ++i) {
					float aa = (a[i] - avg_a) / var_a;
					float bb = (b[i] - avg_b) / var_b;
					ncc += aa * bb;
				}
				ncc = std::max(0.f, ncc / n);
				float err = 1 - ncc;

				errors[y * calib.w * calib.ndisp + x * calib.ndisp + i] = err;
			}
		}
	}
}

void lsolve(int w, int h, int steps, const float *scores, int *result)
{
	// compute result depth map (get the best depth for each pixel)
	for (int y = 0; y < h; ++y) {
		for (int x = 0; x < w; ++x) {
			float best = std::numeric_limits<float>::infinity();
			int beststep = -1;
			for (int s = 0; s < steps; ++s) {
				float score = scores[y * w * steps + x * steps + s];
				if (score <= best) {
					best = score;
					beststep = s;
				}
			}
			result[y * w + x] = beststep;
		}
	}
}

template <typename T>
struct CompImg {
	image<T> img;
	image<T> lossless;
	image_s mask;
	uint32_t ncomp = 0, nuncomp = 0;
	std::vector<Plane> planes;
	std::vector<float> stddevs;
    std::random_device rd;
    std::mt19937 g;
	CompImg() : g(rd())
	{}

	void alloc(uint32_t w, uint32_t h, uint32_t nc)
	{
		img = image<T>(w, h, nc);
		lossless = image<T>(w, h, nc);
		mask = image_s(w, h);
	}

	void recon(int bd)
	{
		img = lossless;
		for (uint32_t y = 0; y < img.height(); ++y) {
			for (uint32_t x = 0; x < img.width(); ++x) {
				uint16_t pid = mask.at2d(x, y);
				if (pid == 0) continue; // already copied

				float stddev = stddevs[pid];
				std::normal_distribution<float> dist(0, stddev);
				for (uint32_t c = 0; c < img.channels(); ++c) {
					float v = planes[pid * img.channels() + c].get_val_at((float)x / (img.width() - 1), (float)y / (img.height() - 1));
					img.at2d(x, y, c) = std::min(std::max(v + dist(g), 0.f), 1.f) * (1 << bd);
				}
			}
		}
	}

	void copy(uint32_t x, uint32_t y, const T *val)
	{
		for (int c = 0; c < img.channels(); ++c) {
			lossless.at2d(x, y, c) = img.at2d(x, y, c) = val[c];
		}
		++nuncomp;
		mask.at2d(x, y) = 0;
	}
	uint32_t sp = 1;
	uint32_t plane(Plane *pl, float stddev)
	{
		if (sp >= planes.size() / img.channels()) {
			planes.resize((sp + 1) * img.channels());
			stddevs.resize(sp + 1);
		}
		for (int c = 0; c < img.channels(); ++c) {
			planes[sp * img.channels() + c] = pl[c];
		}
		stddevs[sp] = stddev;
		return sp++;
	}
	void superpixel(uint32_t x, uint32_t y, uint32_t sp)
	{
		++ncomp;
		const double mean = 0.0;
		const double stddev = stddevs[sp];
		std::normal_distribution<float> dist(mean, stddev);
		for (int c = 0; c < img.channels(); ++c) {
			float v = planes[sp * img.channels() + c].get_val_at((float)x / (img.width() - 1), (float)y / (img.height() - 1));
			img.at2d(x, y, c) = std::min(std::max(v + dist(g), 0.f), 1.f) * (1 << (sizeof(T) * 8));
			lossless.at2d(x, y, c) = (1 << (sizeof(T) * 8 - 1));
		}
		mask.at2d(x, y) = sp;
	}

};

void write_contours(const char *fn, const cv::Mat &in)
{
	cv::Mat img;
	cv::erode(in, img, cv::Mat(), cv::Point(-1, -1), 5, 1, 1);

	image_b out(img.cols, img.rows, 4);
	for (uint32_t y = 0; y < out.height(); ++y) {
		for (uint32_t x = 0; x < out.width(); ++x) {
			cv::Vec3b v = img.at<cv::Vec3b>(y, x);
			if (v[0] == 255 && v[1] == 255 && v[2] == 255) {
				out.at2d(x, y, 0) = 0;
				out.at2d(x, y, 1) = 0;
				out.at2d(x, y, 2) = 0;
				out.at2d(x, y, 3) = 0;
			} else {
				out.at2d(x, y, 0) = 0;
				out.at2d(x, y, 1) = 0;
				out.at2d(x, y, 2) = 0;
				out.at2d(x, y, 3) = 255;
			}
		}
	}
	image_io::save(out, fn);
}

template <typename T>
float planecomp(double thressigma, const image<T> &im0, CompImg<T> &out, int bitdepth, int n, const image_b *mask = nullptr)
{
	uint32_t w = im0.width(), h = im0.height();

	out.alloc(w, h, im0.channels());

	cv::Mat inputMatrix(h, w, CV_8UC3, cv::Scalar(0, 0, 0));
	for (uint32_t y = 0; y < h; ++y) {
		for (uint32_t x = 0; x < w; ++x) {
			int c0, c1, c2;
			if (im0.channels() == 3) {
				c0 = im0.at2d(x, y, 0) / (1 << (bitdepth - 8));
				c1 = im0.at2d(x, y, 1) / (1 << (bitdepth - 8));
				c2 = im0.at2d(x, y, 2) / (1 << (bitdepth - 8));
			} else {
				c0 = im0.at2d(x, y, 0) / (1 << (bitdepth - 8));
				c1 = ((int)im0.at2d(x, y, 1) + (int)im0.at2d(x, y, 2)) / 2 / (1 << (bitdepth - 8));
				c2 = im0.at2d(x, y, 3) / (1 << (bitdepth - 8));
			}
			inputMatrix.at<cv::Vec3b>(y, x) = cv::Vec3b(c2, c1, c0);
		}
	}

    SLIC slic;
	slic.GenerateSuperpixels(inputMatrix, n);
	int *result = slic.GetLabel();
	n = 0;
	for (int i = 0; i < w * h; ++i) n = std::max(result[i] + 1, n);
	if (mask) ++n; // all masked pixel will end in patch n-1
	uint32_t tobedone = 0;

	for (uint32_t y = 0; y < h; ++y) {
		for (uint32_t x = 0; x < w; ++x) {
			if (mask && mask->at2d(x, y) != 0) result[y * w + x] = n - 1;
			else ++tobedone;
		}
	}

	std::vector<uint32_t> cluster_cnt(n, 0);
	for (uint32_t y = 0; y < h; ++y) {
		for (uint32_t x = 0; x < w; ++x) {
			int c = result[y * w + x];
			++cluster_cnt[c];
		}
	}
	std::vector<uint32_t> cluster_off(n, 0);
	for (int i = 1; i < n; ++i) {
		cluster_off[i] = cluster_off[i - 1] + cluster_cnt[i - 1];
	}
	cluster_off.push_back(cluster_off.back() + cluster_cnt.back());
	std::vector<std::pair<uint32_t, uint32_t>> cluster_pixels(cluster_off.back());
	std::fill(cluster_cnt.begin(), cluster_cnt.end(), 0);
	for (uint32_t y = 0; y < h; ++y) {
		for (uint32_t x = 0; x < w; ++x) {
			int c = result[y * w + x];
			cluster_pixels[cluster_off[c] + cluster_cnt[c]++] = std::make_pair(x, y);
		}
	}

	float sc = 1 << bitdepth;

	uint32_t x0, y0, x1, y1, x2, y2;
    std::random_device rd;
    std::mt19937 g(rd());
	std::vector<float> mses(n, 0);

	uint32_t done = 0;
	uint32_t nnn = n - (mask ? 1 : 0);
	uint32_t cntsucc = 0;
	for (uint32_t i = 0; i < nnn; ++i) {
		bool last = i == nnn - 1;
		uint32_t o = cluster_off[i];
		uint32_t nn = cluster_off[i + 1] - o;

		float min_peakerr[128]; for (int j = 0; j < 128; ++j) min_peakerr[j] = std::numeric_limits<float>::infinity();
		float min_avgerr[128];
		Plane min_plane[128];
		std::uniform_int_distribution<> dist(0, nn - 1);
		if (nn >= 3) {
			for (int c = 0; c < im0.channels(); ++c) {
				std::vector<T> cache(nn);
				for (int i = 0; i < nn; ++i) {
					std::tie(x0, y0) = cluster_pixels[o + i];
					cache[i] = im0.at2d(x0, y0, c);
				}

				std::vector<Eigen::Vector3d> coords;
				for (uint32_t j = 0; j < nn; ++j) {
					std::tie(x0, y0) = cluster_pixels[o + j];
					coords.emplace_back((float)x0 / (w - 1), (float)y0 / (h - 1), cache[j] / sc);
				}
				Eigen::Matrix<Eigen::Vector3d::Scalar, Eigen::Dynamic, Eigen::Dynamic> coord(3, nn);
				for (size_t i = 0; i < nn; ++i) coord.col(i) = coords[i];
				// calculate centroid
				Eigen::Vector3d centroid(coord.row(0).mean(), coord.row(1).mean(), coord.row(2).mean());
				// subtract centroid
				coord.row(0).array() -= centroid(0); coord.row(1).array() -= centroid(1); coord.row(2).array() -= centroid(2);
				auto svd = coord.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeThinV);
				Eigen::Vector3d plane_normal = svd.matrixU().rightCols<1>();
				double err = svd.singularValues()(2);

				Plane p = { -plane_normal(0) / plane_normal(2), -plane_normal(1) / plane_normal(2), plane_normal.dot(centroid) / plane_normal(2) };
				float peakerr = 0;
				double avgerr = 0;
				for (uint32_t j = 0; j < nn; ++j) {
					std::tie(x0, y0) = cluster_pixels[o + j];
					float p0[] = { (float)x0 / (w - 1), (float)y0 / (h - 1), cache[j] / sc };
					float e = std::abs(p.dist(p0));
					peakerr = std::max(peakerr, e);
					avgerr += e;
				}
				avgerr /= nn;
				if (last) std::cout << "Channel " << c << " error: " << avgerr << std::endl;
				min_plane[c] = p;
				min_avgerr[c] = avgerr;
				min_peakerr[c] = peakerr;
			}
		}
		mses[i] = 0;
		for (int j = 0; j < im0.channels(); ++j) mses[i] += min_avgerr[j];
		mses[i] /= im0.channels();
		float thres = 6.f / 255.f;

		thres = std::numeric_limits<float>::infinity();
		if (min_peakerr[0] > thres || min_peakerr[1] > thres || min_peakerr[2] > thres || mses[i] > thressigma) {
			for (uint32_t j = 0; j < nn; ++j) {
				std::tie(x0, y0) = cluster_pixels[o + j];
				out.copy(x0, y0, im0.ptr2d(x0, y0));
			}
		} else {
			uint32_t sp;
			sp = out.plane(min_plane, mses[i]);
			for (uint32_t j = 0; j < nn; ++j) {
				std::tie(x0, y0) = cluster_pixels[o + j];
				out.superpixel(x0, y0, sp);
			}
			++cntsucc;
		}
		done += nn;
	}

	// coppy masked pixels
	if (mask) {
		uint32_t o = cluster_off[n - 1];
		uint32_t nn = cluster_off[n] - o;
		for (uint32_t j = 0; j < nn; ++j) {
			std::tie(x0, y0) = cluster_pixels[o + j];
			out.copy(x0, y0, im0.ptr2d(x0, y0));
		}
	}

	return (float)cntsucc / n;
}

struct Args {
	std::vector<std::string> in;
	bool oc = false;
	bool decomp = false;
	int n = 1000;
	std::string validate;
	std::string out;
	int bitdepth = 14;
	double threshold = 1.5 / 255.f / 1.f;
	Args(int argc, const char **argv)
	{
		args::parser args(argc, argv, "Blubcompression");
		const int ARG_IN = args.add_nonopt("INPUT"); args.range(1);
		const int ARG_OC = args.add_opt('c', "only-comp", "Do only compression, no stereo");
		const int ARG_DE = args.add_opt('d', "decompress", "Decompress planepng file");
		const int ARG_VA = args.add_opt('v', "validate", "Compare lossless parts with");
		const int ARG_N  = args.add_opt('n', "superpixels", "Number of superpixels");
		const int ARG_OU = args.add_opt('o', "out", "Write output");
		const int ARG_BD = args.add_opt('b', "bitdepth", "Bitdepth for output (default 14)");
		const int ARG_TH = args.add_opt('t', "threshold", "Threshold (default 1.5 / 255.f / 1.f)");

		for (int arg = args.next(); arg != args::parser::end; arg = args.next()) {
			if (arg == ARG_IN)      in.push_back(args.val<std::string>());
			else if (arg == ARG_OC) oc = true;
			else if (arg == ARG_DE) decomp = true;
			else if (arg == ARG_N)  n = args.val<int>();
			else if (arg == ARG_OU) out = args.val<std::string>();
			else if (arg == ARG_VA) validate = args.val<std::string>();
			else if (arg == ARG_BD) bitdepth = args.val<int>();
			else if (arg == ARG_TH) threshold = args.val<double>();
		}
	}
};
long GetFileSize(const char *filename)
{
    struct stat stat_buf;
    int rc = stat(filename, &stat_buf);
    return rc == 0 ? stat_buf.st_size : -1;
}

#define BUFSIZE ((uint32_t)1024 * 1024)
uint32_t write_final(std::ostream &os, std::istream &ismask, uint32_t mask_s, std::istream &islossless, uint32_t lossless_s, const std::vector<Plane> &planes, const std::vector<float> &stddevs)
{
	uint32_t s = stddevs.size(), nc = stddevs.empty() ? 0 : planes.size() / stddevs.size();
	os.write((const char*)&s, 4);
	os.write((const char*)&nc, 4);
	os.write((const char*)planes.data(), planes.size() * sizeof(Plane));
	os.write((const char*)stddevs.data(), stddevs.size() * sizeof(float));

	os.write((const char*)&mask_s, 4);
	uint32_t c = 0;
	char buf[BUFSIZE];
	while (c < mask_s) {
		uint32_t wr = std::min(BUFSIZE, mask_s - c);
		ismask.read(buf, wr);
		os.write(buf, wr);
		c += wr;
	}
	os.write((const char*)&lossless_s, 4);
	c = 0;
	while (c < lossless_s) {
		uint32_t wr = std::min(BUFSIZE, lossless_s - c);
		islossless.read(buf, wr);
		os.write(buf, wr);
		c += wr;
	}
	os.flush();
	return os.tellp();
}
uint32_t write_final(const char *out_file, const char *mask, const char *lossless, const std::vector<Plane> &planes, const std::vector<float> &stddevs)
{
	uint32_t mask_s = GetFileSize(mask), lossless_s = GetFileSize(lossless);
	std::ofstream os(out_file);
	std::ifstream masks(mask), losslesss(lossless);
	write_final(os, masks, mask_s, losslesss, lossless_s, planes, stddevs);
}

void read_final(const char *in_file, const char *mask, const char *lossless, std::vector<Plane> &planes, std::vector<float> &stddevs)
{
	std::ifstream is(in_file);
	uint32_t s, nc;
	is.read((char*)&s, 4);
	is.read((char*)&nc, 4);
	planes.resize(s * nc);
	stddevs.resize(s);
	is.read((char*)planes.data(), planes.size() * sizeof(Plane));
	is.read((char*)stddevs.data(), stddevs.size() * sizeof(float));

	is.read((char*)&s, 4);
	uint32_t c = 0;
	char buf[BUFSIZE];
	std::ofstream osmask(mask);
	while (c < s) {
		uint32_t wr = std::min(BUFSIZE, s - c);
		is.read(buf, wr);
		osmask.write(buf, wr);
		c += wr;
	}

	is.read((char*)&s, 4);
	c = 0;
	std::ofstream oslossless(lossless);
	while (c < s) {
		uint32_t wr = std::min(BUFSIZE, s - c);
		is.read(buf, wr);
		oslossless.write(buf, wr);
		c += wr;
	}
}

void calcavgerr(const image_f &gt, const image_b &noocc, int *result) {
	float avgerr = 0;
	float avgerr_noocc = 0;
	int pxcnt = 0, noocccnt = 0;
	for (uint32_t y = 0; y < gt.height(); ++y) {
		for (uint32_t x = 0; x < gt.width(); ++x) {
			float vgt = gt.at2d(x, y);
			float v = result[x + y * gt.width()];

			float err = std::abs(v - vgt);
			if (std::isinf(err) || std::isnan(err)) continue;
			avgerr += err;
			++pxcnt;
			if (noocc.at2d(x, y) == 255) {
				avgerr_noocc += err;
				++noocccnt;
			}
		}
	}
	avgerr /= pxcnt;
	avgerr_noocc /= noocccnt;

	std::cout << "avgerr: " << avgerr << std::endl << "avgerr noocc: " << avgerr_noocc << std::endl;
}
bool isdir(const char *fn)
{
	struct stat st;
	if(stat(fn,&st) == 0)
		return st.st_mode & S_IFDIR != 0;
	return false;
}
int main(int argc, const char **argv)
{
	Args args(argc, argv);
	std::cout << "Threshold: " << args.threshold << std::endl;
	int n = args.n;

	if (args.oc) {
		std::vector<uint32_t> bytes(512, 0);
		std::vector<uint32_t> bytes_copy(512, 0);
		std::vector<float> timer(512, 0);
		std::vector<float> timerpng(512, 0);
#pragma omp parallel for
		for (int i = 0; i < args.in.size(); ++i) {
			const std::string &file = args.in[i];
			std::string prefix = std::to_string(omp_get_thread_num()) + "_";
			int bitdepth;
			image_s im0;
			im0 = load_raw(file.c_str(), bitdepth);
			std::string maskname = file.substr(0, file.find_last_of('.')) + "_mask.png";
			std::cout << "File: " << file << " mask: " << maskname << std::endl;
			image_b *mask0 = nullptr;
			image_b mask0_dta(im0.width(), im0.height(), 1);
			if (std::ifstream(maskname.c_str()).good()) {
				std::cout << "mask exists" << std::endl;
				image_b mask_big = image_io::load(maskname.c_str());
				for (uint32_t y = 0; y < mask0_dta.height(); ++y) {
					for (uint32_t x = 0; x < mask0_dta.width(); ++x) {
						mask0_dta.at2d(x, y) = (mask_big.at2d(x * 2, y * 2) == 0 || mask_big.at2d(x * 2 + 1, y * 2) == 0 || mask_big.at2d(x * 2, y * 2 + 1) == 0 || mask_big.at2d(x * 2 + 1, y * 2 + 1) == 0) ? 0 : 255;
					}
				}
				mask0 = &mask0_dta;
			}
			std::cout << "Use mask: " << !!mask0 << std::endl;
// #define DEBUG
			std::ofstream final0_png("final0.ppng");
#ifdef DEBUG
			std::ofstream mask0_png("mask0.png"), lossless0_png("lossless0.png"), copy0_png("copy0.png");
#else
			std::stringstream mask0_png, lossless0_png, copy0_png;
#endif
			auto startpng = std::chrono::high_resolution_clock::now();
			image_io::save_png(im0, copy0_png);
			auto endpng = std::chrono::high_resolution_clock::now();
			auto start = std::chrono::high_resolution_clock::now();
			CompImg<uint16_t> comp0;
			planecomp(args.threshold, im0, comp0, bitdepth, n, mask0);
			std::cout << "Finished pc" << std::endl;
			image_io::save_png(comp0.mask, mask0_png);
			image_io::save_png(comp0.lossless, lossless0_png);
			uint32_t mask_s = mask0_png.tellp(), lossless_s = lossless0_png.tellp();
#ifdef DEBUG
			mask0_png.close();
			lossless0_png.close();
			std::ifstream imask0_png("mask0.png"), ilossless0_png("lossless0.png");
			uint32_t cur = write_final(final0_png, imask0_png, mask_s, ilossless0_png, lossless_s, comp0.planes, comp0.stddevs);
#else
			uint32_t cur = write_final(final0_png, mask0_png, mask_s, lossless0_png, lossless_s, comp0.planes, comp0.stddevs);
#endif
			auto end = std::chrono::high_resolution_clock::now();
			std::chrono::duration<float> elapsed = end - start;
			float cur_sec = elapsed.count();
			std::chrono::duration<float> elapsedpng = endpng - startpng;
			float cur_secpng = elapsedpng.count();
			timer[omp_get_thread_num()] += cur_sec;
			timerpng[omp_get_thread_num()] += cur_secpng;
			uint32_t cur_copy = copy0_png.tellp();
			bytes[omp_get_thread_num()] += cur;
			bytes_copy[omp_get_thread_num()] += cur_copy;
#pragma omp critical
			{
				std::cout << "### " << file << " ### Cur rate: 1:" << (float)cur_copy / cur << " [" << cur_copy << "/" << cur << "] took " << cur_sec << " sec" << std::endl;
			}
		}
		uint32_t bytes_sum = 0;
		uint32_t bytes_copy_sum = 0;
		float timer_sum = 0, timer_sumpng = 0;
		for (int i = 0; i < omp_get_num_threads(); ++i) {
			bytes_sum += bytes[i];
			bytes_copy_sum += bytes_copy[i];
			timer_sum += timer[i];
			timer_sumpng += timerpng[i];
		}
		std::cout << bytes_sum << " bytes = " << bytes_sum / 1024.f / 1024.f << " MiB" << std::endl;
		std::cout << bytes_copy_sum << " bytes = " << bytes_copy_sum / 1024.f / 1024.f << " MiB" << std::endl;
		std::cout << "Rate: 1:" << (float)bytes_copy_sum / bytes_sum << std::endl;
		std::cout << "Time [sec]: " << timer_sum << std::endl;
		std::cout << "Time PNG [sec]: " << timer_sumpng << std::endl;
		return 0;
	}

	if (args.decomp) {
		CompImg<uint16_t> comp0;
		read_final(args.in[0].c_str(), "__tmp_mask.png", "__tmp_lossless.png", comp0.planes, comp0.stddevs);
		comp0.mask = image_io::load("__tmp_mask.png");
		comp0.lossless = image_io::load("__tmp_lossless.png");
		if (!args.validate.empty()) {
			int bitdepth;
			std::cout << "Starting validation" << std::endl;
			std::cout << "Open " << args.validate.c_str() << std::endl;
			image_s im0 = load_raw(args.validate.c_str(), bitdepth);
			std::cout << im0.width() << " " << im0.height() << std::endl;
			std::cout << comp0.lossless.width() << " " << comp0.lossless.height() << std::endl;
			for (uint32_t y = 0; y < comp0.mask.height(); ++y) {
				for (uint32_t x = 0; x < comp0.mask.width(); ++x) {
					if (comp0.mask.at2d(x, y) != 0) continue;
					for (int j = 0; j < 4; ++j) {
						if (comp0.lossless.at2d(x, y, j) != im0.at2d(x, y, j)) {
							std::cerr << "Found mismatch @" << x << " " << y << " " << j << ": " << comp0.lossless.at2d(x, y, j) << " != " << im0.at2d(x, y, j) << std::endl;
						}
					}
				}
			}
		}
		comp0.recon(args.bitdepth);
		image_io::save(comp0.img, "oc_recon.png");
		image_s bayered(comp0.img.width() * 2, comp0.img.height() * 2);
		for (uint32_t y = 0; y < comp0.img.height(); ++y) {
			for (uint32_t x = 0; x < comp0.img.width(); ++x) {
				bayered.at2d(x * 2, y * 2) = comp0.img.at2d(x, y, 0);
				bayered.at2d(x * 2 + 1, y * 2) = comp0.img.at2d(x, y, 1);
				bayered.at2d(x * 2, y * 2 + 1) = comp0.img.at2d(x, y, 2);
				bayered.at2d(x * 2 + 1, y * 2 + 1) = comp0.img.at2d(x, y, 3);
			}
		}
		image_io::save(bayered, "oc_bayer.tiff");
		return 0;
	}

	for (int i = 0; i < args.in.size(); ++i) {
		std::cout << "###########" << std::endl << args.in[i] << std::endl;
		std::string prefix = std::to_string(i) + "_";

		MiddleburryCalib calib = read_calib((args.in[i] + "/calib.txt").c_str());
		image_b im0 = image_io::load((args.in[i] + "/im0.png").c_str());
		image_b im1 = image_io::load((args.in[i] + "/im1.png").c_str());
		image_b noocc = image_io::load((args.in[i] + "/mask0nocc.png").c_str());
		image_f gt = image_io::load((args.in[i] + "/disp0GT.pfm").c_str());
		std::stringstream comp0_png, comp1_png, mask0_png, mask1_png, lossless0_png, lossless1_png, copy0_png, copy1_png;
		image_io::save_png(im0, copy0_png);
		image_io::save_png(im1, copy1_png);

		calib.vmin = 0;
		calib.vmax = calib.ndisp - 1;

		CompImg<uint8_t> comp0, comp1;
		float p0 = planecomp(args.threshold, im0, comp0, 8, n);
		float p1 = planecomp(args.threshold, im1, comp1, 8, n);
		std::cout << "Ratio success: " << (int)((p0 + p1) / 2.f * 10000 + 0.5) / 100.f << std::endl;

		image_f im0g = image_manip::grayscale(im0);
		image_f im1g = image_manip::grayscale(im1);

		image_f comp0g = image_manip::grayscale(comp0.img);
		image_f comp1g = image_manip::grayscale(comp1.img);
		image_io::save_png(comp0.img, comp0_png);
		image_io::save_png(comp1.img, comp1_png);
		image_io::save_png(comp0.mask, mask0_png);
		image_io::save_png(comp1.mask, mask1_png);

		image_io::save_png(comp0.lossless, lossless0_png);
		image_io::save_png(comp1.lossless, lossless1_png);

		std::vector<float> errors, errorscomp;
		stereo(calib, im0g, im1g, errors);
		stereo(calib, comp0g, comp1g, errorscomp);
		std::vector<int> result(im0g.width() * im0g.height()), resultcomp(im0g.width() * im0g.height());
		lsolve(im0g.width(), im0g.height(), calib.ndisp, errors.data(), result.data());
		lsolve(im0g.width(), im0g.height(), calib.ndisp, errorscomp.data(), resultcomp.data());

		std::cout << "Original:" << std::endl;
		calcavgerr(gt, noocc, result.data());
		std::cout << "Compressed:" << std::endl;
		calcavgerr(gt, noocc, resultcomp.data());

		std::stringstream final0_ss, final1_ss;
		std::ofstream final0_f, final1_f;
		std::ostream *final0_png = &final0_ss, *final1_png = &final1_ss;
		if (!args.out.empty()) {
			final0_f.open((prefix + "_0_" + args.out).c_str());
			final1_f.open((prefix + "_1_" + args.out).c_str());
			final0_png = &final0_f;
			final1_png = &final1_f;
		}
		std::cout << "save 0" << std::endl;
		uint32_t cur0 = write_final(*final0_png, mask0_png, mask0_png.tellp(), lossless0_png, lossless0_png.tellp(), comp0.planes, comp0.stddevs);
		uint32_t cur_copy0 = copy0_png.tellp();

		std::cout << "save 1" << std::endl;
		uint32_t cur1 = write_final(*final1_png, mask1_png, mask1_png.tellp(), lossless1_png, lossless1_png.tellp(), comp1.planes, comp1.stddevs);
		uint32_t cur_copy1 = copy1_png.tellp();

		std::cout << "Quality:" << std::endl;
		std::cout << "Original:" << std::endl;
		calcavgerr(gt, noocc, result.data());
		std::cout << "Compressed:" << std::endl;
		calcavgerr(gt, noocc, resultcomp.data());
		std::cout << "Size LHS:" << std::endl;
		std::cout << cur0 << " bytes = " << cur0 / 1024.f / 1024.f << " MiB" << std::endl;
		std::cout << cur_copy0 << " bytes = " << cur_copy0 / 1024.f / 1024.f << " MiB" << std::endl;
		std::cout << "Rate: 1:" << (float)cur_copy0 / cur0 << std::endl;
		std::cout << "Size RHS:" << std::endl;
		std::cout << cur1 << " bytes = " << cur1 / 1024.f / 1024.f << " MiB" << std::endl;
		std::cout << cur_copy1 << " bytes = " << cur_copy1 / 1024.f / 1024.f << " MiB" << std::endl;
		std::cout << "Rate: 1:" << (float)cur_copy1 / cur1 << std::endl << std::endl;
	}

	return 0;
}


//==============================================================================
//         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/exponential/include/functions/pow.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/sdk/meta/as_floating.hpp>

#include <nt2/include/constants/eight.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/two.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>

NT2_TEST_CASE_TPL(pow,  NT2_REAL_TYPES)
{
  using nt2::pow;
  using nt2::tag::pow_;

  typedef typename nt2::meta::call<pow_(T,T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<T>(), nt2::Inf<T>()), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Nan<T>(), nt2::Nan<T>()), nt2::Nan<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Minf<T>(), nt2::Minf<T>()), nt2::Nan<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<T>(), nt2::Minf<T>()), nt2::Zero<r_t>(), 0);
#endif
  NT2_TEST_ULP_EQUAL(pow(T(-1),T(6)), T(1), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Mone<T>(), nt2::Mone<T>()), nt2::Mone<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::One<T>(), nt2::One<T>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<T>(), nt2::Zero<T>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(T(-1),T(5)), T(-1), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<T>(), nt2::One<T>()), nt2::Zero<r_t>(), 0);
}

NT2_TEST_CASE_TPL(pow_real_int,  NT2_REAL_TYPES)
{
  using nt2::pow;
  using nt2::tag::pow_;
  using boost::simd::native;
  typedef typename nt2::meta::as_integer<T>::type          iT;
  typedef typename nt2::meta::call<pow_(T,iT)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  #ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<T>(),3), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Minf<T>(),3), nt2::Minf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Nan<T>(),3), nt2::Nan<r_t>(), 0);
  #endif

  NT2_TEST_ULP_EQUAL(pow(nt2::Two<T>(),-3), 1/nt2::Eight<r_t>(),  0);
}

NT2_TEST_CASE_TPL(pow_int,  NT2_TYPES)
{
  using nt2::pow;
  using nt2::tag::pow_;
  using boost::simd::native;
  typedef typename nt2::meta::as_integer<T>::type          iT;
  typedef typename nt2::meta::call<pow_(T,iT)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_ULP_EQUAL(pow(nt2::Mone<T>(),3), nt2::Mone<r_t>(),     0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Mone<T>(),4), nt2::One<r_t>(),      0);
  NT2_TEST_ULP_EQUAL(pow(nt2::One<T>(),3),  nt2::One<r_t>(),      0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Two<T>(),3),  nt2::Eight<r_t>(),    0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<T>(),0), nt2::One<r_t>(),      0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<T>(),3), nt2::Zero<r_t>(),     0);
}

NT2_TEST_CASE(pow_staticint)
{
  using nt2::pow;
  NT2_TEST_ULP_EQUAL(pow(4.f, boost::mpl::integral_c<unsigned, 3u>()), 64.f, 0);
  NT2_TEST_ULP_EQUAL(pow<3>(4.f), 64.f, 0);
  NT2_TEST_ULP_EQUAL(pow<-3>(4.f), 0.015625f, 0);
  NT2_TEST_ULP_EQUAL(pow<3>(4), 64, 0);
  NT2_TEST_ULP_EQUAL(pow(7.f, boost::mpl::integral_c<unsigned, 4u>()), 2401.f, 0);
}


// Copyright 2015-2018 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/accumulators/ostream.hpp>
#include <boost/histogram/accumulators/weighted_mean.hpp>
#include <boost/histogram/weight.hpp>
#include <sstream>
#include "is_close.hpp"
#include "throw_exception.hpp"
#include "utility_str.hpp"

using namespace boost::histogram;
using namespace std::literals;

int main() {
  using m_t = accumulators::weighted_mean<double>;
  m_t a;
  BOOST_TEST_EQ(a.sum_of_weights(), 0);
  BOOST_TEST_EQ(a, m_t{});

  a(weight(0.5), 1);
  a(weight(1.0), 2);
  a(weight(0.5), 3);

  BOOST_TEST_EQ(a.sum_of_weights(), 2);
  BOOST_TEST_EQ(a.sum_of_weights_squared(), 1.5);
  BOOST_TEST_EQ(a.value(), 2);
  BOOST_TEST_IS_CLOSE(a.variance(), 0.8, 1e-3);

  BOOST_TEST_EQ(str(a), "weighted_mean(2, 2, 0.8)"s);
  BOOST_TEST_EQ(str(a, 25, false), " weighted_mean(2, 2, 0.8)"s);
  BOOST_TEST_EQ(str(a, 25, true), "weighted_mean(2, 2, 0.8) "s);

  auto b = a;
  b += a; // same as feeding all samples twice

  BOOST_TEST_EQ(b.sum_of_weights(), 4);
  BOOST_TEST_EQ(b.value(), 2);
  BOOST_TEST_IS_CLOSE(b.variance(), 0.615, 1e-3);

  BOOST_TEST_EQ(m_t() += m_t(), m_t());
  BOOST_TEST_EQ(m_t(1, 2, 3, 4) += m_t(), m_t(1, 2, 3, 4));
  BOOST_TEST_EQ(m_t() += m_t(1, 2, 3, 4), m_t(1, 2, 3, 4));

  return boost::report_errors();
}


/**
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at

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

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

#include <boost/ut.hpp>

#include <mpp/arith.hpp>
#include <mpp/mat.hpp>

#include "../include/utils.hpp"

using namespace boost::ut;
using namespace boost::ut::bdd;
using namespace mpp;

namespace
{
	template<typename Mats, bool ConstructMat>
	void test_op(std::string_view test_name, const auto& op)
	{
		test(test_name) = [&, test_name]<typename Mat, typename Mat2, typename Mat3>(
							  std::tuple<std::type_identity<Mat>, std::type_identity<Mat2>, std::type_identity<Mat3>>) {
			const auto [mat, mat2, expected_mat] =
				parse_test(test_name, parse_mat<Mat>, parse_mat<Mat2>, parse_mat<Mat3>);

			const auto out = [&]() {
				if constexpr (ConstructMat)
				{
					return Mat3{ op(mat, mat2) };
				}
				else
				{
					return op(mat, mat2);
				}
			}();

			if constexpr (ConstructMat)
			{
				cmp_mat_types(out, expected_mat);
			}

			cmp_mat_to_expr_like(out, expected_mat);
		} | Mats{};
	}

	template<typename Mats, bool ConstructMat>
	void test_num_op(const std::string& test_name, const auto& op)
	{
		test(test_name) = [&, test_name]<typename Mat, typename Mat2>(
							  std::tuple<std::type_identity<Mat>, std::type_identity<Mat2>>) {
			const auto [mat, val, expected_mat] =
				parse_test(test_name, parse_mat<Mat>, parse_val<typename Mat::value_type>, parse_mat<Mat2>);

			const auto out = [&]() {
				if constexpr (ConstructMat)
				{
					return Mat2{ op(mat, val) };
				}
				else
				{
					return op(mat, val);
				}
			}();

			if constexpr (ConstructMat)
			{
				cmp_mat_types(out, expected_mat);
			}

			cmp_mat_to_expr_like(out, expected_mat);
		} | Mats{};
	}
} // namespace

int main()
{
	feature("Addition") = []() {
		test_op<join_mats<all_mats<int, 2, 3>, all_mats<int, 2, 3>, all_mats<int, 2, 3>>, false>("ariths/2x3_add.txt",
			std::plus{});
		test_op<join_mats<all_mats<int, 2, 3>, all_mats<int, 2, 3>, all_mats<int, 2, 3>>, true>("ariths/2x3_add.txt",
			std::plus{});
	};

	feature("Subtraction") = []() {
		test_op<join_mats<all_mats<int, 2, 3>, all_mats<int, 2, 3>, all_mats<int, 2, 3>>, false>(
			"ariths/2x3_subtract.txt",
			std::minus{});
		test_op<join_mats<all_mats<int, 2, 3>, all_mats<int, 2, 3>, all_mats<int, 2, 3>>, true>(
			"ariths/2x3_subtract.txt",
			std::minus{});
	};

	feature("Multiplication (matrix multiplied with scalar)") = []() {
		test_num_op<join_mats<all_mats<int, 2, 3>, all_mats<int, 2, 3>>, false>("ariths/2x3_multiply.txt",
			std::multiplies{});
		test_num_op<join_mats<all_mats<int, 2, 3>, all_mats<int, 2, 3>>, true>("ariths/2x3_multiply.txt",
			std::multiplies{});
	};

	feature("Multiplication (matrix multiplied with matrix)") = []() {
		test_op<join_mats<all_mats<int, 2, 3>, all_mats<int, 3, 1>, all_mats<int, 2, 1>>, false>(
			"ariths/2x3_3x1_multiply.txt",
			std::multiplies{});
		test_op<join_mats<all_mats<int, 2, 3>, all_mats<int, 3, 1>, all_mats<int, 2, 1>>, true>(
			"ariths/2x3_3x1_multiply.txt",
			std::multiplies{});
	};

	feature("Division (matrix divided with scalar)") = []() {
		test_num_op<join_mats<all_mats<double, 2, 3>, all_mats<double, 2, 3>>, false>("ariths/2x3_divide.txt",
			std::divides{});
		test_num_op<join_mats<all_mats<double, 2, 3>, all_mats<double, 2, 3>>, true>("ariths/2x3_divide.txt",
			std::divides{});
	};

	return 0;
}


#include "stdafx.h"
#include "BoostGraph.h"
#include <sstream>
#include <boost/algorithm/string.hpp>

using std::vector;
using std::string;
using std::stringstream;

namespace
{

const char C_INPUT_DOT_1[] = R"***(
digraph G {
1 [root=1];
2;
3;
4;
5;
1->2 [weight=2];
2->3 [weight=3];
2->4 [weight=-3];
3->1 [weight=-4];
4->5 [weight=2];
}
)***";

const char C_INPUT_TEXT_1[] = R"***(
5 5 1
1 2 2
2 3 3
2 4 -3
3 1 -4
4 5 2
)***";

const char C_OUTPUT_1[] = R"***(
0 0
2 1 2
5 2 2 3
-1 2 2 4
1 3 2 4 5
)***";

const char C_INPUT_DOT_2[] = R"***(
digraph G {
1 [root=1];
2;
3;
4;
1->2 [weight=3];
1->3 [weight=7];
2->3 [weight=2];
2->4 [weight=1];
3->2 [weight=2];
3->4 [weight=2];
4->3 [weight=2];
}
)***";

const char C_OUTPUT_2[] = R"***(
0 0
3 1 2
5 2 2 3
4 2 2 4
)***";

const char C_INPUT_DOT_3[] = R"***(
digraph G {
1 [root=1];
2;
3;
4;
1->2 [weight=3];
3->4 [weight=4];
}
)***";

const char C_OUTPUT_3[] = R"***(
0 0
3 1 2
No
No
)***";

const char C_INPUT_TEXT_4[] = R"***(
4 4 1
1 2 2
2 3 4
3 1 -9
2 4 5
)***";

const char C_INPUT_DOT_4[] = R"***(
digraph G {
1 [root=1];
2;
3;
4;
1->2 [weight=2];
2->3 [weight=4];
3->1 [weight=-9];
2->4 [weight=5];
}
)***";

const char C_OUTPUT_4[] = R"***(
No
4 2 3 1 2
)***";

// Используем split для игнорирования пробелов и переносов строк при проверке.
vector<string> SplitWords(string const& text)
{
	std::string trimmed = boost::trim_copy(text);

	vector<string> words;
	boost::split(words, trimmed, boost::is_space(), boost::token_compress_on);
	return words;
}

void RunGraphvizInputTestcase(std::string const& inputText, std::string const& expectedOutput)
{
	CBoostGraph graph;
	stringstream input(inputText);
	stringstream output;
	BOOST_CHECK(graph.ReadGraphviz(input));
	graph.RunBellmanFord(output);
	vector<string> words = SplitWords(output.str());
	vector<string> checkWords = SplitWords(expectedOutput);
	BOOST_CHECK(words == checkWords);
}

void RunTextInputTestcase(std::string const& inputText, std::string const& expectedOutput)
{
	CBoostGraph graph;
	stringstream input(inputText);
	stringstream output;
	BOOST_CHECK(graph.ReadText(input));
	graph.RunBellmanFord(output);
	vector<string> words = SplitWords(output.str());
	vector<string> checkWords = SplitWords(expectedOutput);
	BOOST_CHECK(words == checkWords);
}

}

BOOST_AUTO_TEST_SUITE(Graph_with_Bellmand_Ford_algorithm)
	BOOST_AUTO_TEST_CASE(can_load_graphviz_format)
	{
		RunGraphvizInputTestcase(C_INPUT_DOT_1, C_OUTPUT_1);
	}

	BOOST_AUTO_TEST_CASE(can_load_text_format)
	{
		RunTextInputTestcase(C_INPUT_TEXT_1, C_OUTPUT_1);
	}

	BOOST_AUTO_TEST_CASE(has_no_negative_loop_false_positives)
	{
		RunGraphvizInputTestcase(C_INPUT_DOT_2, C_OUTPUT_2);
	}

	BOOST_AUTO_TEST_CASE(prints_No_if_no_path)
	{
		RunGraphvizInputTestcase(C_INPUT_DOT_3, C_OUTPUT_3);
	}

	BOOST_AUTO_TEST_CASE(finds_negative_loop_and_prints_it_for_dot_input)
	{
		RunGraphvizInputTestcase(C_INPUT_DOT_4, C_OUTPUT_4);
	}

	BOOST_AUTO_TEST_CASE(finds_negative_loop_and_prints_it_for_text_input)
	{
		RunTextInputTestcase(C_INPUT_TEXT_4, C_OUTPUT_4);
	}
BOOST_AUTO_TEST_SUITE_END()


#include <GL/freeglut.h>
#include <iostream>
#include <vector>
#include <Eigen/Eigen>

#include "m_estimators.h"
#include "example_func_ax_plus_b_eq_y_jacobian.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

double a = 0.3;
double b = 1.0;

double a_barron = 0.3;
double b_barron = 1.0;

double a_cauchy = 0.3;
double b_cauchy = 1.0;

double a_huber = 0.3;
double b_huber = 1.0;

double barron_c = 0.25;
double barron_alpha = 1;

std::vector<std::pair<double, double>> input_data;

int main(int argc, char *argv[]){
	double y;
	for(double x = -10; x <= 10; x+= 0.01){
		example_func_ax_plus_b(y,  x,  a,  b);
		y += ((double(rand()%1000000)/1000000.0) - 0.5) * 2.0 * 0.1;
		input_data.emplace_back(x,y);
	}

	//outliers
	for(double x = -10; x <= 0; x+= 0.02){
		example_func_ax_plus_b(y,  x,  a,  b);
		y += ((double(rand()%1000000)/1000000.0) - 0.5) * 2.0 + 5;
		input_data.emplace_back(x,y);
	}
	for(double x = 0; x <= 10; x+= 0.02){
		example_func_ax_plus_b(y,  x,  a,  b);
		y += ((double(rand()%1000000)/1000000.0) - 0.5) * 2.0 - 5;
		input_data.emplace_back(x,y);
	}

	a = 0.5;
	a_barron = 0.5;
	a_cauchy = 0.5;
	a_huber = 0.5;

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("adaptive_robust_loss_function_demo");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glPointSize(4);
	glLineWidth(5);
	glColor3f(1,0,0);
	glBegin(GL_LINE_STRIP);
	double y;
	for(double x = -10; x < 10; x+=0.01){
		example_func_ax_plus_b(y,  x,  a_barron,  b_barron);
		glVertex3f(x, y, 0);
	}
	glEnd();

	glColor3f(0,1,0);
	glBegin(GL_LINE_STRIP);
	for(double x = -10; x < 10; x+=0.01){
		example_func_ax_plus_b(y,  x,  a_cauchy,  b_cauchy);
		glVertex3f(x, y, 0);
	}
	glEnd();

	glColor3f(0,0,1);
	glBegin(GL_LINE_STRIP);
	for(double x = -10; x < 10; x+=0.01){
		example_func_ax_plus_b(y,  x,  a_huber,  b_huber);
		glVertex3f(x, y, 0);
	}
	glEnd();

	glColor3f(0,0,0);
	glBegin(GL_LINE_STRIP);
	for(double x = -10; x < 10; x+=0.01){
		example_func_ax_plus_b(y,  x,  a,  b);
		glVertex3f(x, y, 0);
	}
	glEnd();

	glColor3f(0.5,0.5,0.9);
	glBegin(GL_POINTS);
	for(const auto &d:input_data){
		glVertex3f(d.first, d.second, 0);
	}
	glEnd();

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'o':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < input_data.size() ; i++){
				double delta;
				observation_equation_example_func_ax_plus_b_eq_y(delta, input_data[i].first, input_data[i].second, a, b);

				Eigen::Matrix<double, 1, 2> jacobian;
				observation_equation_example_func_ax_plus_b_eq_y_jacobian(jacobian, input_data[i].first, input_data[i].second, a, b);

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir, 0, -jacobian(0,0));
				tripletListA.emplace_back(ir, 1, -jacobian(0,1));
				tripletListP.emplace_back(ir, ir,  1);
				tripletListB.emplace_back(ir, 0,  delta);

			}

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				a += h_x[0];
				b += h_x[1];

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'b':{
			double min_sum = 1000000000.0;
			for(double alpha = -10; alpha <=2; alpha += 0.1){
				double Z_tilde = get_approximate_partition_function(-10, 10, alpha, 1, 100);
				double sum = 0;
				for(size_t i = 0; i < input_data.size() ; i++){
					double delta;
					observation_equation_example_func_ax_plus_b_eq_y(delta, input_data[i].first, input_data[i].second, a, b);
					sum += get_truncated_robust_kernel(delta, alpha, barron_c, Z_tilde);
				}
				if(sum < min_sum){
					min_sum = sum;
					barron_alpha = alpha;
				}
			}
			std::cout << "barron_alpha: " << barron_alpha << std::endl;

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < input_data.size() ; i++){
				double delta;
				observation_equation_example_func_ax_plus_b_eq_y(delta, input_data[i].first, input_data[i].second, a_barron, b_barron);

				Eigen::Matrix<double, 1, 2> jacobian;
				observation_equation_example_func_ax_plus_b_eq_y_jacobian(jacobian, input_data[i].first, input_data[i].second, a_barron, b_barron);

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir, 0, -jacobian(0,0));
				tripletListA.emplace_back(ir, 1, -jacobian(0,1));
				tripletListP.emplace_back(ir, ir, get_barron_w(delta, barron_alpha, barron_c));
				tripletListB.emplace_back(ir, 0,  delta);
			}

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				a_barron += h_x[0];
				b_barron += h_x[1];

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'c':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < input_data.size() ; i++){
				double delta;
				observation_equation_example_func_ax_plus_b_eq_y(delta, input_data[i].first, input_data[i].second, a_cauchy, b_cauchy);

				Eigen::Matrix<double, 1, 2> jacobian;
				observation_equation_example_func_ax_plus_b_eq_y_jacobian(jacobian, input_data[i].first, input_data[i].second, a_cauchy, b_cauchy);

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir, 0, -jacobian(0,0));
				tripletListA.emplace_back(ir, 1, -jacobian(0,1));
				tripletListP.emplace_back(ir, ir, get_barron_w(delta, 0, barron_c));
				tripletListB.emplace_back(ir, 0,  delta);
			}

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				a_cauchy += h_x[0];
				b_cauchy += h_x[1];

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'l':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < input_data.size() ; i++){
				double delta;
				observation_equation_example_func_ax_plus_b_eq_y(delta, input_data[i].first, input_data[i].second, a_huber, b_huber);

				Eigen::Matrix<double, 1, 2> jacobian;
				observation_equation_example_func_ax_plus_b_eq_y_jacobian(jacobian, input_data[i].first, input_data[i].second, a_huber, b_huber);

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir, 0, -jacobian(0,0));
				tripletListA.emplace_back(ir, 1, -jacobian(0,1));
				tripletListP.emplace_back(ir, ir, get_barron_w(delta, 1, barron_c));
				tripletListB.emplace_back(ir, 0,  delta);
			}

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				a_huber += h_x[0];
				b_huber += h_x[1];

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case '-':{
			barron_alpha -= 0.1;
			std::cout << "barron_alpha " << barron_alpha << std::endl;
			break;
		}
		case '=':{
			barron_alpha += 0.1;
			std::cout << "barron_alpha " << barron_alpha << std::endl;
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "'o': optimize" << std::endl;
	std::cout << "'b': optimize robust Barron" << std::endl;
	std::cout << "'c': optimize robust Cauchy" << std::endl;
	std::cout << "'l': optimize robust L1L2" << std::endl;
	std::cout << "'-': barron_alpha -= 0.1" << std::endl;
	std::cout << "'=': barron_alpha += 0.1" << std::endl;
}


/* H0 XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX */
/* H0 X                                                                            */
/* H0 X   libAtoms+QUIP: atomistic simulation library                              */
/* H0 X                                                                            */
/* H0 X   Portions of this code were written by                                    */
/* H0 X     Albert Bartok-Partay, Silvia Cereda, Gabor Csanyi, James Kermode,      */
/* H0 X     Ivan Solt, Wojciech Szlachta, Csilla Varnai, Steven Winfield.          */
/* H0 X                                                                            */
/* H0 X   Copyright 2006-2010.                                                     */
/* H0 X                                                                            */
/* H0 X   These portions of the source code are released under the GNU General     */
/* H0 X   Public License, version 2, http://www.gnu.org/copyleft/gpl.html          */
/* H0 X                                                                            */
/* H0 X   If you would like to license the source code under different terms,      */
/* H0 X   please contact Gabor Csanyi, gabor@csanyi.net                            */
/* H0 X                                                                            */
/* H0 X   Portions of this code were written by Noam Bernstein as part of          */
/* H0 X   his employment for the U.S. Government, and are not subject              */
/* H0 X   to copyright in the USA.                                                 */
/* H0 X                                                                            */
/* H0 X                                                                            */
/* H0 X   When using this software, please cite the following reference:           */
/* H0 X                                                                            */
/* H0 X   http://www.libatoms.org                                                  */
/* H0 X                                                                            */
/* H0 X  Additional contributions by                                               */
/* H0 X    Alessio Comisso, Chiara Gattinoni, and Gianpietro Moras                 */
/* H0 X                                                                            */
/* H0 XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX */

/* This file contains routines which use the Computational Geometry
   Algorithms Library (CGAL, http://www.cgal.org) to compute alpha
   shapes. It is used to determine a crack front given the set of
   crack surface atoms. */

#define _USE_MATH_DEFINES
#include <math.h>

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/algorithm.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Alpha_shape_2.h>
#include <CGAL/Arr_segment_traits_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/bounding_box.h>

#include <iostream>
#include <fstream>
#include <vector>
#include <list>

#include <boost/iterator/transform_iterator.hpp>

extern "C" {
#include "libatoms.h"
}

typedef CGAL::Exact_predicates_exact_constructions_kernel K;

typedef K::FT FT;

typedef K::Point_2  Point;
typedef K::Segment_2  Segment;

typedef CGAL::Alpha_shape_vertex_base_2<K> Vb;
typedef CGAL::Alpha_shape_face_base_2<K>  Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb> Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds> Triangulation_2;

typedef CGAL::Alpha_shape_2<Triangulation_2>  Alpha_shape_2;

typedef Alpha_shape_2::Face  Face;
typedef Alpha_shape_2::Vertex Vertex;
typedef Alpha_shape_2::Edge Edge;
typedef Alpha_shape_2::Face_handle  Face_handle;
typedef Alpha_shape_2::Vertex_handle Vertex_handle;

typedef Alpha_shape_2::Face_circulator  Face_circulator;
typedef Alpha_shape_2::Vertex_circulator  Vertex_circulator;

typedef Alpha_shape_2::Locate_type Locate_type;

typedef Alpha_shape_2::Face_iterator  Face_iterator;
typedef Alpha_shape_2::Vertex_iterator  Vertex_iterator;
typedef Alpha_shape_2::Edge_iterator  Edge_iterator;
typedef Alpha_shape_2::Edge_circulator  Edge_circulator;

typedef Alpha_shape_2::Alpha_iterator Alpha_iterator;
typedef Alpha_shape_2::Alpha_shape_edges_iterator Alpha_shape_edges_iterator;
typedef Alpha_shape_2::Alpha_shape_vertices_iterator Alpha_shape_vertices_iterator;

typedef CGAL::Arr_segment_traits_2<K> Traits_2;
typedef CGAL::Arrangement_2<Traits_2> Arrangement_2;
typedef CGAL::Arr_walk_along_line_point_location<Arrangement_2> Walk_pl;

std::map<Point,int>::key_type get_key(std::map<Point,int>::value_type aPair) {
  return aPair.first;
}

typedef std::map<Point,int>::key_type (*get_key_t)(std::map<Point,int>::value_type);
typedef std::map<Point,int>::iterator map_iterator;
typedef boost::transform_iterator<get_key_t, map_iterator> mapkey_iterator;

extern "C" void c_alpha_shape_2(int *n, double x[], double y[],
                                double *alpha, int *shape_n, int shape_list[],
                                int *error)
{
  INIT_ERROR;

  // Map from points to original indices in input data
  std::map<Point,int> point_map;

  for (int i=0; i<*n; i++) {
    point_map[Point(x[i],y[i])] = i;
  }

  mapkey_iterator points_begin(point_map.begin(), get_key);
  mapkey_iterator points_end(point_map.end(), get_key);

  Alpha_shape_2 A(points_begin, points_end, FT(*alpha), Alpha_shape_2::GENERAL);

  Arrangement_2 arr;

  // Traverse alpha shape edges and add to arrangement
  for(Alpha_shape_edges_iterator it =  A.alpha_shape_edges_begin();
      it != A.alpha_shape_edges_end();
      ++it){
    CGAL::insert(arr, A.segment(*it));
  }

  // Check we have one bounded face
  if (arr.number_of_faces() != 2) {
    RAISE_ERROR("c_alpha_shape_2: number of bounded faces != 1, try increasing alpha");
  }

  Arrangement_2::Face_const_iterator fi = arr.faces_begin();
  ++fi; // skip over the unbounded face which all arrangements have

  Arrangement_2::Ccb_halfedge_const_circulator ccb = fi->outer_ccb();

  // Check shape_list is big enough to store alpha shape
  int tmp_shape_length = 0;
  do {
    tmp_shape_length++;
    ++ccb;
  } while (ccb != fi->outer_ccb());
  if (tmp_shape_length > *shape_n) {
    RAISE_ERROR("c_alpha_shape_2: size of alpha shape exceeds length of shape_list");
  }

  // traverse outer CCB again, adding results to shape_list
  ccb = fi->outer_ccb();
  *shape_n = 0;
  do {
    shape_list[(*shape_n)++] = point_map[ccb->source()->point()];
    ++ccb;
  } while (ccb != fi->outer_ccb());
}

extern "C" void c_crack_front_alpha_shape(int *n, double x[], double y[],
                                          double *alpha, double *angle_threshold,
                                          int *front_n, int front_list[], int *error)
{
  INIT_ERROR;

  // Map from points to original indices in input data
  std::map<Point,int> point_map;

  for (int i=0; i<*n; i++) {
    point_map[Point(x[i],y[i])] = i;
  }

  mapkey_iterator points_begin(point_map.begin(), get_key);
  mapkey_iterator points_end(point_map.end(), get_key);

  Alpha_shape_2 A(points_begin, points_end, FT(*alpha), Alpha_shape_2::GENERAL);

  Arrangement_2 arr;

  // Traverse alpha shape edges and add to arrangement
  for(Alpha_shape_edges_iterator it =  A.alpha_shape_edges_begin();
      it != A.alpha_shape_edges_end();
      ++it){
    CGAL::insert(arr, A.segment(*it));
  }

  // Check we have one bounded face
  if (arr.number_of_faces() != 2) {
    RAISE_ERROR("c_crack_front_alpha_shape: number of bounded faces != 1, try increasing alpha");
  }

  // Compute bounding box of original point set and use it
  // to perform a vertical ray shooting query
  K::Iso_rectangle_2 bbox = bounding_box(points_begin, points_end);
  Point p = Point(bbox.xmax(), bbox.ymin());
  Walk_pl walk_pl(arr);
  CGAL::Object    obj = walk_pl.ray_shoot_up (p);
  Arrangement_2::Vertex_const_handle xmax_vertex;
  if (!CGAL::assign (xmax_vertex, obj)) {
    RAISE_ERROR("c_crack_front_alpha_shape: vertical line search did not hit a vertex");
  }

  Arrangement_2::Face_const_iterator fi = arr.faces_begin();
  ++fi; // skip over the unbounded face which all arrangements have

  // find half-line with source at xmax_vertex
  Arrangement_2::Ccb_halfedge_const_circulator curr, xmax_edge;
  for(xmax_edge = fi->outer_ccb(); xmax_edge->source() != xmax_vertex; ++xmax_edge);

  double bearing;
  Point p1, p2;
  std::list<int> front;

  // start at xmax_edge and traverse outer CCB until angle along line
  // exceeds angle_threshold
  curr = xmax_edge;
  do {
    p1 = curr->source()->point();
    p2 = curr->target()->point();

    bearing = 180./M_PI*std::atan2(CGAL::to_double(p2.y())-CGAL::to_double(p1.y()),
                                   CGAL::to_double(p2.x())-CGAL::to_double(p1.x()));

    front.push_back(point_map[curr->source()->point()]);
  } while (++curr != xmax_edge && bearing < *angle_threshold);

  // now go back to xmax_edge, and go around backwards until
  // angle is less than -angle_theshold
  curr = xmax_edge;
  --curr; // don't want xmax_edge vertex to appear twice in output list
  do {
    p1 = curr->twin()->source()->point();
    p2 = curr->twin()->target()->point();

    bearing = 180./M_PI*std::atan2(CGAL::to_double(p2.y())-CGAL::to_double(p1.y()),
                                   CGAL::to_double(p2.x())-CGAL::to_double(p1.x()));

    front.push_front(point_map[curr->source()->point()]);
  } while (--curr != xmax_edge && bearing > -*angle_threshold);

  // Copy output into array front_list
  if (front.size() > *front_n) {
    RAISE_ERROR("c_crack_front_alpha_shape: Not enough space in front_list");
  }
  *front_n=0;
  for (std::list<int>::iterator it = front.begin(); it != front.end(); ++it) {
    front_list[(*front_n)++] = *it;
  }
}

#ifdef MAIN_PROGRAM
int main()
{
  FILE *stream = fopen("pos_xz","r");
  char buffer[256];
  int i,n;

  fgets(buffer,256,stream);
  sscanf(buffer,"%d",&n);

  double *x = new double[n];
  double *y = new double[n];

  for (i=0; i<n; i++) {
    fgets(buffer,256,stream);
    sscanf(buffer,"%lf %lf",&(x[i]),&(y[i]));
  }
  fclose(stream);

  double alpha = 100.0;
  double angle_threshold = 160.0;

  int front_n = 100;
  int *front_list = new int[front_n];

  int error = 0;
  crack_front_alpha_shape(&n, x, y, &alpha, &angle_threshold, &front_n, front_list, &error);

  if (error == 0) {
    for (i = 0; i < front_n; i++)
      std::cout << front_list[i] << std::endl;
  }
}
#endif