#include <ros/ros.h>
#include <exception>
#include "doogie_drivers/ir_sensor_driver.hpp"

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>

using namespace boost::accumulators;

class Calibration {
 public:
  Calibration() { ir_sensor_driver.init(); }
  void start() {
    ROS_INFO("Calibration has started!");
    timer_ = nh_.createTimer(ros::Duration(0.1), &Calibration::timerCallback, this);
    ros::spin();
  }

 private:
  ros::NodeHandle nh_;
  ros::Timer timer_;
  accumulator_set<float, stats<tag::mean>> left_front_acc_;
  accumulator_set<float, stats<tag::mean>> left_acc_;
  accumulator_set<float, stats<tag::mean>> right_front_acc_;
  accumulator_set<float, stats<tag::mean>> right_acc_;
  doogie_drivers::IRSensorDriver ir_sensor_driver;
  int num_samples = 0;

  void showResults() {
    ROS_INFO("----------------------------");
    ROS_INFO("Channel 0 mean value = %.2f", mean(left_front_acc_));
    ROS_INFO("Channel 1 mean value = %.2f", mean(left_acc_));
    ROS_INFO("Channel 2 mean value = %.2f", mean(right_front_acc_));
    ROS_INFO("Channel 3 mean value = %.2f", mean(right_acc_));
    ROS_INFO("----------------------------");
  }

  void timerCallback(const ros::TimerEvent& time) {
    int max_num_samples = 10;
    left_front_acc_(ir_sensor_driver.computeDistance(doogie_drivers::LEFT_FRONT, true));
    left_acc_(ir_sensor_driver.computeDistance(doogie_drivers::LEFT, true));
    right_front_acc_(ir_sensor_driver.computeDistance(doogie_drivers::RIGHT_FRONT, true));
    right_acc_(ir_sensor_driver.computeDistance(doogie_drivers::RIGHT, true));

    num_samples++;
    if (num_samples > max_num_samples) {
      this->showResults();
      ros::shutdown();
    }

    ROS_INFO("Sample %d/%d", num_samples, max_num_samples);
  }
};

int main(int argc, char **argv) {
  ros::init(argc, argv, "calibration_node");

  Calibration calibration;

  try {
    calibration.start();
  } catch (std::runtime_error error) {
    ROS_FATAL("%s", error.what());
    ros::shutdown();
  }

  return 0;
}


/**
 * @addtogroup DFNs
 * @{
 */

#ifndef POSEESTIMATOR_PRIMITIVESPOSEESTIMATOR_HPP
#define POSEESTIMATOR_PRIMITIVESPOSEESTIMATOR_HPP

#include "PoseEstimatorInterface.hpp"

#include <Helpers/ParametersListHelper.hpp>
#include <opencv2/video/tracking.hpp>
#include <Eigen/Dense>

namespace CDFF
{
namespace DFN
{
namespace PoseEstimator
{
    /**
	 * Estimate the pose of a robot element given the primitives found in the image
	 *
	 */
    class PrimitivesPoseEstimator : public PoseEstimatorInterface
    {
        public:

            PrimitivesPoseEstimator();
            virtual ~PrimitivesPoseEstimator();

            virtual void configure();
            virtual void process();

        private:

            struct PrimitivesPoseEstimatorOptionsSet
            {
                int numFrames; //Number of frames that is waited before estimating a pose to wait for a more stable input
                double maxDistance; //Max distance between positions allowed to update the Kalman Filter
            };

            Helpers::ParametersListHelper parametersHelper;
            PrimitivesPoseEstimatorOptionsSet parameters;
            static const PrimitivesPoseEstimatorOptionsSet DEFAULT_PARAMETERS;

            void SortPrimitives();
            void InitializeKalman(cv::Point measPt);
            cv::Point PredictPosition();
            Eigen::Quaternionf ExtractOrientation(const cv::Mat& inputImage, cv::Point centerWheelPx, const cv::Mat& inputDepth);
            cv::Vec3f get3DCoordinates(cv::Point point);

            std::vector<std::vector<double>> m_primitives;
            std::unique_ptr<cv::KalmanFilter> m_kalman_filter;

            cv::Point m_last_point;
            int m_count;
            double m_last_wheel_x;

    };
}
}
}

#endif // POSEESTIMATOR_PRIMITIVESPOSEESTIMATOR_HPP

/** @} */


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE order_book_tests
#include <boost/test/unit_test.hpp>
#include "order_book.h"

BOOST_AUTO_TEST_CASE( test_buy_ordering )
{
	ae::order_book book;

    book.insert(ae::order("A", 100, 10));
    book.insert(ae::order("A", 100, 7));
    book.insert(ae::order("A", 100, 11));
    book.insert(ae::order("A", 100, 9));

    BOOST_REQUIRE_EQUAL(4, book.buy_orders().size());
    BOOST_REQUIRE_EQUAL(0, book.sell_orders().size());

    BOOST_REQUIRE_EQUAL(11, book.buy_orders().top().price());
    book.buy_orders().pop();

    BOOST_REQUIRE_EQUAL(10, book.buy_orders().top().price());
    book.buy_orders().pop();

    BOOST_REQUIRE_EQUAL(9, book.buy_orders().top().price());
    book.buy_orders().pop();

    BOOST_REQUIRE_EQUAL(7, book.buy_orders().top().price());
    book.buy_orders().pop();
}

BOOST_AUTO_TEST_CASE( test_buy_ordering_same_price )
{
	ae::order_book book;

    book.insert(ae::order("A", 100, 10));
    book.insert(ae::order("A", 101, 10));
    book.insert(ae::order("A", 102, 10));
    book.insert(ae::order("A", 103, 10));

    BOOST_REQUIRE_EQUAL(4, book.buy_orders().size());
    BOOST_REQUIRE_EQUAL(0, book.sell_orders().size());

    BOOST_REQUIRE_EQUAL(100, book.buy_orders().top().quantity());
    book.buy_orders().pop();

    BOOST_REQUIRE_EQUAL(101, book.buy_orders().top().quantity());
    book.buy_orders().pop();

    BOOST_REQUIRE_EQUAL(102, book.buy_orders().top().quantity());
    book.buy_orders().pop();

    BOOST_REQUIRE_EQUAL(103, book.buy_orders().top().quantity());
    book.buy_orders().pop();
}

BOOST_AUTO_TEST_CASE( test_sell_ordering )
{
	ae::order_book book;

    book.insert(ae::order("A", -100, 10));
    book.insert(ae::order("A", -100, 7));
    book.insert(ae::order("A", -100, 11));
    book.insert(ae::order("A", -100, 9));

    BOOST_REQUIRE_EQUAL(0, book.buy_orders().size());
    BOOST_REQUIRE_EQUAL(4, book.sell_orders().size());

    BOOST_REQUIRE_EQUAL(7, book.sell_orders().top().price());
    book.sell_orders().pop();

    BOOST_REQUIRE_EQUAL(9, book.sell_orders().top().price());
    book.sell_orders().pop();

    BOOST_REQUIRE_EQUAL(10, book.sell_orders().top().price());
    book.sell_orders().pop();

    BOOST_REQUIRE_EQUAL(11, book.sell_orders().top().price());
    book.sell_orders().pop();
}

BOOST_AUTO_TEST_CASE( test_sell_ordering_same_price )
{
	ae::order_book book;

    book.insert(ae::order("A", -100, 10));
    book.insert(ae::order("A", -101, 10));
    book.insert(ae::order("A", -102, 10));
    book.insert(ae::order("A", -103, 10));

    BOOST_REQUIRE_EQUAL(0, book.buy_orders().size());
    BOOST_REQUIRE_EQUAL(4, book.sell_orders().size());

    BOOST_REQUIRE_EQUAL(100, book.sell_orders().top().quantity());
    book.sell_orders().pop();

    BOOST_REQUIRE_EQUAL(101, book.sell_orders().top().quantity());
    book.sell_orders().pop();

    BOOST_REQUIRE_EQUAL(102, book.sell_orders().top().quantity());
    book.sell_orders().pop();

    BOOST_REQUIRE_EQUAL(103, book.sell_orders().top().quantity());
    book.sell_orders().pop();
}

BOOST_AUTO_TEST_CASE( test_first_price_entered_selected )
{
	ae::order_book book;

	book.insert(ae::order("B", -100, 1.11));
	book.insert(ae::order("F", -50, 1.1));
	book.insert(ae::order("D", 100, 1.11));

	auto trades = book.match();

	BOOST_REQUIRE_EQUAL(2, trades.size());

	auto trade = trades[0];

	BOOST_REQUIRE_EQUAL(1.1, trade.match_price());
	BOOST_REQUIRE_EQUAL(50, trade.match_quantity());

	trade = trades[1];

	BOOST_REQUIRE_EQUAL(1.11, trade.match_price());
	BOOST_REQUIRE_EQUAL(50, trade.match_quantity());
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/equal.hpp>
#include <boost/hana/length.hpp>
#include <boost/hana/maximum.hpp>
#include <boost/hana/ordering.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

int main() {
    static_assert(
        hana::maximum.by(hana::ordering(hana::length), hana::make_tuple(
            hana::make_tuple(),
            hana::make_tuple(1, '2'),
            hana::make_tuple(3.3, nullptr, 4)
        ))
        == hana::make_tuple(3.3, nullptr, 4)
    , "");
}


#include "geometric/bounding_box.hpp"

#include <algorithm>
#include <cmath>       // for atan, exp
#include <type_traits> // for move, swap

#include <boost/math/constants/constants.hpp>

namespace nepomuk
{
namespace geometric
{

WGS84BoundingBox::WGS84BoundingBox(WGS84Coordinate lower_left, WGS84Coordinate upper_right)
    : lower_left(std::move(lower_left)), upper_right(std::move(upper_right))
{
}

WGS84BoundingBox::WGS84BoundingBox(std::uint32_t const horizontal,
                                   std::uint32_t const vertical,
                                   std::uint32_t const zoom_level,
                                   double const tile_size,
                                   std::int32_t const mercator_buffer)
{
    double min_lon = horizontal * tile_size - mercator_buffer;
    double max_lon = (horizontal + 1.0) * tile_size + mercator_buffer;

    double min_lat = (vertical + 1.0) * tile_size + mercator_buffer;
    double max_lat = vertical * tile_size - mercator_buffer;

    // 2^z * TILE_SIZE
    const double shift = (1u << static_cast<unsigned>(zoom_level)) * tile_size;

    auto const shift_lon = [shift](double const lon) {
        return (lon - 0.5 * shift) / shift * 360.0;
    };

    auto const shift_lat = [shift](double const lat) {
        auto const tmp =
            boost::math::constants::pi<double>() * constants::rad_to_degree * (1 - 2 * lat / shift);

        auto const clamped = std::max<long double>(-180., std::min<long double>(180., tmp));
        auto const normalized =
            constants::rad_to_degree * 2. * std::atan(std::exp(clamped * constants::degree_to_rad));

        return normalized - 90.;
    };

    min_lon = shift_lon(min_lon);
    max_lon = shift_lon(max_lon);
    min_lat = shift_lat(min_lat);
    max_lat = shift_lat(max_lat);

    lower_left = WGS84Coordinate(makeLatLonFromDouble<FixedLongitude>(min_lon),
                                 makeLatLonFromDouble<FixedLatitude>(min_lat));
    upper_right = WGS84Coordinate(makeLatLonFromDouble<FixedLongitude>(max_lon),
                                  makeLatLonFromDouble<FixedLatitude>(max_lat));
}

bool WGS84BoundingBox::contains(WGS84Coordinate const &coordinate) const
{
    return lower_left.longitude <= coordinate.longitude &&
           coordinate.longitude <= upper_right.longitude &&
           lower_left.latitude <= coordinate.latitude &&
           coordinate.latitude <= upper_right.latitude;
}

double WGS84BoundingBox::width() const
{
    return doubleFromLatLon(upper_right.longitude) - doubleFromLatLon(lower_left.longitude);
}

double WGS84BoundingBox::height() const
{
    return doubleFromLatLon(upper_right.latitude) - doubleFromLatLon(lower_left.latitude);
}

MercatorBoundingBox::MercatorBoundingBox(MercatorCoordinate lower_left_,
                                         MercatorCoordinate upper_right_)
    : lower_left(lower_left_), upper_right(upper_right_)
{
    // mercator transformations switch invert latitude (counting from upper right). If the
    // coordinates don't match up (e.g. due to conversion issues) we need to swap them to the
    // correct order
    if (lower_left.latitude > upper_right.latitude)
        std::swap(lower_left.latitude, upper_right.latitude);
}

MercatorBoundingBox::MercatorBoundingBox(std::uint32_t const horizontal,
                                         std::uint32_t const vertical,
                                         std::uint32_t const zoom_level,
                                         double const tile_size,
                                         std::int32_t const mercator_buffer)
{
    WGS84BoundingBox bbox(horizontal, vertical, zoom_level, tile_size, mercator_buffer);
    lower_left = MercatorCoordinate(bbox.lower_left);
    upper_right = MercatorCoordinate(bbox.upper_right);
    std::swap(lower_left.latitude, upper_right.latitude);
}

bool MercatorBoundingBox::contains(MercatorCoordinate const &coordinate) const
{
    return lower_left.longitude <= coordinate.longitude &&
           coordinate.longitude <= upper_right.longitude &&
           lower_left.latitude <= coordinate.latitude &&
           coordinate.latitude <= upper_right.latitude;
}

double MercatorBoundingBox::width() const
{
    return doubleFromLatLon(upper_right.longitude) - doubleFromLatLon(lower_left.longitude);
}
double MercatorBoundingBox::height() const
{
    return doubleFromLatLon(upper_right.latitude) - doubleFromLatLon(lower_left.latitude);
}

} // namespace geometric
} // namespace nepomuk


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

#include <iostream>
#include <iomanip>

#include <mpo19/clustering.hpp>
#include <mpo19/typedefs.hpp>
#include <mpo19/point.hpp>
#include <mpo19/linkage.hpp>
#include <mpo19/python.hpp>
#include <boost/hana.hpp>
#include <numeric>
#include <utility>
#include <iostream>
#include <iomanip>
#include <random>
#include <unordered_set>
#include <sstream>

#define BOOST_TEST_MODULE clustering_test
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

using namespace mpo19;
namespace bdata = boost::unit_test::data;

constexpr int m = 1'000'000;

BOOST_DATA_TEST_CASE(clustering_test
                     , bdata::random(0, m) ^ bdata::xrange(10)
                     , seed, idx)
{
    (void) idx;
    constexpr size_t n = 101;
    size_t t = 1;
    std::mt19937 mt(seed);
    std::uniform_int_distribution<int> distr{1, m};

    static_assert(n*(n-1) < m, "");

    std::unordered_set<size_t> randoms;

    while(randoms.size() < n*(n-1)/2) {
        randoms.insert(distr(mt));
    }

    auto rand_it = randoms.begin();
    table_t<size_t> precomputed{boost::extents[n][n]};
    for(index_t i = 0; i != index_t(n); ++i) {
        for(index_t j = 0; j != index_t(n); ++j) {
            if(i > j) continue;
            if(i == j) {
                precomputed[i][j] = 0;
            } else {
                precomputed[i][j] = *rand_it;
                precomputed[j][i] = precomputed[i][j];
                ++rand_it;
            }
        }
    }

    std::stringstream ss;

    auto python_dendrogram = python{}.hierarchical_clustering(precomputed, t, "single");
    for(auto const& c : python_dendrogram) {
        ss << c;
    }
    std::string python_dendrogram_str = ss.str();
    ss.str(std::string{});

    hierarchical_clustering<opt_min_linkage<table_t<size_t>>> hc{precomputed};
    auto hc_dendrogram = hc(t);
    for(auto const& c : hc_dendrogram) {
        ss << c;
    }
    std::string hc_dendrogram_str = ss.str();
    ss.str(std::string{});

    BOOST_TEST(python_dendrogram_str == hc_dendrogram_str);

}


#include <boost/ut.hpp>// single header
// import boost.ut;        // single module (C++20)
#include "open_viii/compression/L4Z.hpp"
#include "open_viii/compression/LZSS.hpp"
#include "tl/random.hpp"
#include "tl/utility.hpp"
int
  main()
{
  using namespace boost::ut::literals;
  using namespace boost::ut::operators::terse;
  using namespace boost::ut;
  static constexpr tl::utility::sequence<1U, 100U> run{};
  [[maybe_unused]] suite                           tests = [] {
    "lzss compress and uncompress"_test = []() {
      const auto run_once = [&]<size_t i>() {
        const auto                  random_char = tl::random::iota<char, i>();
        [[maybe_unused]] const auto compressed_random_char =
          open_viii::compression::LZSS::compress(random_char);
        [[maybe_unused]] const auto decompressed_random_char =
          open_viii::compression::LZSS::decompress(compressed_random_char);
        expect(std::ranges::equal(random_char, decompressed_random_char));
      };
      run(run_once);
    };
    "lz4 compress and uncompress"_test = []() {
      const auto run_once = [&]<size_t i2>() {
        const auto                  random_char = tl::random::iota<char, i2>();
        [[maybe_unused]] const auto compressed_random_char =
          open_viii::compression::l4z::compress(random_char);
        [[maybe_unused]] const auto decompressed_random_char =
          open_viii::compression::l4z::decompress(compressed_random_char, i2);
        expect(std::ranges::equal(random_char, decompressed_random_char));
      };
      run(run_once);
    };
  };
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. 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/multiprecision/cpp_int.hpp>

#ifndef BOOST_NO_CXX11_CONSTEXPR

template <class T>
void silence_unused(const T&){}

template <class T>
void test1()
{
   constexpr T i1 = 2u;
   constexpr T i2;
   constexpr T i3 = -3;
   constexpr T i4(i1);

   silence_unused(i1);
   silence_unused(i2);
   silence_unused(i3);
   silence_unused(i4);
}
template <class T>
void test2()
{
   constexpr T i1 = 2u;
   constexpr T i2;
   constexpr T i3 = -3;

   silence_unused(i1);
   silence_unused(i2);
   silence_unused(i3);
}
template <class T>
void test3()
{
   constexpr T i1 = 2u;
   constexpr T i2;

   silence_unused(i1);
   silence_unused(i2);
}

using namespace boost::multiprecision;

template void test1<number<cpp_int_backend<64, 64, unsigned_magnitude, unchecked, void>, et_off> >();
template void test1<number<cpp_int_backend<64, 64, signed_magnitude, unchecked, void>, et_off> >();
template void test3<number<cpp_int_backend<2048, 2048, unsigned_magnitude, unchecked, void>, et_off> >();
template void test2<number<cpp_int_backend<2048, 2048, signed_magnitude, unchecked, void>, et_off> >();

#endif


#include <boost/geometry/strategies/cartesian/distance_pythagoras_box_box.hpp>


#include <GAGPL/SPECTRUM/PseudoclusterFinder.h>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
#include <utility>

int main(int argc, char *argv[])
{
	// 1. Read test data.
	std::ifstream infile (argv[1]);

	std::vector<gag::Peak> peaks;

	//vector< iterator_range<string::iterator> > vec;

	std::string line;

	if(infile.is_open())
	{
		while(std::getline(infile, line))
		{
			//std::vector<std::string> peak;
			//boost::split(peak, line, boost::is_any_of("\t"));
			std::istringstream is;
			is.str(line);
			double k1; float k2;
			is >> k1 >> k2;
			gag::Peak pk(k1, k2);
			peaks.push_back(pk);
		}
	}
	infile.close();

	gag::PseudoclusterFinder pf;
	gag::Param& param = gag::Param::instance();
	param.setParameter("max_charge",4);
	param.setParameter("min_charge",1);
	param.setParameter<double>("tolerance",5e-6);
	param.setParameter<double>("A2tolerance", 1e-5);
	param.setParameter("min_num_peaks",2);
	param.setParameter("max_continuous_peaks",2);
	param.setParameter("gap_distance",1.1);

	std::map<std::string, gag::PseudoclusterList> cluster;
	pf.run(peaks,cluster);
	std::cout << "Cluster_ID\tPeak_ID\tCharge\tMax\tMonoMass" << std::endl;
	std::map<std::string, gag::PseudoclusterList>::iterator it;
	for(it=cluster.begin(); it !=cluster.end(); it++)
	{
	//	//std::cout << "Charge: " << (*it).first << std::endl;
		gag::PseudoclusterList& pl = (*it).second;
		for(std::vector<gag::Pseudocluster>::iterator ip = pl.begin(); ip != pl.end(); ip++)
		{
			std::cout << (*ip).getClusterID() << "\t" << (*ip).getHeadID() << "\t" << (*ip).getChargeState()
				<< "\t" << (*ip).getMaxPeaks() << "\t" << (*ip).getPeakList().at(0).getMZ() << std::endl;

			//gag::IsopeakList& xy = (*ip).getPeakList();
	//		for(size_t xx=0; xx < xy.size(); xx++)
	//		{
	//			std::cout << xy[xx].getMZ() << " ";
	//		}
	//		std::cout << std::endl;
	//		//std::cout << (*ip).getChargeState() << "\t" << xy[0].getMZ() << std::endl;
		}
	}

	return 0;
}


#include "IntrinsicModel.h"
#include <iostream>
#include <fstream>
#include "Utility.h"
#include "SparseMat.h"
#include <cstdio>
#include <boost/filesystem.hpp>

float IMConfig::absLambda = 1000;
float IMConfig::albedoLambda  = 1;
float IMConfig::shadeLambda = 1;
float IMConfig::windowSize = 3;
float IMConfig::chromThrd = 0.005;
float IMConfig::distThrd = 0.0005;
float IMConfig::txVar = 0.1;
float IMConfig::rWeight = 100;
bool IMConfig::useEqCons = true;

ublas_cm_f constructShadeSparseMat(Constraint& cons,int w, int h )
{
	int nzCnt = 0;
	for (int i = 0; i < cons.size(); i++)
	{
		GrpEle& ge = cons[i];
		int geSize = ge.m_eMember.size();
		nzCnt += geSize * 4;
	}
	std::cout << "image size:" << w << "," << h << std::endl;
	float fSz = (float)w * (float)h * (float)w * (float)h;
	std::cout << "matrix size: :" << fSz << ",nzcnt:" << nzCnt << ",sparsity:" << (float)nzCnt/fSz << std::endl;
	return ublas_cm_f(h * w,h * w,nzCnt);
}

ublas_cm_f constructAlbedoSparseMat(Constraint& cons,int w ,int h){
	//
	return constructShadeSparseMat(cons,w,h);
}
ublas_cm_f constructRetinexSparseMat(int w ,int h){
	//
	int nzCnt = w * h * 8;
	return ublas_cm_f(w * h , w * h, nzCnt);
}

IntrinsicModel::IntrinsicModel(mat_3f img,std::string modelName)
:m_img(img.clone())
,m_modelName(modelName)
,m_shadeScrib(NULL,NULL)
,m_albedoScrib(NULL,NULL)
,m_absShadeScrib(NULL,NULL)
,m_eqCons(IMConfig::useEqCons),m_isCS(false),m_isCR(false),m_isAbsS(false),m_cpnInited(false)
,m_csLambda(IMConfig::shadeLambda),m_crLambda(IMConfig::albedoLambda),m_absLambda(IMConfig::absLambda)
,m_sparsity(0)
,m_matBuild(false)
,m_lWinSize(3)
,m_matrixTime(0){

	for (int i = 0; i < m_img.size().height; i++)
	{
		for (int j = 0; j <m_img.size().width; j++)
		{
			m_img(i,j)[0] += 1e-5;
			m_img(i,j)[1] += 1e-5;
			m_img(i,j)[2] += 1e-5;

		}
	}
	m_csInited = false;
	m_crInited = false;
	m_absInited = false;
	m_dm.init(m_img); //intialize the data manager
	m_ROIMask = mat_u(m_img.size());
	m_ROIMask = 255; //set all front pixels
	m_featureMask = mat_u(m_img.size());
	m_featureMask = 255;
	int iw = m_img.size().width;
	int ih = m_img.size().height;
	m_pixNorm = mat_3f(ih,iw);
	m_pixNorm = cv::Vec3f(0,0,0); //initialize as zero vectors
	m_grayImg = mat_f(m_img.size());
	m_logImg = mat_f(m_img.size());
	for (int i = 0; i < ih; i++)
	{
		for (int j = 0; j < iw; j++)
		{
			m_grayImg(i,j) = cv::norm(m_img(i,j)); //
			m_logImg(i,j) = log(m_grayImg(i,j));
		}
	}
	//init the linear equation matrix
	int matSz = m_img.size().width * m_img.size().height;
	m_allConsMat = ublas_cm_f(matSz,matSz);
	m_allConsVec = ublas_vec_f(matSz);
	for(int i = 0; i <matSz; i++)
		m_allConsVec(i) = 0;

}
void setMask(mat_u dstMsk, mat_3f srcMsk = mat_u(0,0)){
	cv::Size mskSz = srcMsk.size();
	dstMsk = 255;
	if (mskSz.width > 0 && mskSz.height > 0)
	{
		for (int i = 0; i < mskSz.height; i++)
		{
			for (int j = 0; j < mskSz.width; j++)
			{
				if (srcMsk(i,j)[0] == 0 )
				{
					dstMsk(i,j) = 0;
				}
			}
		}
	}

}
void IntrinsicModel::setROIMask(mat_3f maskImg){
	::setMask(m_ROIMask,maskImg);
	m_dm.getMask() = m_ROIMask.clone();
}

void IntrinsicModel::setFeatureMask(mat_3f maskImg){
	::setMask(m_featureMask,maskImg);
}

void IntrinsicModel::addShadeScrib(Constraint& cons,float w){
	Constraint filterCons;
	for (int i = 0; i < cons.size(); i++)
	{
		GrpEle& ge = cons[i];
		if (ge.m_eMember.size() > 1)
		{
			filterCons.push_back(ge);
		}
	}
	m_cs = maskConstraint(filterCons);
	m_shadeScrib.m_dmPtr = &m_dm;
	m_shadeScrib.m_strokePtr = &m_cs;
	m_shadeScrib.initScribbles();
	m_isCS = true;
	m_csInited = false;
}

void IntrinsicModel::addAlbedoScrib(Constraint& cons,float w){
	Constraint filterCons;
	for (int i = 0; i < cons.size(); i++)
	{
		GrpEle& ge = cons[i];
		if (ge.m_eMember.size() > 1)
		{
			filterCons.push_back(ge);
		}
	}

	m_cr = maskConstraint(filterCons);
	m_albedoScrib.m_dmPtr = &m_dm;
	m_albedoScrib.m_strokePtr = &m_cr;
	m_albedoScrib.initScribbles();
	m_crInited = false;
	m_isCR = true;
}

void IntrinsicModel::addAbsShadeScrib(Constraint& cons, float w){
	m_abs = maskConstraint(cons);
	m_absShadeScrib.m_dmPtr = &m_dm;
	m_absShadeScrib.m_strokePtr = &m_abs;
	m_absShadeScrib.initScribbles();
	m_absInited= false;
	m_isAbsS = true;
}

Constraint IntrinsicModel::genConsAlbedoCons(float distThrd , float varThrd ){
	//use texture matching
	m_tpgs.m_dmPtr = &m_dm;
	m_tpgs.m_trd_dist = distThrd;
	m_tpgs.m_trd_chrvar = varThrd;
	m_tpgs.reset(); //clear all
	m_tpgs.group();
	return m_tpgs.m_groups; //get the constraint
}

Constraint IntrinsicModel::genConsShadeCons(){
	Constraint cons;
	return cons;
}

Constraint IntrinsicModel::genAbsShadeCons(float absVal){
	int iw = m_img.size().width;
	int ih = m_img.size().height;

	float maxVal = 0;
	int my = 0,mx = 0;
	for (int i = 0; i < ih; i++)
	{
		for (int j = 0; j < iw; j++)
		{
			float tmpVal = cv::norm(m_img(i,j));
			if (tmpVal > maxVal)
			{
				maxVal = tmpVal;
				my = i;
				mx = j;
			}
		}
	}
	Constraint cons;
	GrpEle grp;
	Element ele(my* iw + mx,1.0);
	ele.m_rgb = cv::Vec3f(absVal,absVal,absVal); // set a triple
	grp.m_eMember.push_back(ele);
	cons.push_back(grp);
	return cons;
}

void IntrinsicModel::reduceSdEdge(){
	//eliminate the possible sharp shading edges caused by color planar assumption
	//only process pixels at the color edge
	//using reflectance result as guidance
	mat_3f chromImg = m_dm.getChrom();
	//cv::imshow("chrom",refImg);
	//cv::waitKey(0);
	mat_3f sdImg = m_dm.getShading();
	mat_3f sdBk = sdImg.clone();
	int ih = sdImg.size().height;
	int iw = sdImg.size().width;
	int dx[] ={0,1,1};
	int dy[] = {1,0,1};
	int avgWinSz = 3;
	int hSz = avgWinSz / 2;

	float *weight = new float[avgWinSz * avgWinSz];
	bool distWeighted = false;
	float sum = 0;
	for (int i = - hSz; i <= hSz; i++)
	{
		for (int j = -hSz; j <= hSz; j++)
		{
			int  y = i + hSz;
			int  x = j + hSz;
			float tmpW;
			if (distWeighted)
			{
				float tmp = float(i * i + j * j) / (hSz * hSz);
				tmpW = std::exp(-tmp);
			}
			else
				tmpW = 1;
			weight[y * avgWinSz + x] = tmpW;
		}
	}

	mat_3f edgeImg(ih,iw);
	edgeImg = cv::Vec3f(0,0,0);
	for (int i = 0; i < ih - 1; i++)
	{
		for (int j = 0; j < iw - 1; j++)
		{
			if (!m_ROIMask(i,j))
			{
				continue;
			}
			cv::Vec3f r1 = chromImg(i,j);
			std::vector<int> nx;
			std::vector<int> ny;

			for (int k = 0; k < 3; k++)
			{
				int i1= i + dy[k];
				int j1 = j + dx[k];
				cv::Vec3f r2 = chromImg(i1,j1);

				if(Utility::vecDist(r1,r2) > 0.025){
					nx.push_back(-dx[k]);
					ny.push_back(-dy[k]);
					nx.push_back(dx[k]);
					ny.push_back(dy[k]);
					edgeImg(i,j) = cv::Vec3f(1,1,1);
				}
			}
			if (nx.empty())
			{
				continue;
			}
			cv::Vec3f avgVec(0,0,0);
			int ncnt = 0;
			for (int k1 = 0; k1 < nx.size(); k1++)
			{
				int y = i + ny[k1];
				int x = j + nx[k1];
				if ( y < 0 || y >= ih || x < 0|| x >= iw || !m_ROIMask(y,x))
				{
					continue;
				}
				avgVec += sdImg(y,x);
				ncnt++;
			}
			avgVec[0] /= ncnt;
			avgVec[1] /= ncnt;
			avgVec[2] /= ncnt;
			sdImg(i,j) = avgVec;
		}
	}
	//sdBk.copyTo(sdImg); //
	//cv::imshow("edge",edgeImg);
	//cv::waitKey(0);
}

void IntrinsicModel::decompose( bool normSd /*= false*/,bool normRef /*= false*/,bool wse /*= false*/, bool gamma /*= false*/ )
{
	int iw = m_img.size().width;
	int ih = m_img.size().height;
	mat_3f sdImg = m_dm.getShading();
	mat_3f refImg = m_dm.getAlbedo();

	if (wse)
	{
		reduceSdEdge(); //
	}
	//do gamma correction
	if (normSd)
	{
		//cv::imshow("mask",m_ROIMask);
		//cv::waitKey(0);

		DataManager::normalizeImage(sdImg,m_ROIMask);
	}
	if (normRef)
	{
		DataManager::normalizeImage(refImg,m_ROIMask);
	}

	if (gamma)
	{
		float gval = 2.2;
		IntrinsicModel::gammaCorrect(sdImg,gval);
		IntrinsicModel::gammaCorrect(refImg,gval);
	}

	//sdImg *= 1.5;
}

Constraint  IntrinsicModel::maskConstraint(Constraint& cons){
	Constraint mCons;
	int h = m_img.size().height;
	int w = m_img.size().width;
	for (int i = 0; i <cons.size(); i++)
	{
		GrpEle& ge = cons[i];
		GrpEle ge1 = ge;
		ge1.m_eMember.clear();
		for (int j = 0; j <ge.m_eMember.size(); j++)
		{
			Element& ele = ge.m_eMember[j];
			int py = ele.m_eleID / w;
			int px = ele.m_eleID % w;
			if (m_ROIMask(py,px))
			{
				ge1.m_eMember.push_back(ele);
			}
		}
		if (ge1.m_eMember.size() > 0)
		{
			mCons.push_back(ge1);
		}
	}
	return mCons;
}
Constraint IntrinsicModel::dszCons(int sz11, int sz12, Constraint cons){
	//
	int sz21 = sz11 >> 1;
	int sz22 = sz12 >> 1;

	mat_i sMat(sz21,sz22);
	sMat = 1;
	Constraint dszCons;
	for (int i = 0;i < cons.size(); i++)
	{
		GrpEle ge = cons[i];
		if (ge.m_eMember.empty())
		{
			continue;
		}
		GrpEle ge1 = ge;
		ge1.m_eMember.clear();
		for (int j = 0; j < ge.m_eMember.size() ;j++)
		{
			Element ele1 = ge.m_eMember[j];
			int px = ge.m_eMember[j].m_eleID % sz12;
			int py = ge.m_eMember[j].m_eleID / sz12;
			px >>= 1;
			py >>= 1;
			if (px >= 0 && px < sz22 && py >= 0 && py < sz21 && sMat(py,px))
			{
				ele1.m_eleID = py * sz22 + px;
				ge1.m_eMember.push_back(ele1);
				sMat(py,px) = 0;
			}
		}
		if (!ge1.m_eMember.empty())
		{
			dszCons.push_back(ge1);
		}
	}
	//
	return dszCons;
}

Constraint IntrinsicModel::uszCons(int sz11, int sz12, int sz21, int sz22, Constraint cons){
	Constraint upCons;
	for (int i = 0;i < cons.size(); i++)
	{
		GrpEle ge = cons[i];
		if (ge.m_eMember.empty())
		{
			continue;
		}
		GrpEle ge1 = ge;
		ge1.m_eMember.clear();
		for (int j = 0; j < ge.m_eMember.size() ;j++)
		{
			int px = ge.m_eMember[j].m_eleID % sz12;
			int py = ge.m_eMember[j].m_eleID / sz12;
			px <<= 1;
			py <<= 1;
			if (px >= 0 && px < sz21 && py >= 0 && py < sz22)
				ge1.m_eMember.push_back(Element(py * sz22 + px,ge.m_eMember[j].m_conf));
		}
		if (!ge1.m_eMember.empty())
		{
			upCons.push_back(ge1);
		}
	}
	return upCons;
}

ublas::matrix<mat_f> IntrinsicModel::uszAVec(int sz21, int sz22){
	//
	mat_3f aVecMat2(m_img.size());
	for (int i = 0; i < m_img.size().height; i++)
	{
		for (int j = 0; j < m_img.size().width; j++)
		{
			aVecMat2(i,j) = cv::Vec3f(m_aVecMat(i,j)(0,0),m_aVecMat(i,j)(1,0)
				,m_aVecMat(i,j)(2,0));
		}
	}
	mat_3f uszAVecMat2(sz21,sz22);
	cv::resize(aVecMat2,uszAVecMat2,uszAVecMat2.size(),0,0,cv::INTER_LINEAR);
	ublas::matrix<mat_f> uszAVecMat(sz21,sz22);
	for (int i = 0; i < sz21; i++)
	{
		for (int j = 0; j < sz22; j++)
		{
			uszAVecMat(i,j) = mat_f(3,1);
			uszAVecMat(i,j)(0,0) = uszAVecMat2(i,j)[0];
			uszAVecMat(i,j)(1,0) = uszAVecMat2(i,j)[1];
			uszAVecMat(i,j)(2,0) = uszAVecMat2(i,j)[2];
		}
	}
	return uszAVecMat;
}

void IntrinsicModel::initCPNlMatrix(int m_lWinSize){
	if (!m_cpnInited)
	{
		int hLWinSize = m_lWinSize / 2; //half window size
		int sizeN = 2 * 2 * hLWinSize + 1;
		int numNoneZeroEntry = sizeN * sizeN;
		int iW = m_img.size().width;
		int iH = m_img.size().height;
		//m_sLapmat = ublas_smat_f(iH * iW, iH * iW);
		this->m_mpinvMat = ublas::matrix<mat_f>(iH, iW); //each pixel has an "pinv(m)" matrix
		this->m_aVecMat = ublas::matrix<mat_f>(iH,iW); //"a" vector for each pixel
		//for each local window, construct the local
		int mNumRow = m_lWinSize * m_lWinSize + 3;
		mat_f mMatrix(mNumRow , 3); //matrix defined by Eq (8)
		mat_f invM(3,mNumRow);
		invM = 0;
		mMatrix = 0; //set to zero
		float m_epsilon = 1e-7;
		float sqrtEpsilon = sqrt(m_epsilon);
		mMatrix(mNumRow - 3,0) = mMatrix(mNumRow - 2, 1) = mMatrix(mNumRow - 1,2) = sqrtEpsilon;//set the last three rows
		for (int y = 0; y < iH; y++)
		{
			for (int x = 0; x < iW; x++)
			{
				m_mpinvMat(y,x) = invM;
			}
		}

		for (int y = hLWinSize; y < iH - hLWinSize; y++)
		{
			for (int x = hLWinSize; x < iW - hLWinSize; x++)
			{
				//set the matrix defined in Eq (8)
				if (!m_ROIMask(y,x))
				{
					continue;
				}
				for (int m = - hLWinSize; m <= hLWinSize; m++)
				{

					int yc = y + m;
					for (int n = - hLWinSize; n <= hLWinSize; n++)
					{
						int xc = x +n;
						int tmpIdx = (m + hLWinSize) * m_lWinSize + n + hLWinSize;
						mMatrix( tmpIdx, 0) = m_img(yc,xc)[0];//r
						mMatrix( tmpIdx, 1) = m_img(yc,xc)[1];//g
						mMatrix( tmpIdx, 2) = m_img(yc,xc)[2];//b
					}
				}
				mat_f mPinv = pinv(mMatrix); // pseudo inverse
				m_mpinvMat(y , x) = mPinv; //
			}
		}
		m_cpnInited = true;
	}
}

void IntrinsicModel::_cons2SparseMat(){
	//now formulate the constraint as matrix
	float w = 1.0;
	if(m_isCS){
		std::cout << "constant shading constraint to sparse matrix..." << std::endl;
		if (!m_csInited)
		{
			_shadeConstraint2SparseMat();
			m_csInited = true;
		}
		m_allConsMat = m_allConsMat + w * m_shadeConsMat;
		m_allConsVec += w * m_shadeConsVec;
	}
	if (m_isCR)
	{
		std::cout << "constant albedo constraint to sparse matrix..." << std::endl;
		if(!m_crInited)
		{
			_albedoConstraint2SparseMat();
			m_crInited = true;
		}
		m_allConsMat = m_allConsMat + w * m_albedoConsMat;
		m_allConsVec += w * m_albedoConsVec; //

	}
	if (m_isAbsS)
	{
		std::cout << "absolute shading constraint to sparse matrix..." << std::endl;
		if (!m_absInited)
		{
			_absoluteShadeConstraint2SparseMat();
			m_absInited =true;
		}
		float absW = 1.0;
		m_allConsMat = m_allConsMat + absW * m_absConsMat;
		m_allConsVec += absW * m_absConsVec;

	}
	typedef ublas_cm_f::iterator1 it1_t;
	typedef ublas_cm_f::iterator2 it2_t;
	int nzCnt = 0;
	float iw = m_img.size().width;
	float ih = m_img.size().height;
	for (it2_t it2 = m_allConsMat.begin2();it2 != m_allConsMat.end2(); it2++)
	{
		int idx1, idx2;
		for (it1_t it1 = it2.begin(); it1 != it2.end(); it1++,nzCnt++);
	}
	m_sparsity = double(nzCnt) / double(iw * ih * iw * ih);

}

void IntrinsicModel::lMat2sLmat(){
	//
	int ih = m_img.size().height;
	int iw = m_img.size().width;
	int h = m_lapMat.size1();
	int w = m_lapMat.size2();
	int hLWinSize = m_lWinSize / 2; //half window size
	int sizeN = 2 * 2 * hLWinSize + 1;

	m_sLapmat = ublas_cm_f( iw * ih, iw * ih);

	for (int i = 0; i < iw * ih ;i++)
	{
		int y = i / iw;
		int x = i % iw;
		for (int k = 0; k < w; k++)
		{
			if (m_lapMat(i,k)==0)
			{
				continue;
			}
			int yjc = y + k/sizeN - 2 * hLWinSize;
			int xjc = x + k%sizeN - 2 * hLWinSize;
			//std::cout << "y,x,y1,x1:" << y <<"," << x << "," << yjc << "," << xjc << std::endl;
			//getchar();
			m_sLapmat(i , yjc * iw + xjc) = m_lapMat(i,k);
		}
	}
}

void IntrinsicModel::_buildLaplacianMatrix(){
	std::cout << "initialize laplacian matrix..." << std::endl;
	int hLWinSize = m_lWinSize / 2; //half window size
	int sizeN = 2 * 2 * hLWinSize + 1;
	int numNoneZeroEntry = sizeN * sizeN;
	int iW = m_img.size().width;
	int iH = m_img.size().height;
	m_lapMat = ublas_mat_f(iH * iW, numNoneZeroEntry);
	for (int i = 0;i < m_lapMat.size1(); i++)
	{
		for (int j = 0; j < m_lapMat.size2(); j++)
		{
			m_lapMat(i,j) = 0;
		}
	}
	//for each local window, construct the local
	int mNumRow = m_lWinSize * m_lWinSize + 3;
	mat_f mMatrix(mNumRow , 3); //matrix defined by Eq (8)
	mMatrix = 0; //set to zero
	float m_epsilon = 1e-7;
	float sqrtEpsilon = sqrt(m_epsilon);
	mMatrix(mNumRow - 3,0) = mMatrix(mNumRow - 2, 1) = mMatrix(mNumRow - 1,2) = sqrtEpsilon;//set the last three rows
	for (int y = hLWinSize; y < iH - hLWinSize; y++)
	{
		for (int x = hLWinSize; x < iW - hLWinSize; x++)
		{
			//set the matrix defined in Eq (8)
			for (int m = - hLWinSize; m <= hLWinSize; m++)
			{

				int yc = y + m;
				for (int n = - hLWinSize; n <= hLWinSize; n++)
				{
					int xc = x +n;
					int tmpIdx = (m + hLWinSize) * m_lWinSize + n + hLWinSize;
					mMatrix( tmpIdx, 0) = m_img(yc,xc)[0];//r
					mMatrix( tmpIdx, 1) = m_img(yc,xc)[1];//g
					mMatrix( tmpIdx, 2) = m_img(yc,xc)[2];//b
				}
			}

			mat_f mPinv = pinv(mMatrix); // pseudo inverse
			mat_f mMatrix_mPinv = mMatrix * mPinv; //mi * pinv(mi)
			cv::Size tmpSize = mMatrix_mPinv.size();
			mat_f nMat = mat_f::eye(tmpSize) - mMatrix_mPinv; //n matrix
			cv::Size nMatSize = nMat.size();
			mat_f nMatTrans(cv::Size(nMatSize.width,nMatSize.height));
			cv::transpose(nMat,nMatTrans);
			mat_f nTrans_nMat = nMatTrans * nMat; //
			for (int mi = -hLWinSize; mi <= hLWinSize; mi++)
			{
				int yic = y + mi;
				for (int ni = -hLWinSize; ni <= hLWinSize; ni++)
				{
					int xic = x + ni;
					int i = yic * iW + xic; //pixel (yic, xic), row index in the Laplacian matrix
					int ik = (mi + hLWinSize) * m_lWinSize + ni + hLWinSize;

					for (int mj = -hLWinSize; mj <= hLWinSize; mj++)
					{
						int yjc = y + mj;
						for (int nj = -hLWinSize; nj <= hLWinSize; nj++)
						{
							int xjc = x + nj;
							int jk = (mj + hLWinSize) * m_lWinSize + nj + hLWinSize;
							int j = (yjc - yic + 2 * hLWinSize) * sizeN + (xjc - xic + 2 * hLWinSize);
							m_lapMat(i,j) += nTrans_nMat(ik,jk);
						}
					}
				}
			}
		}
	}
	m_matBuild = true;
	lMat2sLmat();
}

void IntrinsicModel::init()
{
	_init();
}
void IntrinsicModel::computeAVec()
{
	//compute "a" vector for each pixel

	initCPNlMatrix(m_lWinSize);
	int iW = m_img.size().width;
	int iH = m_img.size().height;
	mat_i pixAStatus(iH,iW);
	pixAStatus = 0;
	int hLWinSize = m_lWinSize / 2; //half window size
	//all marginal pixels should use the
	for (int y = 0;y < iH; y++)
	{
		for (int x = 0; x < iW; x++)
		{
			m_aVecMat(y,x) = mat_f(3,1);
			m_aVecMat(y,x) = 0;

			if (y >= hLWinSize && x >= hLWinSize && y < iH - hLWinSize && x < iW - hLWinSize)
			{
				pixAStatus(y,x) = 1;
			}
		}
	}
	//
	mat_3f shadeImg = m_dm.getShading();
	//
	for ( int y = hLWinSize; y < iH - hLWinSize; y++)
	{
		for (int x = hLWinSize; x < iW - hLWinSize; x++)
		{

			if (!m_ROIMask(y,x))
			{
				continue;
			}
			mat_f pmMat = m_mpinvMat(y,x);
			mat_f sVec(m_lWinSize * m_lWinSize + 3,1);
			sVec = 0; //
			int pIdx = 0;
			for (int i = -hLWinSize; i <= hLWinSize; i++)
			{
				int py = i + y;
				for (int j = -hLWinSize; j <= hLWinSize; j++, pIdx++)
				{
					int px = j + x;
					sVec(pIdx,0) = shadeImg(py,px)[0]; //get the shading value
				}
			}
			//now compute the a vector
			mat_f aVec = m_mpinvMat(y,x) * sVec; //
			m_aVecMat(y,x) = aVec;
			for (int i = -hLWinSize; i <= hLWinSize; i++)
			{
				int py = y + i;
				for (int j = -hLWinSize; j <= hLWinSize; j++)
				{
					int px = x + j;
					if (!pixAStatus(py,px) &&m_ROIMask(py,px))
					{
						m_aVecMat(py,px) = aVec;
						pixAStatus(py,px) = 1;
					}
				}
			}
		}
	}
	//average the a vectors
	//avgCPNormal(false);
}

mat_3f IntrinsicModel::aVec2Mat3f(){
	//
	mat_3f vis(m_img.size());
	for (int i = 0; i < m_img.size().height; i++)
	{
		for (int j = 0; j < m_img.size().width; j++)
		{
			mat_f tmpA = m_aVecMat(i,j);
			vis(i,j)[0] = tmpA(0,0);
			vis(i,j)[1] = tmpA(1,0);
			vis(i,j)[2] = tmpA(2,0);
		}
	}
	return vis;
}

void IntrinsicModel::visAVec(){
	mat_3f vis = aVec2Mat3f();
	static int cnt = 0;
	char buffer[256];
	sprintf(buffer,"%s_%d","window",cnt++);
	cv::imshow(buffer,vis);
	cv::waitKey(0);
}

void IntrinsicModel::avgCPNormal(bool distWeithed ){
	//
	int hLWinSize = m_lWinSize / 2;

	int avgWinSz = 3;
	int hSz = avgWinSz / 2;
	float *weight = new float[avgWinSz * avgWinSz];
	float sum = 0;
	for (int i = - hSz; i <= hSz; i++)
	{
		for (int j = -hSz; j <= hSz; j++)
		{
			int  y = i + hSz;
			int  x = j + hSz;
			float tmpW;
			if (distWeithed)
			{
				float tmp = float(i * i + j * j) / (hSz * hSz);
				tmpW = std::exp(-tmp);
			}
			else
				tmpW = 1;
			weight[y * avgWinSz + x] = tmpW;
			sum += tmpW;
		}
	}
	for (int i = 0; i < avgWinSz * avgWinSz; i++)
	{
		weight[i] /= sum;
	}
	//
	int iH = m_img.size().height;
	int iW = m_img.size().width;

	ublas::matrix<mat_f> aVecMatBK(m_aVecMat);

	for (int i = 0; i < iH; i++)
	{
		for (int j = 0; j < iW; j++)
		{
			if (!m_ROIMask(i,j))
			{
				continue;
			}
			mat_f avgVec(3,1);
			avgVec  = 0;
			float wSum = 0;
			for (int k1 = -hSz; k1 <= hSz; k1++)
			{
				for (int k2 = -hSz; k2 <= hSz; k2++)
				{
					int y = i + k1;
					int x = j + k2;
					if ( y < 0 || y >= iH || x < 0 || x >= iW || !m_ROIMask(y,x))
					{
						continue;
					}
					float tmpW =weight[(k1 + hSz) * avgWinSz + k2 + hSz];
					wSum += tmpW;
					mat_f curVec = m_aVecMat(y,x);
					avgVec = avgVec + tmpW * curVec;
				}
			}
			avgVec /= wSum;
			aVecMatBK(i,j) = avgVec.clone();
		}
	}
	m_aVecMat = aVecMatBK;
	delete[] weight;
}

mat_3f IntrinsicModel::decomposeFromAVec(){
	// s = I * a for each pixel
	int iH = m_img.size().height;
	int iW = m_img.size().width;
	mat_3f sImg = m_dm.getShading();
	mat_3f rImg = m_dm.getAlbedo();
	sImg = cv::Vec3f(0,0,0);
	for (int i = 0; i < iH; i++)
	{
		for (int j = 0; j < iW; j++)
		{
			//std::cout << "i,j:" << i <<"," << j << std::endl;
			if (!m_ROIMask(i,j))
			{
				continue;
			}
			mat_f aVec = m_aVecMat(i,j);
			cv::Vec3f rgbVec = m_img(i,j);
			float sVal = aVec(0,0) * rgbVec[0] + aVec(1,0) * rgbVec[1] + aVec(2,0) * rgbVec[2];
			sImg(i,j) = cv::Vec3f(sVal,sVal,sVal);
			sVal = sVal > 1e-8 ? 1/sVal: 0;
			rImg(i,j) = cv::Vec3f(rgbVec[0] * sVal,rgbVec[1] * sVal,rgbVec[2] * sVal);
		}
	}
	return sImg;
}

void IntrinsicModel::solve(){
	m_solveTime = 0.0;
	_cons2SparseMat(); //
	//std::cout << "solve the linear system..." << std::endl;
	_solve();
	//convert the solution vector to shading image
	int iw = m_img.size().width;
	mat_3f sdImg = m_dm.getShading();
	mat_3f inputImg = m_dm.getInput();
	mat_3f refImg = m_dm.getAlbedo();
	for (int i = 0; i < m_solutionVec.size(); i++)
	{
		int px = i % iw;
		int py = i / iw;
		float sdVal = m_solutionVec(i);
		if (!m_ROIMask(py,px))
		{
			sdVal = 0;
		}
		sdImg(py,px) = cv::Vec3f(sdVal,sdVal,sdVal);
		cv::Vec3f rgb = inputImg(py,px);
		sdVal = sdVal > 1e-8? 1/sdVal: 0;
		refImg(py,px) = cv::Vec3f(rgb[0] * sdVal, rgb[1] * sdVal, rgb[2] * sdVal);
	}
	//
}

void IntrinsicModel::saveAlbedoConsVis(std::string basePath, std::string imgName){
	if (m_isCR && m_tpgs.m_dmPtr)
	{
		char buffer[512];
		m_tpgs.saveGrpVis(basePath+"/"+imgName,"tpgs",20);
	}

}

void IntrinsicModel::saveResult(std::string basePath, std::string imgName){
	//save the result
	char subDirName[256];
	//if (m_tpgs.m_dmPtr)
	//{
		//sprintf(subDirName,"%s_%s_%f_%f_%f",imgName.c_str(),m_modelName.c_str(),IMConfig::chromThrd,IMConfig::txVar,IMConfig::distThrd);
	//}else{
		sprintf(subDirName,"%s_%s",imgName.c_str(),m_modelName.c_str());
	//}
	basePath = basePath + "/" +  subDirName;
	//CreateDirectory(basePath.c_str(),NULL); //create the directory

	boost::filesystem::path rootPath ( basePath.c_str() );
	boost::system::error_code returnedError;
	boost::filesystem::create_directories( rootPath, returnedError );

	std::string sdImgPath = basePath + "/" + imgName + "_sd_" + m_modelName + ".png";
	std::string refImgPath = basePath + "/" + imgName + "_ref_" + m_modelName + ".png";
	std::string chromImgPath = basePath + "/" + imgName + "_chrom_" + m_modelName + ".png";

	cv::imwrite(sdImgPath,m_dm.getShading() * 255.0);
	cv::imwrite(refImgPath,m_dm.getAlbedo() * 255.0);
	cv::imwrite(chromImgPath,m_dm.getChrom() * 255.0);
	//save albedo constraints
	//saveAlbedoConsVis(basePath,imgName);
	//save other information
	std::string fname = basePath + "/" + imgName + ".txt";
	FILE* fid = fopen(fname.c_str(),"w");
	int iw = m_img.size().width;
	int ih = m_img.size().height;
	fprintf(fid,"image size: %d x %d\n",ih , iw);
	fprintf(fid,"solution time:%f\n",m_solveTime);
	fprintf(fid,"matrix time:%f\n",m_matrixTime);
	fprintf(fid,"matrix sparsity:%f\n",m_sparsity);
	fclose(fid);

}

//void IntrinsicModel::visConstraint(){
	//int h = m_img.size().height;
	//int w = m_img.size().width;
	//if(m_isCS){
		//verifyUserConstraint(m_shadeScrib.m_scrPix,h ,w);
	//}
	//if (m_isCR)
	//{
		//verifyUserConstraint(m_albedoScrib.m_scrPix,h,w);
	//}
	//if (m_isAbsS)
	//{
		//verifyUserConstraint(m_absShadeScrib.m_scrPix,h,w);
	//}
//}

//#define __USE_SM__

void IntrinsicModel::_shadeConstraint2SparseMat(){
	int iw = m_img.size().width;
	int ih = m_img.size().height;
	Constraint cons = m_shadeScrib.m_scrPix;
	m_shadeConsMat = constructShadeSparseMat(cons,iw,ih);
	m_shadeConsVec = ublas_vec_f(iw * ih);
	for (int i =0; i < iw * ih; i++)
	{
		m_shadeConsVec(i) = 0;
	}
#ifdef __USE_SM__
	SparseMat sm(iw * ih, iw * ih);
#endif

	for (int i = 0;i < cons.size(); i++)
	{
		GrpEle stroke = cons[i];
		//for each pixel stroke, set it as the reference and compute the error
		int s_sz = stroke.m_eMember.size();
		//std::cout << "shading grp size:" << s_sz << std::endl;
		if(!m_eqCons)
		{
			float n_factor = 1.0 / s_sz;
			for (int j = 0; j < s_sz; j++)
			{
				int r_px = stroke.m_eMember[j].m_eleID; //reference pixel
				float conf1 = stroke.m_eMember[j].m_conf;
				for (int k = j + 1; k < s_sz; k++)
				{
					int c_px = stroke.m_eMember[k].m_eleID; //current pixel
					float conf2 = stroke.m_eMember[k].m_conf;
					float tmpW = n_factor * (conf1 + conf2);
					m_shadeConsMat(r_px,r_px) += tmpW;
					m_shadeConsMat(c_px,c_px) += tmpW;
					m_shadeConsMat(r_px,c_px) -= tmpW;
					m_shadeConsMat(c_px,r_px) -= tmpW;
				}
			}
		}
		else{
			for (int k = 0; k < s_sz - 1; k++)
			{
				int r_px = stroke.m_eMember[k].m_eleID;
				int c_px = stroke.m_eMember[k+1].m_eleID;
				float conf1 = stroke.m_eMember[k].m_conf;
				float conf2 = stroke.m_eMember[k+1].m_conf;
				float tmp = (conf1 + conf2) * m_csLambda;
#ifdef __USE_SM__
				sm.addElement(r_px,r_px,tmp);
				sm.addElement(c_px,c_px,tmp);
				sm.addElement(r_px,c_px,-tmp);
				sm.addElement(c_px,r_px,-tmp);
#else
				m_shadeConsMat(r_px,r_px) += tmp;
				m_shadeConsMat(c_px,c_px) += tmp;
				m_shadeConsMat(r_px,c_px) -= tmp;
				m_shadeConsMat(c_px,r_px) -= tmp;
#endif
			}
		}
	}
#ifdef 	__USE_SM__
	sm.merge();
	sm.dumpToFile("d:/ELEZQI/shade_sparse.txt");
#endif
}

void IntrinsicModel::_albedoConstraint2SparseMat(){
	int iw = m_img.size().width;
	int ih = m_img.size().height;
	int pxCnt = ih * iw;
	m_albedoConsMat = constructAlbedoSparseMat(m_cr,iw ,ih);
	//m_albedoConsMat = ublas_cm_f(pxCnt,pxCnt);
	m_albedoConsVec = ublas_vec_f(pxCnt);
	for (int i = 0; i < pxCnt; i++)
	{
		m_albedoConsVec(i) = 0;
	}
}
void IntrinsicModel::_absoluteShadeConstraint2SparseMat(){
	int iw = m_img.size().width;
	int ih = m_img.size().height;
	this->m_absConsMat = ublas_cm_f(iw * ih, iw * ih);
	this->m_absConsVec = ublas_vec_f(iw * ih);
	for (int i = 0; i < iw * ih; i++)
	{
		m_absConsVec(i) = 0;
	}

	Constraint absShadeCons = m_absShadeScrib.m_scrPix;
	//std::cout << "# of abs stroke:" << absShadeCons.size() << std::endl;
	for (int i = 0;i < absShadeCons.size(); i++)
	{
		GrpEle stroke = absShadeCons[i];
		//std::cout<<"stroke size:" << stroke.m_eMember.size() << std::endl;
		for (int j = 0; j < stroke.m_eMember.size(); j++)
		{
			int pid = stroke.m_eMember[j].m_eleID;
			float pval = m_absShadeScrib.scrVal(pid / iw, pid % iw)[0];

			if (m_modelName == "RI")
			{
				pval = log(pval);
			}
			m_absConsMat(pid,pid) = m_absLambda; //1 * lambda
			m_absConsVec(pid) = m_absLambda * pval; //get the absolute shading value
		}
	}
}

void IntrinsicModel::saveConsGrpVal(std::string basePath,std::string imgName){
	//
	char buffer[256];
	if (m_isCS)
	{
		m_shadeScrib.updateScrVal();
		m_shadeScrib.writeScrPixVal(basePath + "/" + imgName + "_cs.txt",false);
		m_shadeScrib.writeScrPixVal(basePath + "/" + imgName + "_cs.png",true);

	}
	if (m_isCR)
	{
		m_albedoScrib.updateScrVal();
		m_albedoScrib.writeScrPixVal(basePath + "/" + imgName + "_cr.txt",false);
		m_albedoScrib.writeScrPixVal(basePath + "/" + imgName + "_cr.png",true);

	}
}

void IntrinsicModel::saveSparseMatrix(std::string rootPath){
	//
	std::string smPath = rootPath + "\\" +"allCons_mat.txt";
	std::string bPath = rootPath + "\\" + "allcons_vec.txt";
	std::string dimPath = rootPath + "\\" + "img_dim.txt";

	typedef ublas_cm_f::iterator1 it1_t;
	typedef ublas_cm_f::iterator2 it2_t;
	ublas_cm_f& resultMat = m_allConsMat;

	std::ofstream ofs;
	ofs.open(smPath.c_str(),std::ios_base::out);
	for (it1_t it1 = resultMat.begin1();it1 != resultMat.end1(); it1++)
	{
		int idx1, idx2;
		for (it2_t it2 = it1.begin(); it2 != it1.end(); it2++)
		{
			idx1 = it2.index1();
			idx2 = it2.index2();
			float eleVal = resultMat(idx1,idx2);
			ofs << idx1 << " " << idx2 << " " << eleVal << "\n";
		}
	}
	ofs.close();
	ofs.open(bPath.c_str(),std::ios::out);

	for (int i = 0; i < m_allConsVec.size(); i++)
	{
		ofs << m_allConsVec(i) << "\n";
	}
	ofs.close();
	//
	ofs.open(dimPath.c_str(),std::ios::out);
	ofs << m_img.size().height << " " << m_img.size().width << "\n";
	ofs.close();
}

void IntrinsicModel::gammaCorrect(mat_3f img, float gamma){
	//
	cv::pow(img,1.0/gamma,img);
}
IntrinsicModel::~IntrinsicModel(){

}


//
// Copyright Karl Meerbergen 2007
//
// 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)
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_STEQR_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_STEQR_HPP

#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/vector_traits.hpp>
#include <boost/numeric/bindings/traits/matrix_traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

  /********************************************************************/
  /*                      eigenvalue problems                         */
  /********************************************************************/

  /* tridiagonal symmetric */

namespace boost { namespace numeric { namespace bindings { namespace lapack {

    namespace detail {

      inline
      void steqr ( char compz, int n, float* d, float* e, float* z, int ldz, float* work, int& info )
      {
        LAPACK_SSTEQR( &compz, &n, d, e, z, &ldz, work, &info ) ;
      }

      inline
      void steqr ( char compz, int n, double* d, double* e, double* z, int ldz, double* work, int& info )
      {
        LAPACK_DSTEQR( &compz, &n, d, e, z, &ldz, work, &info ) ;
      }

    } // namespace detail

    template <typename D, typename E, typename Z, typename W>
    inline
    int steqr( char compz, D& d, E& e, Z& z, W& work ) {

      int const n = traits::vector_size (d);
      assert( traits::vector_size (e) == n-1 );
      assert( traits::matrix_size1 (z) == n );
      assert( traits::matrix_size2 (z) == n );
      assert( compz=='N' || compz=='V' || compz=='I' );

      int lwork = traits::vector_size( work ) ;

      int info;
      detail::steqr( compz, n,
                     traits::vector_storage( d ),
                     traits::vector_storage( e ),
                     traits::matrix_storage( z ),
                     traits::leading_dimension( z ),
                     traits::vector_storage( work ),
                     info ) ;
      return info;
    } // steqr()

    template <typename D, typename E, typename Z>
    inline
    int steqr( char compz, D& d, E& e, Z& z, optimal_workspace ) {
      int lwork = 0 ;
      if (compz != 'N') lwork = 2 * traits::vector_size( d ) - 2 ;

      traits::detail::array<typename traits::vector_traits<D>::value_type> work( lwork );

      return steqr( compz, d, e, z, work ) ;
    }

    template <typename D, typename E, typename Z>
    inline
    int steqr( char compz, D& d, E& e, Z& z, minimal_workspace ) {
      int lwork = 1 ;
      if (compz != 'N') lwork = 2 * traits::vector_size( e ) ;

      traits::detail::array<typename traits::vector_traits<D>::value_type> work( lwork );

      return steqr( compz, d, e, z, work ) ;
    }

    template <typename D, typename E, typename Z>
    inline
    int steqr( char compz, D& d, E& e, Z& z ) {
      return steqr( compz, d, e, z, minimal_workspace() ) ;
    }

}}}}

#endif // BOOST_NUMERIC_BINDINGS_LAPACK_GBSV_HPP


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_DETAIL_INTERVAL_MORPHISM_HPP_JOFA_080315
#define BOOST_ICL_DETAIL_INTERVAL_MORPHISM_HPP_JOFA_080315

#include <boost/icl/detail/notate.hpp>
#include <boost/icl/concept/interval_set_value.hpp>
#include <boost/icl/concept/element_set_value.hpp>
#include <boost/icl/concept/set_value.hpp>
#include <boost/icl/concept/map_value.hpp>
#include <boost/icl/associative_interval_container.hpp>
#include <boost/icl/associative_element_container.hpp>

namespace boost{namespace icl
{
    namespace segmental
    {
        template <typename ElementContainerT, typename IntervalContainerT>
        void atomize(ElementContainerT& result, const IntervalContainerT& src)
        {
            ICL_const_FORALL(typename IntervalContainerT, itv_, src)
            {
                const typename IntervalContainerT::key_type& itv   = icl::key_value<IntervalContainerT>(itv_);
                typename IntervalContainerT::codomain_type   coval = icl::co_value<IntervalContainerT>(itv_);

                for(typename IntervalContainerT::domain_type element = first(itv); element <= last(itv); ++element)
                    icl::insert(result, icl::make_value<ElementContainerT>(element, coval));
            }
        }

        template <typename IntervalContainerT, typename ElementContainerT>
        void cluster(IntervalContainerT& result, const ElementContainerT& src)
        {
            typedef typename IntervalContainerT::key_type key_type;
            ICL_const_FORALL(typename ElementContainerT, element_, src)
            {
                const typename ElementContainerT::key_type&  key
                    = key_value<ElementContainerT>(element_);
                const typename codomain_type_of<ElementContainerT>::type& coval
                    = co_value<ElementContainerT>(element_);

                result += icl::make_value<IntervalContainerT>(key_type(key), coval);
            }
        }

        template <typename AtomizedType, typename ClusteredType>
        struct atomizer
        {
            void operator()(AtomizedType& atomized, const ClusteredType& clustered)
            {
                segmental::atomize(atomized, clustered);
            }
        };

        template <typename ClusteredType, typename AtomizedType>
        struct clusterer
        {
            void operator()(ClusteredType& clustered, const AtomizedType& atomized)
            {
                segmental::cluster(clustered, atomized);
            }
        };

        template <typename JointType, typename SplitType>
        struct joiner
        {
            void operator()(JointType& joint, SplitType& split)
            {
                icl::join(split);
                ICL_FORALL(typename SplitType, split_, split)
                    joint.insert(*split_);
            }
        };

        template <typename AbsorberType, typename EnricherType>
        struct identity_absorber
        {
            void operator()(AbsorberType& absorber, EnricherType& enricher)
            {
                icl::absorb_identities(enricher);
                ICL_FORALL(typename EnricherType, enricher_, enricher)
                    absorber.insert(*enricher_);
            }
        };

    } // namespace Interval

    template<>
    inline std::string binary_template_to_string<segmental::atomizer>::apply() { return "@"; }
    template<>
    inline std::string binary_template_to_string<segmental::clusterer>::apply() { return "&"; }
    template<>
    inline std::string binary_template_to_string<segmental::joiner>::apply() { return "j"; }
    template<>
    inline std::string binary_template_to_string<segmental::identity_absorber>::apply() { return "a0"; }
}} // namespace boost icl

#endif // BOOST_ICL_DETAIL_INTERVAL_MORPHISM_HPP_JOFA_080315


/**
 * \ file GainCompressorFilter.cpp
 */

#include <ATK/Dynamic/GainCompressorFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 64;

BOOST_AUTO_TEST_CASE( GainFilter_threshold_test )
{
  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter;
  filter.set_threshold(10);
  BOOST_CHECK_EQUAL(filter.get_threshold(), 10);
}

BOOST_AUTO_TEST_CASE( GainFilter_threshold_db_test )
{
  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter;
  filter.set_threshold_db(20);
  BOOST_CHECK_EQUAL(filter.get_threshold(), 100);
}

BOOST_AUTO_TEST_CASE( GainFilter_threshold_range_test )
{
  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter;
  BOOST_CHECK_THROW(filter.set_threshold(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( GainFilter_ratio_test )
{
  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter;
  filter.set_ratio(10);
  BOOST_CHECK_EQUAL(filter.get_ratio(), 10);
}

BOOST_AUTO_TEST_CASE( GainFilter_ratio_range_test )
{
  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter;
  BOOST_CHECK_THROW(filter.set_ratio(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( GainCompressorFilter_softness_test )
{
  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter;
  filter.set_softness(0.5);
  BOOST_CHECK_EQUAL(filter.get_softness(), 0.5);
}

BOOST_AUTO_TEST_CASE( GainCompressorFilter_softness_range_test )
{
  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter;
  BOOST_CHECK_THROW(filter.set_softness(-0.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( GainCompressorFilter_const_1_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter;
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(10);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(1, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainCompressorFilter_const_0_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(1, outdata[i], 0.1); // if input is zero, we still need a gain of 1 to have a progression of 1 for values < threshold
  }
}

BOOST_AUTO_TEST_CASE( GainCompressorFilter_const_1_threshold_05_ratio_2_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(2);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.836990654, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE(GainCompressorFilter_const_1_threshold_05_ratio_2_test_LUT)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(2);
  filter.set_softness(1);
  filter.wait_for_LUT_completion();

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.836990654, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainCompressorFilter_const_1_threshold_05_ratio_4_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.765739262, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE(GainCompressorFilter_const_1_threshold_05_ratio_4_test_LUT)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainCompressorFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(4);
  filter.set_softness(1);
  filter.wait_for_LUT_completion();

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.765739262, outdata[i], 0.1);
  }
}


#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <iostream>
#include <iterator>
#include <sstream>

#include <boost/assign/list_of.hpp>
using boost::assign::list_of;

#include <infermous/base/CRFUtil.h>
using namespace infermous;

//#################### HELPERS ####################

enum Colour
{
  RED,
  BLUE
};

namespace Eigen {
template <> struct NumTraits<Colour> : NumTraits<int> {};
}

std::string to_string(const std::vector<Eigen::Vector2i>& v)
{
  std::ostringstream oss;
  for(std::vector<Eigen::Vector2i>::const_iterator it = v.begin(), iend = v.end(); it != iend; ++it)
  {
    oss << it->transpose() << '\n';
  }
  return oss.str();
}

//#################### TESTS ####################

BOOST_AUTO_TEST_SUITE(test_CRFUtil)

BOOST_AUTO_TEST_CASE(make_circular_neighbour_offsets_test)
{
  std::vector<Eigen::Vector2i> offsets = CRFUtil::make_circular_neighbour_offsets(3);

  std::vector<Eigen::Vector2i> expectedOffsets = list_of
    (Eigen::Vector2i(0,-3))
    (Eigen::Vector2i(-2,-2))
    (Eigen::Vector2i(-1,-2))
    (Eigen::Vector2i(0,-2))
    (Eigen::Vector2i(1,-2))
    (Eigen::Vector2i(2,-2))
    (Eigen::Vector2i(-2,-1))
    (Eigen::Vector2i(-1,-1))
    (Eigen::Vector2i(0,-1))
    (Eigen::Vector2i(1,-1))
    (Eigen::Vector2i(2,-1))
    (Eigen::Vector2i(-3,0))
    (Eigen::Vector2i(-2,0))
    (Eigen::Vector2i(-1,0))
    (Eigen::Vector2i(1,0))
    (Eigen::Vector2i(2,0))
    (Eigen::Vector2i(3,0))
    (Eigen::Vector2i(-2,1))
    (Eigen::Vector2i(-1,1))
    (Eigen::Vector2i(0,1))
    (Eigen::Vector2i(1,1))
    (Eigen::Vector2i(2,1))
    (Eigen::Vector2i(-2,2))
    (Eigen::Vector2i(-1,2))
    (Eigen::Vector2i(0,2))
    (Eigen::Vector2i(1,2))
    (Eigen::Vector2i(2,2))
    (Eigen::Vector2i(0,3));

  BOOST_CHECK_EQUAL(to_string(offsets), to_string(expectedOffsets));
}

BOOST_AUTO_TEST_CASE(make_square_neighbour_offsets_test)
{
  std::vector<Eigen::Vector2i> offsets = CRFUtil::make_square_neighbour_offsets(2);

  std::vector<Eigen::Vector2i> expectedOffsets = list_of
    (Eigen::Vector2i(-2,-2))
    (Eigen::Vector2i(-1,-2))
    (Eigen::Vector2i(0,-2))
    (Eigen::Vector2i(1,-2))
    (Eigen::Vector2i(2,-2))
    (Eigen::Vector2i(-2,-1))
    (Eigen::Vector2i(-1,-1))
    (Eigen::Vector2i(0,-1))
    (Eigen::Vector2i(1,-1))
    (Eigen::Vector2i(2,-1))
    (Eigen::Vector2i(-2,0))
    (Eigen::Vector2i(-1,0))
    (Eigen::Vector2i(1,0))
    (Eigen::Vector2i(2,0))
    (Eigen::Vector2i(-2,1))
    (Eigen::Vector2i(-1,1))
    (Eigen::Vector2i(0,1))
    (Eigen::Vector2i(1,1))
    (Eigen::Vector2i(2,1))
    (Eigen::Vector2i(-2,2))
    (Eigen::Vector2i(-1,2))
    (Eigen::Vector2i(0,2))
    (Eigen::Vector2i(1,2))
    (Eigen::Vector2i(2,2));

  BOOST_CHECK_EQUAL(to_string(offsets), to_string(expectedOffsets));
}

BOOST_AUTO_TEST_CASE(predict_labels_test)
{
  ProbabilitiesGrid<Colour> probabilities(2,2);
  probabilities(0,0)[RED] = 0.6f;
  probabilities(1,0)[RED] = 0.3f;
  probabilities(0,1)[RED] = 0.2f;
  probabilities(1,1)[RED] = 0.8f;

  for(size_t y = 0; y < 2; ++y)
  {
    for(size_t x = 0; x < 2; ++x)
    {
      probabilities(x,y)[BLUE] = 1.0f - probabilities(x,y)[RED];
    }
  }

  Grid<Colour> labels = CRFUtil::predict_labels(probabilities);

  Grid<Colour> expectedLabels(2,2);
  expectedLabels(0,0) = expectedLabels(1,1) = RED;
  expectedLabels(1,0) = expectedLabels(0,1) = BLUE;

  BOOST_CHECK_EQUAL(labels, expectedLabels);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <boost/astronomy/coordinate/cartesian_representation.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/io.hpp>

using namespace boost::astronomy::coordinate;
using namespace boost::units;
using namespace boost::units::si;

typedef boost::geometry::model::point<double, 3, boost::geometry::cs::spherical<boost::geometry::degree>> geometry_point;

int main()
{
    //creating point without any value
    cartesian_representation<double, quantity<si::length>, quantity<si::length>, quantity<si::length>> point1;

    //creating a point by directly providing values of x, y and z
    auto point2 = make_cartesian_representation(10.0 * meter, 20.0 * meter, 30.0 * meter);

    //assigning value of individual component
    point1.set_x(50.0 * meter);
    point1.set_y(40.0 * meter);
    point1.set_z(880.0 * meter);

    //or to set all the values in single statement tuple could be used as follow
    point2.set_x_y_z(38.5 * meter, 50.0 * meter, 64.23 * meter);

    //creating a point from another point
    auto point3 = make_cartesian_representation(point2);

    //creating a point from boost::geometry::model::point
    //any type of point can be used here
    //here we will demonstrate with cartesian point
    //https://www.boost.org/doc/libs/1_71_0/libs/geometry/doc/html/geometry/reference/models/model_point.html
    //https://www.boost.org/doc/libs/1_71_0/libs/geometry/doc/html/geometry/reference/cs.html
    geometry_point gp(45.0, 60.0, 50.0);
    auto point4 = make_cartesian_representation<double, quantity<si::length>, quantity<si::length>, quantity<si::length>>(gp);

    //accessing each component of representation
    std::cout << point4.get_x() << point4.get_y()<<std::endl; //methods get_y and get_z are available

    //get boost::geometry::model::point of current object
    auto stored_point = point3.get_point();

    //get the tuple of the component in the coordinate
    std::tuple<quantity<si::length>, quantity<si::length>, quantity<si::length>>
        components = point3.get_x_y_z();

    std::cin.get();
    return 0;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock. 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/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_complex.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif
#ifdef TEST_GMP
#include <boost/multiprecision/gmp.hpp>
#endif
#ifdef TEST_MPFR
#include <boost/multiprecision/mpfr.hpp>
#endif
#ifdef TEST_MPC
#include <boost/multiprecision/mpc.hpp>
#endif

struct A { virtual void g() = 0; };

void f(A&);
void f(boost::multiprecision::cpp_bin_float_50);
void f(boost::multiprecision::cpp_int);
void f(boost::multiprecision::cpp_rational);
void f(boost::multiprecision::cpp_dec_float_50);
void f(boost::multiprecision::cpp_complex_100);
#ifdef TEST_FLOAT128
void f(boost::multiprecision::float128);
#endif
#ifdef TEST_GMP
void f(boost::multiprecision::mpz_int);
void f(boost::multiprecision::mpf_float);
void f(boost::multiprecision::mpq_rational);
#endif
#ifdef TEST_MPFR
void f(boost::multiprecision::mpfr_float);
#endif
#ifdef TEST_MPC
void f(boost::multiprecision::mpc_complex);
#endif

void h(A&a)
{
   f(a);
}


/*
 * Copyright 2019 Tomasz bla Fortuna. All rights reserved.
 * License: MIT
 * bla@thera.be, https://github.com/blaa/flatritrie
 */

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

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

/*
 * Store IPs in a expanded form: a pair: (IP=1.0.0.0/8, ID=1) will get
 * expanded to 16777216 IPs and stored separately in a map.
 *
 * IP Addresses are stored from the most generic (eg. /8) to more specific (like
 * /24 or /32) and more specific entries overwrite the previously stored generic
 * ones.
 *
 * This takes a lot of RAM, might not be very cache local, but should be fast
 * for querying. Won't do for IPv6 and /48 mask though.
 */
template<typename K=uint32_t, typename V=int32_t>
class IPMap {
protected:
    void expand_ip(V value, const std::string &address) {
        std::vector<std::string> address_split;
        boost::split(address_split, address, boost::is_any_of("/"));
        assert(address_split.size() == 2);
        in_addr_t addr = inet_addr(address_split[0].c_str());
        assert((int32_t)addr != -1);

        int mask = atoi(address_split[1].c_str());

        int ip_first = ntohl(addr);
        int ip_last = ip_first | (0xFFFFFFFF >> mask);

        if (mask == 32) {
            this->map[ip_first] = value;
            return;
        }

        for (int ip = ip_first; ip <= ip_last; ip++) {
            this->map[ip] = value;
        }
    }

    /* Forbid copying */
    IPMap(const IPMap &map);

    std::unordered_map<K, V> map;
public:
    IPMap(int reserve=1000000) {
        /* Reserving huge space can decrease number of collisions and improve
         * performance a lot */
        this->map.reserve(reserve);
    }

    void add(const std::string &addr, V value) {
        this->expand_ip(value, addr);
    }

    int query_string(const std::string &addr) {
        in_addr ip_parsed;
        int ret = inet_aton(addr.c_str(), &ip_parsed);
        assert(ret != 0);
        uint32_t ip_network = ntohl(ip_parsed.s_addr);
        return this->query(ip_network);
    }

    V query(K ip) const {
        auto t = map.find(ip);
        if (t == map.end()) {
            return -1;
        } else {
            return t->second;
        }
    }

    int size() const {
        return this->map.size();
    }
};


/* -*- c++ -*- */
/*
 * Copyright 2004,2006,2010-2012 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "correlate_access_code_bb_impl.h"
#include <gnuradio/io_signature.h>
#include <gnuradio/blocks/count_bits.h>
#include <boost/format.hpp>
#include <stdexcept>
#include <cstdio>

namespace gr {
  namespace digital {

    correlate_access_code_bb::sptr
    correlate_access_code_bb::make(const std::string &access_code, int threshold)
    {
      return gnuradio::get_initial_sptr
	(new correlate_access_code_bb_impl(access_code, threshold));
    }

    correlate_access_code_bb_impl::correlate_access_code_bb_impl(
        const std::string &access_code, int threshold)
      : sync_block("correlate_access_code_bb",
		      io_signature::make(1, 1, sizeof(char)),
		      io_signature::make(1, 1, sizeof(char))),
	d_data_reg(0), d_flag_reg(0), d_flag_bit(0), d_mask(0),
	d_threshold(threshold)
    {
      if(!set_access_code(access_code)) {
	GR_LOG_ERROR(d_logger, "access_code is > 64 bits");
	throw std::out_of_range ("access_code is > 64 bits");
      }
    }

    correlate_access_code_bb_impl::~correlate_access_code_bb_impl()
    {
    }

    bool
    correlate_access_code_bb_impl::set_access_code(
        const std::string &access_code)
    {
      unsigned len = access_code.length();	// # of bytes in string
      if(len > 64)
	return false;

      // set len top bits to 1.
      d_mask = ((~0ULL) >> (64 - len)) << (64 - len);

      d_flag_bit = 1LL << (64 - len);	// Where we or-in new flag values.
                                        // new data always goes in 0x0000000000000001
      d_access_code = 0;
      for(unsigned i=0; i < 64; i++){
	d_access_code <<= 1;
	if(i < len)
	  d_access_code |= access_code[i] & 1;	// look at LSB only
      }

      return true;
    }

    int
    correlate_access_code_bb_impl::work(int noutput_items,
					gr_vector_const_void_star &input_items,
					gr_vector_void_star &output_items)
    {
      const unsigned char *in = (const unsigned char*)input_items[0];
      unsigned char *out = (unsigned char*)output_items[0];

      for(int i = 0; i < noutput_items; i++) {
	// compute output value
	unsigned int t = 0;

	t |= ((d_data_reg >> 63) & 0x1) << 0;
	t |= ((d_flag_reg >> 63) & 0x1) << 1;	// flag bit
	out[i] = t;

	// compute hamming distance between desired access code and current data
	unsigned long long wrong_bits = 0;
	unsigned int nwrong = d_threshold+1;
	int new_flag = 0;

	wrong_bits  = (d_data_reg ^ d_access_code) & d_mask;
	nwrong = gr::blocks::count_bits64(wrong_bits);

	// test for access code with up to threshold errors
	new_flag = (nwrong <= d_threshold);

	// shift in new data and new flag
	d_data_reg = (d_data_reg << 1) | (in[i] & 0x1);
	d_flag_reg = (d_flag_reg << 1);
	if(new_flag) {
	  d_flag_reg |= d_flag_bit;
	}
      }

      return noutput_items;
    }

  } /* namespace digital */
} /* namespace gr */


#define BOOST_TEST_MAIN TestExpression
#include <string>
#include <vector>
#include <boost/test/unit_test.hpp>
#include <sam/Expression.hpp>
#include <sam/tuples/VastNetflow.hpp>

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

using namespace sam;
using namespace sam::vast_netflow;

//TODO Redo these tests because it seems to be failing in ServerQuery.  However,
//I think there is an example in TestTopk that is working.

BOOST_AUTO_TEST_CASE( number_test )
{
  //FeatureMap featureMap;
  //auto numberToken = std::make_shared<NumberToken<VastNetflow>>(
  //  featureMap, 1.0);
  //auto addOper = std::make_shared<AddOperator<VastNetflow>>(featureMap);

/*  string str = "1.0 + 2.5";
  Expression<FilterGrammar<std::string::const_iterator>> expression(str);
  FeatureMap featureMap;
  std::string key = "blah";
  BOOST_CHECK_EQUAL(expression.evaluate(key, featureMap), 3.5);*/
}

BOOST_AUTO_TEST_CASE( comparison_test )
{
/*  FeatureMap featureMap;
  std::vector<std::string> keys;
  keys.push_back("1");
  keys.push_back("2");
  std::vector<double> frequencies;
  frequencies.push_back(0.85);
  frequencies.push_back(0.1);
  TopKFeature feature(keys, frequencies);
  std::string key = "blah";
  std::string id  = "top2";
  featureMap.updateInsert(key, id, feature);
  string str = "top2.value(0) + top2.value(1) > 0.9";
  Expression<FilterGrammar<std::string::const_iterator>> expression(str);
  BOOST_CHECK_EQUAL(expression.evaluate(key, featureMap), 1);*/
}

/**
 * This checks that expression.evaluate throws an exception when there
 * is not the necessary data in the feature map.
 */
BOOST_AUTO_TEST_CASE ( empty_item )
{
/*  FeatureMap featureMap;
  std::string key = "blah";
  string str = "top2.value(0) + top2.value(1) > 0.9";
  Expression<FilterGrammar<std::string::const_iterator>> expression(str);
  BOOST_CHECK_THROW(expression.evaluate(key, featureMap), std::logic_error);
*/
}


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

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

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

#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( shr_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::integer_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0  operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      using uitype = bd::as_integer_t<A0, unsigned>;
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shr : shift is out of range");
      return uitype(a0) >> a1;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( shr_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      using uitype = bd::as_integer_t<A0, unsigned>;
      return bitwise_cast<A0>(shr(bitwise_cast<uitype>(a0), a1));
    }
  };
} } }

#endif


#include <boost/hana.hpp>
#include <boost/mp11.hpp>
#include <boost/type_index.hpp>
#include <boost/variant.hpp>
#include <boost/variant/recursive_wrapper.hpp>
#include <cassert>
#include <iostream>
#include <iterator>
#include <string_view>
#include <type_traits>

#include <gpm/io.hpp>
#include "../examples/ant/common/nodes.hpp"

// namespace hana = boost::hana;
// using namespace hana::literals;  // contains the _c suffix

template <uint8_t kMaxHash, typename BeginIterType, typename EndIterType>
constexpr uint8_t simpleHash(BeginIterType begin, EndIterType end) {
  uint8_t r = 0;
  for (; begin != end; ++begin) {
    r = (r + 7) ^ *begin;
  }
  return r & (kMaxHash - 1);
}

template <typename RangeType>
constexpr uint8_t simpleHash(RangeType range) {
  return simpleHash(std::begin(range), std::end(range));
}

template <uint8_t kMaxHash, typename... T>
constexpr int checkForColision(boost::hana::tuple<T...>) {
  using hashes = boost::mp11::mp_list<std::integral_constant<
      uint8_t,
      simpleHash<kMaxHash>(std::begin(T::name), std::end(T::name) - 1)>...>;

  using unique_hashes = boost::mp11::mp_unique<hashes>;

  using size_hashes = boost::mp11::mp_size<hashes>;
  using size_unique_hashes = boost::mp11::mp_size<unique_hashes>;

  static_assert(
      std::is_same_v<size_hashes, size_unique_hashes>,
      "colision detected in hash function, please change hash function");
  return 0;
}

template <typename... T>
decltype(auto) variantToTuple(boost::variant<T...>) {
  return boost::hana::tuple<typename boost::unwrap_recursive<T>::type...>{};
}

template <typename VariantType, typename CursorType, uint8_t kMaxHash>
class FactoryV2 {
  using VariantTypeCreateFunction =
      std::add_pointer_t<VariantType(CursorType&)>;

  static std::array<VariantTypeCreateFunction, kMaxHash> makeHashToNode() {
    std::array<VariantTypeCreateFunction, kMaxHash> creatFunctionLUT{nullptr};
    auto nodesAsTuple = variantToTuple(VariantType{});
    checkForColision<kMaxHash>(nodesAsTuple);
    boost::hana::for_each(nodesAsTuple, [&creatFunctionLUT](auto n) {
      using NodeType = decltype(n);
      auto hash =
          simpleHash<kMaxHash>(std::begin(n.name), std::end(n.name) - 1);

      creatFunctionLUT[hash] = [](CursorType& tokenCursor) -> VariantType {
        auto node = NodeType{};
        for (auto& child : node.children) {
          tokenCursor.next();
          child = FactoryV2<VariantType, CursorType, kMaxHash>::factory(
              tokenCursor);
        }
        return node;
      };
    });
    return creatFunctionLUT;
  }

  template <typename TokenType>
  static VariantTypeCreateFunction getCreateFuntion(TokenType token) {
    static std::array<VariantTypeCreateFunction, kMaxHash> creatFunctionLUT =
        makeHashToNode();

    return creatFunctionLUT[simpleHash<kMaxHash>(token.begin(), token.end())];
  }

 public:
  static VariantType factory(CursorType& tokenCursor) {
    auto token = tokenCursor.token();
    return getCreateFuntion(token)(tokenCursor);
  }
};

// template <typename VariantType, typename CursorType>
// std::array<VariantTypeCreateFunction<VariantType, CursorType>, maxHash>
// HashToNode = makeHashToNode<VariantType, CursorType>(VariantType{});

//   template <class T>
//   void operator()(T) {
//     factoryMap[T::name] = [](Iter &tokenIter) {
//       T ret;
//       if constexpr (ret.children.size() != 0)
//         for (auto &n : ret.children) n =
//         factory_imp<VariantType>(++tokenIter); return ret;
//     };
//   }
//
//   template <class T>
//   void operator()(boost::recursive_wrapper<T>) {
//     factoryMap[T::name] = [](Iter &tokenIter) {
//       T ret;
//       if constexpr (ret.children.size() != 0)
//         for (auto &n : ret.children) n =
//         factory_imp<VariantType>(++tokenIter); return ret;
//     };
//   }
// };

// constexpr auto tup = hana::tuple<M, L, R, IF, P2, P3>{};

namespace {
namespace detail {

// constexpr auto checkForColisionV =  checkForColision(ant::NodesVariant);

}
}  // namespace

constexpr int ctpow(int base, int iexp) {
  return iexp == 0 ? 1 : base * ctpow(base, iexp - 1);
}

int main() {
  //   auto va = toVariant(tup);
  //
  //   boost::hana::for_each(tup, [](auto & n){
  //     std::cout << n.name << " " << std::hex << (int)cthash(n) << "\n";
  //   });
  //
  //   boost::apply_visitor([](auto & n){
  //     std::cout << n.name << "\n";
  //     return 0;
  //   }, hashToObject[cthash(Base<'p', '3'>{})]);
  // //   ant::Prog3 p3{};
  // //   auto at = asTuple(ant::NodesVariant{});
  // //   boost::hana::for_each(at, [](auto & n){
  // //     std::cout << n.name << " " << std::hex << (int)cthash(n.name,
  // std::end(n.name)-1) << "\n";
  // //   });
  // //   std::cout << std::hex << (int)cthash(p3.name, std::end(p3.name)-1) <<
  // "\n";

  gpm::RPNTokenCursor optAntIter{"m l m if l l p3 m if l p3 m if"};
  //   std::string_view token{"if"};
  //   VariantTypeCreateFunction<ant::NodesVariant, gpm::RPNTokenCursor> f =
  //   [](gpm::RPNTokenCursor&) -> ant::NodesVariant {return ant::Prog3{};};
  //   auto tmp = f(ttokenCursor);

  constexpr auto maxHash = ctpow(2, 4);
  auto optAnt =
      FactoryV2<ant::NodesVariant, gpm::RPNTokenCursor, maxHash>::factory(
          optAntIter);
  std::cout << boost::apply_visitor(gpm::RPNPrinter<std::string>{}, optAnt);
  //   boost::apply_visitor([](auto const & obj){
  //     std::cout << boost::typeindex::type_id<decltype(obj)>().pretty_name()
  //     << "\n";
  //   },
  //   foo
  //   );
  //
}


#include <iostream>
#include <dlib/dnn.h>
#include <dlib/data_io.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_io.h>
#include <dlib/opencv/cv_image.h>

#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;
using namespace dlib;

// This is a 6x6 convolutional layer that does 2x downsampling.
template <long num_filters, typename SUBNET> using con6d = con<num_filters,9,9,2,2,SUBNET>;

// This is a 3x3 convolutional layer that does not downsampling.
template <long num_filters, typename SUBNET> using con3  = con<num_filters,3,3,1,1,SUBNET>;

// This is a con6d 8x downsampling block as defined by con6d. ReLu is the prefered method
// as it only has to calculate on the positive returns of the function.

template <typename SUBNET> using downsampler  = relu<bn_con<con6d<32, relu<bn_con<con6d<32, relu<bn_con<con6d<32,SUBNET>>>>>>>>>;

// A 3x3 ReLu convolutional layer.
template <typename SUBNET> using rcon3  = relu<bn_con<con3<32,SUBNET>>>;

using net_type = loss_mmod<con<1,9,9,1,1,rcon3<rcon3<rcon3<downsampler<input_rgb_image_pyramid<pyramid_down<5>>>>>>>>;

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

VideoCapture *cap = new VideoCapture();

int main(int argc, char** argv) try
{
    cout << "Starting" << endl;

    if (argc != 3)
    {

        cout << "program.  For example, if you are in the examples folder then execute " << endl;
        cout << "this program by running: " << endl;
        cout << "   ./dnn_mmod_ex faces" << endl;
        cout << endl;
        return 0;
    }
    //const std::string faces_directory = argv[1];
    const std::string fileName = argv[1];
    const std::string netFile = argv[2];

    //std::vector<matrix<rgb_pixel>> images_test;

    //std::vector<std::vector<mmod_rect>> face_boxes_test;

    //load_image_dataset(images_test, face_boxes_test, faces_directory+"/testing.xml");

    net_type net;

    deserialize(netFile.c_str()) >> net;

    //cout << "testing results:  " << test_object_detection_function(net, images_test, face_boxes_test) << endl;

    image_window win;

    //cout << "Debug 1" << endl;

    cap->open(fileName);

    //cout << "Debug 2" << endl;

    int counter = 0;

    int max_frames = 4;

    while(1)
    {
      Mat frame;
        Mat frame_resize;
      // Capture frame-by-frame
        cap->operator>>(frame);

        cv::resize(frame, frame_resize, cv::Size(320,180));

      if ( counter >= max_frames)
      {

        cv_image<bgr_pixel> image(frame_resize);

        matrix<rgb_pixel> dlibImage;

        // dlib::array2d<rgb_pixel> dlibImage;
        assign_image(dlibImage, image);

        auto&& img = dlibImage;

        pyramid_up(img);

        auto dets = net(img);

        win.clear_overlay();

        win.set_image(img);

        for (auto&& d : dets)
        {
            win.add_overlay(d.rect);
            cout << "Debug "  << d.rect << endl;

        }

        counter = 0;
      }
      else
      {

          counter++;
      }
    }

/*
    for (auto&& img : images_test)
    {
        pyramid_up(img);
        auto dets = net(img);
        win.clear_overlay();
        win.set_image(img);
        for (auto&& d : dets)
            win.add_overlay(d);
        cin.get();
    }
    */
    return 0;

}
catch(std::exception& e)
{
    cout << e.what() << endl;
}