/**
 * \ file ButterworthFilter.cpp
 */

#include <ATK/EQ/ButterworthFilter.h>
#include <ATK/EQ/IIRFilter.h>

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

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

constexpr gsl::index PROCESSSIZE = 1024*64;

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_frequency_test )
{
  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  filter.set_cut_frequency(20);
  BOOST_CHECK_EQUAL(filter.get_cut_frequency(), 20);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_frequency_range_test )
{
  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_cut_frequency(0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_order_test )
{
  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  filter.set_order(3);
  BOOST_CHECK_EQUAL(filter.get_order(), 3);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_order_range_test )
{
  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_order(0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_frequency_test )
{
  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  filter.set_cut_frequency(20);
  BOOST_CHECK_EQUAL(filter.get_cut_frequency(), 20);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_frequency_range_test )
{
  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_cut_frequency(0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_order_test )
{
  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  filter.set_order(3);
  BOOST_CHECK_EQUAL(filter.get_order(), 3);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_order_range_test )
{
  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_order(0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_frequency_test )
{
  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  filter.set_cut_frequencies(20, 100);
  BOOST_CHECK_EQUAL(filter.get_cut_frequencies().first, 20);
  BOOST_CHECK_EQUAL(filter.get_cut_frequencies().second, 100);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_frequency_range_test )
{
  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_cut_frequencies(0, 100), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_frequency_range2_test )
{
  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_cut_frequencies(100, 0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_order_test )
{
  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  filter.set_order(3);
  BOOST_CHECK_EQUAL(filter.get_order(), 3);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_order_range_test )
{
  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_order(0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_frequency_test )
{
  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  filter.set_cut_frequencies(20, 100);
  BOOST_CHECK_EQUAL(filter.get_cut_frequencies().first, 20);
  BOOST_CHECK_EQUAL(filter.get_cut_frequencies().second, 100);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_frequency_range_test )
{
  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_cut_frequencies(0, 100), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_frequency_range2_test )
{
  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_cut_frequencies(100, 0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_order_test )
{
  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  filter.set_order(3);
  BOOST_CHECK_EQUAL(filter.get_order(), 3);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_order_range_test )
{
  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_order(0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.03158665365618605));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8408964151592208));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.011129047318919743));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthLowPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.3520674471793266));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 1));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE(IIRFilter_ButterworthHighPassCoefficients_1k_test_special)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024 * 64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 1));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024 * 64);

  checker.process(PROCESSSIZE);
  filter.process(1);
  filter.process(3);
  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8408964152940196));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 1));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthHighPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::ButterworthHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.9961319837713813));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.8406150184993316));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.27288445508133696));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.27185528281637));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::ButterworthBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.8408965558193944));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.8406817187144716));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.9981831475045972));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.9985753881639808));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ButterworthBandStopCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::ButterworthBandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.8408969221170415));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}


//
//  MosekProgram.cpp
//  Gravity
//
//  Created by Guanglei Wang on 14/7/17.
//
//
#include <Eigen/Dense>
#include "MosekProgram.h"
using namespace mosek;
using namespace monty;

MosekProgram::MosekProgram() {
    _mosek_model = new fusion::Model("noname");
    _mosek_model->setLogHandler([=](const std::string & msg){std::cout << msg << std::flush;});

}

MosekProgram::MosekProgram(Model* m) {
    _mosek_model = new fusion::Model("noname");
    _mosek_model->setLogHandler([=](const std::string & msg){std::cout << msg << std::flush;});
    _model = m;
}

MosekProgram::~MosekProgram() {
    _mosek_model->dispose();
}

void MosekProgram::update_model() {};

// remain to do
bool MosekProgram::solve(bool relax) {
//    _mosek_model->setSolverParam("log", 10);
    // Set max solution time
//    _mosek_model->setSolverParam("mioMaxTime", 360.0);
    // Set max relative gap (to its default value)
    _mosek_model->setSolverParam("intpntCoTolRelGap", 1e-6);
//    _mosek_model->setSolverParam("presolveUse", "on");
//    _mosek_model->setSolverParam("intpntCoTolPfeas", 1);
//    _mosek_model->setSolverParam("intpntQoTolPfeas", 1);
//    _mosek_model->setSolverParam("intpntCoTolMuRed", 1);
    // Set max absolute gap (to its default value)
//    _mosek_model->setSolverParam("mioTolRelGap", 1e-4);
//    _mosek_model->setSolverParam("mioTolAbsGap", 0.0);
//    _mosek_model->setSolverParam("numThreads",1);
    //if(!_output) _mosek_model->setSolverParam("log", 0);
    if(relax) {
        for (auto &vv: _mosek_vars)
            vv->makeContinuous();
    }
    _mosek_model->solve();

    if (!relax) {
        cout <<_mosek_model->getProblemStatus(fusion::SolutionType::Integer) << endl;
        cout << _mosek_model->getPrimalSolutionStatus() << endl;
    }
    else {
        cout <<_mosek_model->getProblemStatus(fusion::SolutionType::Interior) << endl;
        cout << _mosek_model->getPrimalSolutionStatus() << endl;
    }

    switch (_mosek_model->getPrimalSolutionStatus()) {
        case mosek::fusion::SolutionStatus::Undefined:
            _status = "Undefined";
            break;
        case mosek::fusion::SolutionStatus::Unknown:
            _status = "Unknown";
            break;
        case mosek::fusion::SolutionStatus::Optimal:
            _status = "Optimal";
            break;
        case mosek::fusion::SolutionStatus::NearOptimal:
            _status = "NearOptimal";
            break;
        case mosek::fusion::SolutionStatus::Feasible:
            _status = "Feasible";
            break;
        case mosek::fusion::SolutionStatus::NearFeasible:
            _status = "NearFeasible";
            break;
        case mosek::fusion::SolutionStatus::Certificate:
            _status = "Certificate";
            break;
        case mosek::fusion::SolutionStatus::NearCertificate:
            _status = "NearCertificate";
            break;
        case mosek::fusion::SolutionStatus::IllposedCert:
            _status = "IllposedCert";
            break;
        default:
            break;
    }
    DebugOn("Cost = " << _mosek_model->primalObjValue() << std::endl);

    // set the optimal value.

    _mosek_model->acceptedSolutionStatus(mosek::fusion::AccSolutionStatus::Feasible);
    _model->_obj_val = _mosek_model->primalObjValue();

    // Note that there is only one way to retrieve solutions, i.e.,
    // variable.level(). It only returns double. Thus, we need to cast the
    // solution to required types.
    //std::cout << "Solution = " << std::endl;
    // cout << "dim: " << _mosek_vars.size() << endl;
    //auto sol = _mosek_vars[0]->level();
    //mosek::fusion::Variable::t s= _mosek_vars[0];
    //cout << "size of s: " << s->size() << endl;

    for (auto i = 0; i < _mosek_vars.size(); i++) {
        auto sol = _mosek_vars[i]->level();
        if(i >= _model->_vars.size()) continue;
        if (_model->_vars[i]->get_intype()== binary_ ||_model->_vars[i]->get_intype()== integer_) {
            for (auto j = 0; j < _model->_vars[i]->get_nb_instances(); j++) {
                auto val = (*sol)[j];
                val = round(val);
                poly_set_val(j, val , _model->_vars[i]);
            }
        }
        else if(!_model->_vars[i]->_is_matrix){
            for (auto j = 0; j < _model->_vars[i]->get_nb_instances(); j++) {
                poly_set_val(j, (*sol)[j], _model->_vars[i]);
            }
        }
        else {
            int n = _model->_vars[i]->_dim[0];
            for(auto j1 = 0; j1 < n; j1++) {
                for(auto j2 = j1; j2 < n; j2++) {
                    string key = to_string(j1)+","+to_string(j2);
                    ((param<double>*)_model->_vars[i])->set_val(key, (*sol)[j1*n+j2]);
                }
            }

        }
    }
    return 0;
}

void MosekProgram::fill_in_mosek_vars() {
    param_* v;
    for(auto& v_p: _model->_vars)
    {
        v = v_p.second;
        if(v->get_id()==-1) {
            throw invalid_argument("Variable needs to be added to model first: use add_var(v) function:" + v->get_name());
        }
        switch (v->get_intype()) {
        case float_: {
            if (v->get_type() == var_c) {
                auto real_var = (var<float>*)v;
                //for (int i = 0; i < real_var->get_nb_instances(); i++) {
                //    _mosek_vars.push_back(_mosek_model->variable(real_var->get_name()+"_"+to_string(i),fusion::Domain::inRange(real_var->get_lb(i), real_var->get_ub(i))));
                //}
                auto lb  = new_array_ptr<double,1>(real_var->get_nb_instances());
                auto ub  = new_array_ptr<double,1>(real_var->get_nb_instances());
                for (int i = 0; i < real_var->get_nb_instances(); i++) {
                    (*lb)[i] = real_var->get_lb(i);
                    (*ub)[i] = real_var->get_ub(i);
                }
                if(!real_var->_in_q_cone && !real_var->_psd) _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), real_var->get_nb_instances(), fusion::Domain::inRange(lb,ub)));
                else if(real_var->_in_q_cone) _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), fusion::Domain::inQCone(real_var->get_nb_instances())));
                else {
//                    _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), fusion::Domain::inPSDCone((1 + sqrt(1+8*real_var->get_nb_instances()))/2)));
                    _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), fusion::Domain::inPSDCone(real_var->_dim[0])));
                }
            }
            else {
                auto sdp_var = (sdpvar<float>*)v;
                _mosek_vars.push_back(_mosek_model->variable(sdp_var->get_name(), fusion::Domain::inPSDCone(sdp_var->_symdim)));
            }
            break;
        }
        case long_: {
            if (v->get_type() == var_c) {
                auto real_var = (var<long double>*)v;
                //for (int i = 0; i < real_var->get_nb_instances(); i++) {
                //    _mosek_vars.push_back(_mosek_model->variable(real_var->get_name()+"_"+to_string(i),fusion::Domain::inRange(real_var->get_lb(i), real_var->get_ub(i))));
                //}
                auto lb  = new_array_ptr<double,1>(real_var->get_nb_instances());
                auto ub  = new_array_ptr<double,1>(real_var->get_nb_instances());
                for (int i = 0; i < real_var->get_nb_instances(); i++) {
                    (*lb)[i] = real_var->get_lb(i);
                    (*ub)[i] = real_var->get_ub(i);
                }
                if(!real_var->_in_q_cone)_mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), real_var->get_nb_instances(), fusion::Domain::inRange(lb,ub)));
                else _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), fusion::Domain::inQCone(real_var->get_nb_instances())));
            }
            else {
                auto sdp_var = (sdpvar<long double>*)v;
                _mosek_vars.push_back(_mosek_model->variable(sdp_var->get_name(), fusion::Domain::inPSDCone(sdp_var->get_nb_instances())));
            }
            break;
        }
        case double_: {
            if (v->get_type() == var_c) {
                auto real_var = (var<double>*)v;
                //for (int i = 0; i < real_var->get_nb_instances(); i++) {
                //    _mosek_vars.push_back(_mosek_model->variable(real_var->get_name()+"_"+to_string(i),fusion::Domain::inRange(real_var->get_lb(i), real_var->get_ub(i))));
                //}
                auto lb  = new_array_ptr<double,1>(real_var->get_nb_instances());
                auto ub  = new_array_ptr<double,1>(real_var->get_nb_instances());
                for (int i = 0; i < real_var->get_nb_instances(); i++) {
                    (*lb)[i] = real_var->get_lb(i);
                    (*ub)[i] = real_var->get_ub(i);
                }
                if(!real_var->_in_q_cone && !real_var->_psd) _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), real_var->get_nb_instances(), fusion::Domain::inRange(lb,ub)));
                else if(real_var->_in_q_cone) _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), fusion::Domain::inQCone(real_var->get_nb_instances())));
                else {
//                    _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), fusion::Domain::inPSDCone((1 + sqrt(1+8*real_var->get_nb_instances()))/2.)));
                    _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), fusion::Domain::inPSDCone(real_var->_dim[0])));
                }
            }
            else {
                auto sdp_var = (sdpvar<double>*)v;
                size_t num = sdp_var->_symdim;
                auto c  = _mosek_model->variable(sdp_var->get_name(), fusion::Domain::inPSDCone(num));
                _mosek_vars.push_back(c);
                std::cout << c->toString() << endl;

            }
            break;
        }
        case integer_: {
            if (v->get_type() == var_c) {
                auto real_var = (var<int>*)v;
                auto lb  = new_array_ptr<double,1>(real_var->get_nb_instances());
                auto ub  = new_array_ptr<double,1>(real_var->get_nb_instances());
                for (int i = 0; i < real_var->get_nb_instances(); i++) {
                    (*lb)[i] = real_var->get_lb(i);
                    (*ub)[i] = real_var->get_ub(i);
                }
                _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), real_var->get_nb_instances(), fusion::Domain::integral(fusion::Domain::inRange(lb,ub))));
            }
            else {
                auto sdp_var = (sdpvar<int>*)v;
                _mosek_vars.push_back(_mosek_model->variable(sdp_var->get_name(), fusion::Domain::inPSDCone(sdp_var->get_nb_instances())));
            }
            break;
        }
        case short_: {
            if (v->get_type() == var_c) {
                auto real_var = (var<short>*)v;
                auto lb  = new_array_ptr<double,1>(real_var->get_nb_instances());
                auto ub  = new_array_ptr<double,1>(real_var->get_nb_instances());
                for (int i = 0; i < real_var->get_nb_instances(); i++) {
                    (*lb)[i] = real_var->get_lb(i);
                    (*ub)[i] = real_var->get_ub(i);
                }
                _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), real_var->get_nb_instances(), fusion::Domain::integral(fusion::Domain::inRange(lb,ub))));
            }
            else {
                auto sdp_var = (sdpvar<short>*)v;
                _mosek_vars.push_back(_mosek_model->variable(sdp_var->get_name(), fusion::Domain::inPSDCone(sdp_var->get_nb_instances())));
            }
            break;
        }
        case binary_: {
            if (v->get_type() == var_c) {
                auto real_var = (var<bool>*)v;
                //for (unsigned int i = 0; i < real_var->get_nb_instances(); i++) {
                //    _mosek_vars.push_back(_mosek_model->variable(real_var->get_name()+"_"+to_string(i),fusion::Domain::binary()));
                //}
                _mosek_vars.push_back(_mosek_model->variable(real_var->get_name(), real_var->get_nb_instances(), fusion::Domain::binary()));
            }
            else {
                auto sdp_var = (sdpvar<bool>*)v;
                _mosek_vars.push_back(_mosek_model->variable(sdp_var->get_name(), fusion::Domain::inPSDCone(sdp_var->get_nb_instances())));
            }
            break;
        }
        default:
            break;
        }
    }
}

/*void MosekProgram::create_mosek_constraints() {
    //size_t idx = 0;
    //size_t nb_inst = 0;
    size_t idx = 0, idx_inst = 0,idx1 = 0, idx2 = 0, idx_inst1 = 0, idx_inst2 = 0, nb_inst = 0, inst = 0;
    size_t c_idx_inst = 0;
    shared_ptr<Constraint> c;
    for(auto& p: _model->_cons) {
        c = p.second;
//        c->print();
        if (c->is_nonlinear()) {
            cout <<  "We haven't implemented quadratic expressions interface for mosek" << endl;
            throw invalid_argument("Mosek cannot handle nonlinear constraints that are not convex quadratic.\n");
        }
        nb_inst = c->get_nb_instances();
        auto fusion_sums = new ndarray<fusion::Expression::t,1>(shape(nb_inst));
        for (int i = 0; i< nb_inst; i++) {
            // get constant part first.
            monty::rc_ptr<mosek::fusion::Expression > expr= fusion::Expr::constTerm(poly_eval(c->get_cst())); // expr is a pointer to the Expression.
            for (auto& it1: c->get_lterms()) {
                //idx = it1.second._p->get_id();
                idx = it1.second._p->get_vec_id();
                //cout << "get_id: " << it1.second._p->get_id() << endl;
                //cout << "get_vec_id: " << it1.second._p->get_vec_id() << endl;

                CType vartype = it1.second._p->get_type();
                if (vartype == var_c) {
                    if(!it1.second._p->_is_matrix) {

                        if (it1.second._coef->_is_transposed) {
                            auto coefs = new_array_ptr<double, 1>(it1.second._p->get_nb_instances());
                            for (int j = 0; j < it1.second._p->get_nb_instances(); j++) {
                                (*coefs)(j) = poly_eval(it1.second._coef, j);
                            }
                            expr = fusion::Expr::add(expr, fusion::Expr::dot(coefs, _mosek_vars[idx]));
                            //    cout << "expr" << expr->toString() << endl;
                        } else {
                            if (is_indexed(it1.second._p)) {
                                idx_inst = it1.second._p->get_id_inst(i);
                            } else {
                                idx_inst = inst;
                            }
                            if (is_indexed(it1.second._coef)) {
                                c_idx_inst = get_poly_id_inst(it1.second._coef);
                            } else {
                                c_idx_inst = inst;
                            }
                            auto lterm = fusion::Expr::mul(poly_eval(it1.second._coef, c_idx_inst),
                                                           _mosek_vars[idx]->index(idx_inst));

                            if (!it1.second._sign) {
                                lterm = fusion::Expr::mul(-1, lterm);
                            }
                            expr = fusion::Expr::add(expr, lterm);
                        }
                    }else{
                        if (it1.second._coef->_is_transposed) {
                            auto coefs = new_array_ptr<double, 1>(it1.second._p->get_nb_instances());
                            for (int j = 0; j < it1.second._p->get_nb_instances(); j++) {
                                (*coefs)(j) = poly_eval(it1.second._coef, j);
                            }
                            expr = fusion::Expr::add(expr, fusion::Expr::dot(coefs, _mosek_vars[idx]));
                            //    cout << "expr" << expr->toString() << endl;
                        } else {
                            pair<size_t, size_t>  pair;
                            if (is_indexed(it1.second._p)) {
//                                idx_inst = it1.second._p->get_id_inst(i);
                                pair = it1.second._p->get_sdp_inst(i);
                            } else {
                                idx_inst = inst;
                            }
                            if (is_indexed(it1.second._coef)) {
                                c_idx_inst = get_poly_id_inst(it1.second._coef);
                            } else {
                                c_idx_inst = inst;
                            }
                            DebugOff("\nindex: " << pair.first << ", " << pair.second);
                            auto lterm = fusion::Expr::mul(poly_eval(it1.second._coef, c_idx_inst),
                                                           _mosek_vars[idx]->index(pair.first,pair.second));

                            if (!it1.second._sign) {
                                lterm = fusion::Expr::mul(-1, lterm);
                            }
                            expr = fusion::Expr::add(expr, lterm);
                        }
                    }

                }
                else if(vartype == sdpvar_c) {
                    if (it1.second._coef->_is_transposed) {
                        auto coefs = new_array_ptr<double,1>(it1.second._p->get_nb_instances());
                        for (int j = 0; j<it1.second._p->get_nb_instances(); j++) {
                            (*coefs)(j) = poly_eval(it1.second._coef,j);
                        }
                        expr = fusion::Expr::add(expr,fusion::Expr::dot(coefs,_mosek_vars[idx]));
                    }
                    else {
                        pair<size_t, size_t>  pair = make_pair(0, 0);

                        if (is_indexed(it1.second._p)) {
                            pair = it1.second._p->get_sdpid();
                        }

                        if (is_indexed(it1.second._coef)) {
                            c_idx_inst = get_poly_id_inst(it1.second._coef);
                        }
                        else {
                            c_idx_inst = inst;
                        }
                        auto lterm = fusion::Expr::mul(poly_eval(it1.second._coef, c_idx_inst), _mosek_vars[idx]->index(pair.first, pair.second));
                        if (!it1.second._sign) {
                            lterm = fusion::Expr::mul(-1, lterm);
                        }
                        expr = fusion::Expr::add(expr,lterm);
                    }

                }else{ //param
                    if (it1.second._coef->_is_transposed) {
                        auto coefs = new_array_ptr<double,1>(it1.second._p->get_nb_instances());
                        for (int j = 0; j<it1.second._p->get_nb_instances(); j++) {
                            (*coefs)(j) = poly_eval(it1.second._coef,j);
                        }
                        expr = fusion::Expr::add(expr,fusion::Expr::dot(coefs,_mosek_vars[idx]));
                        //    cout << "expr" << expr->toString() << endl;
                    }
                    else { //this results in always taking the first instance? it doesn't use i at all?
                        if (is_indexed(it1.second._p)) {
                            idx_inst = it1.second._p->get_id_inst(i); //fixed here, look for similar errors
                        }
                        else {
                            idx_inst = inst;
                        }
                        if (is_indexed(it1.second._coef)) {
                            c_idx_inst = get_poly_id_inst(it1.second._coef);
                        }
                        else {
                            c_idx_inst = inst;
                        }
                        auto lterm = fusion::Expr::constTerm(poly_eval(it1.second._coef, c_idx_inst) * poly_eval(it1.second._p, i)); // was idx_inst
                        if (!it1.second._sign) {
                            lterm = fusion::Expr::mul(-1, lterm);
                        }
                        expr = fusion::Expr::add(expr,lterm);
                    }
                }
            }

            DebugOff("\nconstr in Mosek: " << expr->toString());
            if(c->get_type()==geq) {
                DebugOff("\n >= " << c->get_rhs());
                _mosek_model->constraint(c->get_name()+to_string(i), expr, fusion::Domain::greaterThan(c->get_rhs()));
            }
            else if(c->get_type()==leq) {
                DebugOff("\n <= " << c->get_rhs());
                _mosek_model->constraint(c->get_name()+to_string(i), expr, fusion::Domain::lessThan(c->get_rhs()));
            }
            else if(c->get_type()==eq) {
                DebugOff("\n" << expr->toString());
                DebugOff(" = " << c->get_rhs());
                _mosek_model->constraint(c->get_name()+to_string(i), expr, fusion::Domain::equalsTo(c->get_rhs()));
            }
            inst++;
        }
    }
}*/

size_t get_num_qterms(map<string, qterm>& qterms) {
    size_t qn = 0;
    for (auto& it1: qterms) {
        if ((it1.second._coef->_is_transposed || it1.second._coef->_is_matrix) && !it1.second._p->first->_is_matrix) {
            size_t dim = it1.second._p->first->get_nb_instances();
            qn += dim;
        }
        else if(it1.second._p->first->_is_transposed){
            size_t dim = it1.second._p->first->get_nb_instances();
            qn += dim;
        }
        else qn++;
    }
    return qn;
}

fusion::Expression::t MosekProgram::create_lin_expr(map<string, lterm>& lt, constant_* cst, size_t inst){
    size_t idx = 0, idx_inst = 0, c_idx_inst = 0;
    auto fusion_lterms = new_array_ptr<fusion::Expression::t,1>(lt.size()+1);
    (*fusion_lterms)[0] = fusion::Expr::constTerm(poly_eval(cst,inst));
    int lterm_idx = 1;

    for (auto& it1: lt) {
        idx = it1.second._p->get_vec_id();

        CType vartype = it1.second._p->get_type();
        if (vartype == var_c) {
//                    cout << "\nvar = " << it1.second._p->get_name();
//                    cout << "\nfull Mosek var: " << _mosek_vars[idx]->toString();
            if(!it1.second._p->_is_matrix) {
                if (it1.second._coef->_is_transposed) {
                    auto coefs = new_array_ptr<double, 1>(it1.second._p->get_nb_instances(inst));
                    auto vars = new_array_ptr<fusion::Variable::t,1>(it1.second._p->get_nb_instances(inst));
                    for (int j = 0; j < it1.second._p->get_nb_instances(inst); j++) {
                        (*coefs)(j) = poly_eval(it1.second._coef,inst,j);
                        idx_inst = it1.second._p->get_id_inst(inst,j);
//                                if(idx_inst >= _mosek_vars[idx]->size()){
//                                    cout << "\nidx_inst >= var size: idx_inst = " << idx_inst << ", var size = " << _mosek_vars[idx]->size();
//                                }
                        (*vars)(j) = _mosek_vars[idx]->index(idx_inst);
//                                cout << "\nj, idx, idx_inst = " << j << ", " << idx << ", " << idx_inst << ", var = " <<  _mosek_vars[idx]->index(idx_inst)->toString();
                    }
                    rc_ptr<fusion::Variable> P;
                    if(it1.second._p->get_nb_instances(inst)!=1) P = fusion::Var::vstack(vars);
                    else P = _mosek_vars[idx]->index(idx_inst);
//                            cout << "\nCoef = ";
//                            for(auto& coef: *coefs)
//                                cout << coef << "; ";
//                            cout << "\nP = " << P->toString();
                    auto lterm = fusion::Expr::dot(coefs, P);
                    if (!it1.second._sign) lterm = fusion::Expr::mul(-1, lterm);
                    (*fusion_lterms)[lterm_idx] = lterm;
                    //expr = fusion::Expr::add(expr, fusion::Expr::dot(coefs, _mosek_vars[idx]));
                } else {
                    if (is_indexed(it1.second._p)) idx_inst = it1.second._p->get_id_inst(inst);
                    else idx_inst = inst;

                    if (is_indexed(it1.second._coef)) c_idx_inst = get_poly_id_inst(it1.second._coef);
                    else c_idx_inst = inst;

                    auto lterm = fusion::Expr::mul(poly_eval(it1.second._coef, inst), //was c_idx_inst instead of i
                                                   _mosek_vars[idx]->index(idx_inst));

                    if (!it1.second._sign) lterm = fusion::Expr::mul(-1, lterm);
                    (*fusion_lterms)[lterm_idx] = lterm;
                    //expr = fusion::Expr::add(expr, lterm);
                }
            }else{
                if (it1.second._coef->_is_transposed) {
                    auto coefs = new_array_ptr<double, 1>(it1.second._p->get_nb_instances());
                    for (int j = 0; j < it1.second._p->get_nb_instances(); j++) {
                        (*coefs)(j) = poly_eval(it1.second._coef, j);
                    }
                    (*fusion_lterms)[lterm_idx] = fusion::Expr::dot(coefs, _mosek_vars[idx]);
                    //expr = fusion::Expr::add(expr, fusion::Expr::dot(coefs, _mosek_vars[idx]));
                } else {
                    pair<size_t, size_t>  pair;
                    if (is_indexed(it1.second._p)) {
//                                idx_inst = it1.second._p->get_id_inst(i);
                        pair = it1.second._p->get_sdp_inst(inst);
                    } else idx_inst = inst;

                    if (is_indexed(it1.second._coef)) c_idx_inst = get_poly_id_inst(it1.second._coef);
                    else c_idx_inst = inst;

                    DebugOff("\nindex: " << pair.first << ", " << pair.second);
                    DebugOff("\nMosek var:" << _mosek_vars[idx]->toString());

                    auto lterm = fusion::Expr::mul(poly_eval(it1.second._coef, c_idx_inst),
                                                   _mosek_vars[idx]->index(pair.first,pair.second));

                    if (!it1.second._sign) lterm = fusion::Expr::mul(-1, lterm);
                    (*fusion_lterms)[lterm_idx] = lterm;
                    //expr = fusion::Expr::add(expr, lterm);
                }
            }

        }
        else{ //param
            if (it1.second._coef->_is_transposed) {
                auto coefs = new_array_ptr<double,1>(it1.second._p->get_nb_instances());
                for (int j = 0; j<it1.second._p->get_nb_instances(); j++) {
                    (*coefs)(j) = poly_eval(it1.second._coef,j);
                }
                (*fusion_lterms)[lterm_idx] = fusion::Expr::dot(coefs,_mosek_vars[idx]);
                //expr = fusion::Expr::add(expr,fusion::Expr::dot(coefs,_mosek_vars[idx]));
            }
            else { //this results in always taking the first instance? it doesn't use i at all?
                if (is_indexed(it1.second._p)) idx_inst = it1.second._p->get_id_inst(inst); //fixed here, look for similar errors
                else idx_inst = inst;

                if (is_indexed(it1.second._coef)) c_idx_inst = get_poly_id_inst(it1.second._coef);
                else c_idx_inst = inst;

                auto lterm = fusion::Expr::constTerm(poly_eval(it1.second._coef, c_idx_inst) * poly_eval(it1.second._p, inst)); // was idx_inst
                if (!it1.second._sign) lterm = fusion::Expr::mul(-1, lterm);

                (*fusion_lterms)[lterm_idx] = lterm;
//                        expr = fusion::Expr::add(expr,lterm);
            }
        }
        lterm_idx++;
    }
    fusion::Expression::t res;
    res = fusion::Expr::add(std::shared_ptr<ndarray<fusion::Expression::t,1>>(fusion_lterms));
    return res;
}

//TODO: check use inst
fusion::Expression::t MosekProgram::form_Fx(map<string, qterm>& qterms, size_t qn, size_t inst){
    size_t aidx = 0, idx1 = 0, idx2 = 0, idx_inst = 0;

    //create a var map
    map<pair<size_t,size_t>,int> qvars;
    int idx_in_A = 0;
    for(auto& it1: qterms) {
        idx1 = it1.second._p->first->get_vec_id();
        idx2 = it1.second._p->second->get_vec_id();
        if ((it1.second._coef->_is_transposed || it1.second._coef->_is_matrix) && !it1.second._p->first->_is_matrix) {
            auto dim = it1.second._p->first->get_nb_instances(inst);
            for (int j = 0; j < dim; j++) {
                idx_inst = it1.second._p->first->get_id_inst(inst, j);
                pair<size_t, size_t> p = make_pair(idx1, idx_inst);
                if (qvars.find(p) == qvars.end()) {
                    qvars.insert(make_pair(p, idx_in_A));
                    idx_in_A++;
                }
                idx_inst = it1.second._p->second->get_id_inst(inst, j);
                p = make_pair(idx2, idx_inst);
                if (qvars.find(p) == qvars.end()) {
                    qvars.insert(make_pair(p, idx_in_A));
                    idx_in_A++;
                }
            }
        } else if (it1.second._p->first->_is_transposed) {
//                auto dim = _p->first->get_nb_instances(i);
//                for (int j = 0; j<dim; j++)
//                    res += poly_eval(_coef,i,j) * poly_eval(_p->first, i,j)* poly_eval(_p->second, i,j);
        } else {
            idx_inst = it1.second._p->first->get_id_inst(inst);
            pair<size_t, size_t> p = make_pair(idx1, idx_inst);
            if (qvars.find(p) == qvars.end()) {
                qvars.insert(make_pair(p, idx_in_A));
                idx_in_A++;
            }
            idx_inst = it1.second._p->second->get_id_inst(inst);
            p = make_pair(idx2, idx_inst);
            if (qvars.find(p) == qvars.end()) {
                qvars.insert(make_pair(p, idx_in_A));
                idx_in_A++;
            }
        }
    }
    auto qvars_arr = new_array_ptr<fusion::Variable::t,1>(qvars.size());
    for(auto& it1: qvars) {
        (*qvars_arr)[it1.second] = _mosek_vars[it1.first.first]->index(it1.first.second);
        DebugOff("\nqvar with index " << it1.second << " is (" << it1.first.first << ", " << it1.first.second << ")");
        DebugOff("\ncorresp mosek vars are " << _mosek_vars[it1.first.first]->index(it1.first.second)->toString());
    }

    Eigen::MatrixXf A = Eigen::MatrixXf::Zero(qvars.size(),qvars.size());
    fusion::Variable::t xi = fusion::Var::vstack(qvars_arr);
//    auto qvarsi = new_array_ptr<fusion::Variable::t,1>(qn);

    size_t j2 = 0;
    for (auto& it1: qterms) {
        double sign;
        if(!it1.second._sign) sign = -2;
        else sign = 2;
        idx1 = it1.second._p->first->get_vec_id();
        idx2 = it1.second._p->second->get_vec_id();
        if ((it1.second._coef->_is_transposed || it1.second._coef->_is_matrix) && !it1.second._p->first->_is_matrix) {
            throw invalid_argument("This type of expression in the constraint is not implemented.");
//            auto dim = it1.second._p->first->get_nb_instances(inst);
//            auto qvarsi = new_array_ptr<fusion::Variable::t,1>(dim);
//            for (int j = 0; j<dim; j++) {
////                    res += poly_eval(_coef,i,j) * poly_eval(_p->first, i,j)* poly_eval(_p->second, i,j);
//                if(it1.second._p->first->get_id_inst(inst,j) != it1.second._p->second->get_id_inst(inst,j))
//                    throw invalid_argument("Bilinear expressions in Mosek objective are not implemented.");
//                A(aidx,aidx) = sign*poly_eval(it1.second._coef,inst,j);
//                aidx++;
//                idx_inst = it1.second._p->first->get_id_inst(inst,j);
////                (*qvarsi)(j) = _mosek_vars[idx1]->index(idx_inst);
//            }
//            xi = fusion::Var::vstack(qvarsi);
        }
        else if(it1.second._p->first->_is_transposed){
//                auto dim = _p->first->get_nb_instances(i);
//                for (int j = 0; j<dim; j++)
//                    res += poly_eval(_coef,i,j) * poly_eval(_p->first, i,j)* poly_eval(_p->second, i,j);
            throw invalid_argument("This type of expression in the constraint is not implemented.");
        }
        else {
//                res = poly_eval(_coef,i) * poly_eval(_p->first, i) * poly_eval(_p->second, i);
            idx_inst = it1.second._p->first->get_id_inst(inst);
            int aidx1 = qvars.find(make_pair(idx1,idx_inst))->second;
            DebugOff("\nvars: " << _mosek_vars[idx1]->index(idx_inst)->toString());
            idx_inst = it1.second._p->second->get_id_inst(inst);
            int aidx2 = qvars.find(make_pair(idx2,idx_inst))->second;
            DebugOff(", " << _mosek_vars[idx2]->index(idx_inst)->toString());
            if(aidx1==aidx2) A(aidx1,aidx2) = sign*poly_eval(it1.second._coef,inst);
            else {
                A(aidx1,aidx2) = 0.5*sign*poly_eval(it1.second._coef,inst);
                A(aidx2,aidx1) = 0.5*sign*poly_eval(it1.second._coef,inst);
            }
//            aidx++;

//            (*qvarsi)(j2) = _mosek_vars[idx1]->index(idx_inst);
//            j2++;
        }
//            if (!it1.second._sign) res *= -1;
    }
    DebugOff("\nA = \n" << A);
    DebugOff("\nx = " << xi->toString());

    Eigen::LLT<Eigen::MatrixXf> lltOfA(A); // compute the Cholesky decomposition of A
    Eigen::MatrixXf L = lltOfA.matrixL();
//    Eigen::MatrixXf L = A.sqrt();
    DebugOff("\nL: " << L);

    std::shared_ptr<ndarray<double, 1>> Farr = new_array_ptr<double,1>(qn*qn);
    for(size_t Fi = 0; Fi < L.rows(); Fi++) {
        for(size_t Fj = 0; Fj < L.cols(); Fj++) {
            (*Farr)[Fi*(L.cols())+Fj] = L(Fi,Fj);
        }
    }
    fusion::Matrix::t F = fusion::Matrix::dense(qvars.size(),qvars.size(),Farr);
    DebugOff("\nF = " << F->toString());
    fusion::Expression::t Fx = fusion::Expr::mul(F, xi);
    DebugOff("\nFx = " << Fx->toString());

    return Fx;
}

void MosekProgram::create_mosek_constraints() {
    size_t idx = 0, idx_inst = 0, idx1 = 0, idx2 = 0, idx_inst1 = 0, idx_inst2 = 0, nb_inst = 0, inst = 0;
    size_t c_idx_inst = 0;
    shared_ptr<Constraint> c;
    for(auto& p: _model->_cons) {
        c = p.second;
        DebugOff("\nconstr " << c->get_name());
        if(c->is_nonlinear() && !c->is_quadratic())
            throw invalid_argument("Mosek cannot handle nonlinear constraints that are not convex quadratic.\n");

        nb_inst = c->get_nb_instances();

        if (c->is_quadratic()) {
            size_t qn = get_num_qterms(c->get_qterms());
            auto fusion_cols = new_array_ptr<fusion::Expression::t,1>(nb_inst);
            for (int i = 0; i< nb_inst; i++) {
                //create matrix F;
                fusion::Expression::t Fx = form_Fx(c->get_qterms(),qn,i);

                //build the linear expression
                fusion::Expression::t lin_expr_i = create_lin_expr(c->get_lterms(), c->get_cst(), i);

                //arrange it all into a row, add the row to an array
                auto Earr = new_array_ptr<fusion::Expression::t,1>(2 + qn);
                (*Earr)[0] = fusion::Expr::constTerm(1);
                (*Earr)[1] = fusion::Expr::mul(lin_expr_i,-1);
                for(size_t Fxi = 0; Fxi < qn; Fxi++) {
                    (*Earr)[Fxi+2] = Fx->index(Fxi);
                }
                fusion::Expression::t qexpr = fusion::Expr::hstack(Earr);
                DebugOff("\nqexpr = " << qexpr->toString());

                (*fusion_cols)[i] = qexpr;
            }

            // use the array of rows to create a matrix
            auto M = fusion::Expr::vstack(fusion_cols);
            DebugOff("\nM = " << M->toString());

            // add a conic constraint
            auto mosek_constr = _mosek_model->constraint(c->get_name(), M, fusion::Domain::inRotatedQCone(nb_inst,2+qn));
            DebugOff("\nConstraint generated: " << mosek_constr->toString());
        } // quadratic expression
        else{
            auto fusion_sums = new_array_ptr<fusion::Expression::t, 1>(nb_inst);
            for (int i = 0; i < nb_inst; i++) {
                fusion::Expression::t lin_expr = create_lin_expr(c->get_lterms(), c->get_cst(), i);
                (*fusion_sums)[i] = lin_expr;
                inst++;
            }
            auto E = fusion::Expr::vstack(fusion_sums);

            DebugOff("\nconstr " << c->get_name() << " in Mosek: " << E->toString());
            if (c->get_type() == geq) {
                DebugOff("\n >= " << c->get_rhs());
                _mosek_model->constraint(c->get_name(), E, fusion::Domain::greaterThan(c->get_rhs()));
            } else if (c->get_type() == leq) {
                DebugOff("\n <= " << c->get_rhs());
                _mosek_model->constraint(c->get_name(), E, fusion::Domain::lessThan(c->get_rhs()));
            } else if (c->get_type() == eq) {
                DebugOff(" = " << c->get_rhs());
                _mosek_model->constraint(c->get_name(), E, fusion::Domain::equalsTo(c->get_rhs()));
            }
        } // linear expression
    } //for(p: _model->_cons)
}

fusion::Expression::t MosekProgram::form_Fx(map<string, qterm>& qterms, size_t qn){
    size_t aidx = 0, idx, idx_inst;
    Eigen::MatrixXf A = Eigen::MatrixXf::Zero(qn,qn);
    fusion::Variable::t xi;
    for (auto& it1: qterms) {
        double sign;
        if(!it1.second._sign) sign = -2;
        else sign = 2;
        idx = it1.second._p->first->get_vec_id();
        if ((it1.second._coef->_is_transposed || it1.second._coef->_is_matrix) && !it1.second._p->first->_is_matrix) {
            auto dim = it1.second._p->first->get_nb_instances();
            auto qvarsi = new_array_ptr<fusion::Variable::t,1>(dim);
            for (int j = 0; j<dim; j++) { //now only supports quadratic (not bilinear) terms
//                    res += poly_eval(_coef,i,j) * poly_eval(_p->first, i,j)* poly_eval(_p->second, i,j);
                if(it1.second._p->first->get_id_inst(j) != it1.second._p->second->get_id_inst(j))
                    throw invalid_argument("Bilinear expressions in Mosek objective are not implemented.");
                A(aidx,aidx) = sign*poly_eval(it1.second._coef,0,j);
                aidx++;
                idx_inst = it1.second._p->first->get_id_inst(j);
                (*qvarsi)(j) = _mosek_vars[idx]->index(idx_inst);
            }
            xi = fusion::Var::vstack(qvarsi);
//                (*qvars)(qtermi) = xi; qtermi++;
        }
        else if(it1.second._p->first->_is_transposed){
//                auto dim = _p->first->get_nb_instances(i);
//                for (int j = 0; j<dim; j++)
//                    res += poly_eval(_coef,i,j) * poly_eval(_p->first, i,j)* poly_eval(_p->second, i,j);
            throw invalid_argument("This type of expression in the objective is not implemented.");
        }
        else {
//                res = poly_eval(_coef,i) * poly_eval(_p->first, i) * poly_eval(_p->second, i);
            throw invalid_argument("This type of expression in the objective is not implemented.");
        }
//            if (!it1.second._sign) res *= -1;
    }
    DebugOff("\nA = " << A);
    DebugOff("\nx = " << xi->toString());

    Eigen::LLT<Eigen::MatrixXf> lltOfA(A); // compute the Cholesky decomposition of A
    Eigen::MatrixXf L = lltOfA.matrixL();
    DebugOff("\nL: " << L);

    std::shared_ptr<ndarray<double, 1>> Farr = new_array_ptr<double,1>(qn*qn);
    for(size_t Fi = 0; Fi < L.rows(); Fi++) {
        for(size_t Fj = 0; Fj < L.cols(); Fj++) {
            (*Farr)[Fi*(L.cols())+Fj] = L(Fi,Fj);
        }
    }
    fusion::Matrix::t F = fusion::Matrix::dense(qn,qn,Farr);
    DebugOff("\nF = " << F->toString());
    fusion::Expression::t Fx = fusion::Expr::mul(F, xi);
    DebugOff("\nFx = " << Fx->toString());

    return Fx;
}

void MosekProgram::set_mosek_objective() {
    size_t idx = 0, idx_inst = 0, idx1 = 0, idx2 = 0, idx_inst1 = 0, idx_inst2 = 0, qn;
    size_t c_idx_inst = 0;
    // initialize with the constant part.
    monty::rc_ptr< ::mosek::fusion::Expression >  expr= fusion::Expr::constTerm(poly_eval(_model->_obj.get_cst())); // expr is a pointer to the Expression.
    if(_model->_obj.get_qterms().empty() == false){
        //cerr << "\nMosek doesn't support quadratic objectives!\n";
        qn = get_num_qterms(_model->_obj.get_qterms());

        _mosek_vars.push_back(_mosek_model->variable("r_obj", 1, fusion::Domain::unbounded()));
        //obj = r + (linear part) (see Mosek modelling cookbook)
        expr = fusion::Expr::add(expr,_mosek_vars[_mosek_vars.size()-1]);
        //auto qvars = new_array_ptr<fusion::Variable::t,1>(_model->_obj.get_qterms().size());
        fusion::Expression::t Fx = form_Fx(_model->_obj.get_qterms(),qn);

        auto Earr = new_array_ptr<fusion::Expression::t,1>(2 + qn);
        (*Earr)[0] = fusion::Expr::constTerm(1);
        (*Earr)[1] = _mosek_vars[_mosek_vars.size()-1]->asExpr();
        for(size_t Fxi = 0; Fxi < qn; Fxi++) {
            (*Earr)[Fxi+2] = Fx->index(Fxi);
        }
        fusion::Expression::t qexpr = fusion::Expr::vstack(Earr);
        DebugOn("\nqexpr = " << qexpr->toString());

        //(1,r,Fx) in Qr
        _mosek_model->constraint(qexpr, fusion::Domain::inRotatedQCone());
    }
    for (auto& it1: _model->_obj.get_lterms()) {
        //idx = it1.second._p->get_id();
        idx = it1.second._p->get_vec_id();
        CType vartype = it1.second._p->get_type();
        if (vartype == var_c) {
            if (it1.second._coef->_is_transposed) {
                auto coefs = new_array_ptr<double,1>((it1.second._p->get_nb_instances()));
                for (int j = 0; j<it1.second._p->get_nb_instances(); j++) {
                    (*coefs)(j)=poly_eval(it1.second._coef, j);
                }
                expr = fusion::Expr::add(expr,fusion::Expr::dot(coefs,_mosek_vars[idx]));
            }
            else {
                // get pos.
                idx_inst = it1.second._p->get_id_inst();
                c_idx_inst = get_poly_id_inst(it1.second._coef);
                //auto t = _mosek_vars[idx]->index
                auto lterm = fusion::Expr::mul(poly_eval(it1.second._coef, c_idx_inst), _mosek_vars[idx]->index(idx_inst));
                if (!it1.second._sign) {
                    lterm = fusion::Expr::mul(-1, lterm);
                }
                expr = fusion::Expr::add(expr,lterm);
            }
        }
        else {
            // sdpvar_c
            if (it1.second._coef->_is_transposed) {
                auto coefs = new_array_ptr<double,1>((it1.second._p->get_nb_instances()));
                for (int j = 0; j<it1.second._p->get_nb_instances(); j++) {
                    (*coefs)(j)=poly_eval(it1.second._coef, j);
                }
                expr = fusion::Expr::add(expr,fusion::Expr::dot(coefs,_mosek_vars[idx]));
            }
            else {
                // retrive the index of the parameter.
                // This is quite different from the general variable definition.
                pair<size_t, size_t>  pair = it1.second._p->get_sdpid();
                c_idx_inst = get_poly_id_inst(it1.second._coef);
                auto lterm = fusion::Expr::mul(poly_eval(it1.second._coef, c_idx_inst), _mosek_vars[idx]->index(pair.first,pair.second));

                if (!it1.second._sign) {
                    lterm = fusion::Expr::mul(-1, lterm);
                }
                expr = fusion::Expr::add(expr,lterm);
            }
        }
    }
    if (_model->_objt == maximize) {
        _mosek_model->objective("obj", mosek::fusion::ObjectiveSense::Maximize, expr);
    }
    else {
        _mosek_model->objective("obj", mosek::fusion::ObjectiveSense::Minimize, expr);
    }
//    cout << "\nObj = " << expr->toString() << endl;
}

void MosekProgram::prepare_model() {
    double time_before = get_wall_time();
    fill_in_mosek_vars();
    create_mosek_constraints();
    set_mosek_objective();
    cout << "\nTime spent on building model = " << get_wall_time() - time_before;
    //    print_constraints();
}


// Copyright 2017, 2018 Peter Dimov.
// Distributed under the Boost Software License, Version 1.0.

#include <boost/hash2/fnv1a.hpp>
#include <boost/hash2/siphash.hpp>
#include <boost/hash2/xxhash.hpp>
#include <boost/hash2/spooky2.hpp>
#include <boost/hash2/murmur3.hpp>
#include <boost/hash2/md5.hpp>
#include <boost/hash2/sha1.hpp>
#include <boost/hash2/byte_type.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <boost/type_traits/integral_constant.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_signed.hpp>
#include <boost/array.hpp>
#include <limits>
#include <cstddef>

using boost::hash2::byte_type;

template<class R> struct is_valid_result:
    boost::integral_constant<bool,
        boost::is_integral<R>::value && !boost::is_signed<R>::value
    >
{
};

template<std::size_t N> struct is_valid_result< boost::array<byte_type, N> >:
    boost::integral_constant<bool, N >= 8>
{
};

template<class H> void test_result_type()
{
    BOOST_TEST_TRAIT_TRUE((is_valid_result<typename H::result_type>));
}

template<class H> void test_size_type()
{
    BOOST_TEST_TRAIT_TRUE((boost::is_integral<typename H::size_type>));

    BOOST_TEST_EQ( std::numeric_limits<typename H::size_type>::min(), 0 );
    BOOST_TEST_GE( std::numeric_limits<typename H::size_type>::max(), 0xFFFFFFFFu );
}

template<class H> void test_default_constructible()
{
    H h1;
    typename H::result_type r1 = h1.result();

    H h2;
    typename H::result_type r2 = h2.result();

    BOOST_TEST( r1 == r2 );
}

template<class H> void test_byte_seed_constructible()
{
    byte_type const seed[ 3 ] = { 0x01, 0x02, 0x03 };

    {
        H h1;
        H h2( static_cast<byte_type const*>( 0 ), 0 );

        BOOST_TEST( h1.result() == h2.result() );
    }

    {
        H h1;
        H h2( seed, 0 );

        BOOST_TEST( h1.result() == h2.result() );
    }

    {
        H h1;
        H h2( seed, 3 );

        BOOST_TEST( h1.result() != h2.result() );
    }

    {
        H h1( seed, 3 );
        H h2( seed, 3 );

        BOOST_TEST( h1.result() == h2.result() );
    }

    {
        H h1( seed, 2 );
        H h2( seed, 3 );

        BOOST_TEST( h1.result() != h2.result() );
    }
}

template<class H> void test_integral_seed_constructible()
{
    typename H::result_type r0, r1, r2, r3;

    {
        H h( 0 );
        r0 = h.result();
    }

    {
        H h( 1 );
        r1 = h.result();
    }

    BOOST_TEST( r0 != r1 );

    {
        H h( 2 );
        r2 = h.result();
    }

    BOOST_TEST( r0 != r2 );
    BOOST_TEST( r1 != r2 );

    {
        H h( 1LL << 32 );
        r3 = h.result();
    }

    BOOST_TEST( r0 != r3 );
    BOOST_TEST( r1 != r3 );
    BOOST_TEST( r2 != r3 );

    {
        H h;
        BOOST_TEST( h.result() == r0 );
    }

    {
        H h( 0L );
        BOOST_TEST( h.result() == r0 );
    }

    {
        H h( 1L );
        BOOST_TEST( h.result() == r1 );
    }

    {
        H h( 2L );
        BOOST_TEST( h.result() == r2 );
    }

    {
        H h( 0LL );
        BOOST_TEST( h.result() == r0 );
    }

    {
        H h( 1LL );
        BOOST_TEST( h.result() == r1 );
    }

    {
        H h( 2LL );
        BOOST_TEST( h.result() == r2 );
    }

    {
        H h( static_cast<boost::uint32_t>( 0 ) );
        BOOST_TEST( h.result() == r0 );
    }

    {
        H h( static_cast<boost::uint32_t>( 1 ) );
        BOOST_TEST( h.result() == r1 );
    }

    {
        H h( static_cast<boost::uint32_t>( 2 ) );
        BOOST_TEST( h.result() == r2 );
    }

    {
        H h( static_cast<std::size_t>( 0 ) );
        BOOST_TEST( h.result() == r0 );
    }

    {
        H h( static_cast<std::size_t>( 1 ) );
        BOOST_TEST( h.result() == r1 );
    }

    {
        H h( static_cast<std::size_t>( 2 ) );
        BOOST_TEST( h.result() == r2 );
    }

    {
        H h( static_cast<boost::uint64_t>( 0 ) );
        BOOST_TEST( h.result() == r0 );
    }

    {
        H h( static_cast<boost::uint64_t>( 1 ) );
        BOOST_TEST( h.result() == r1 );
    }

    {
        H h( static_cast<boost::uint64_t>( 2 ) );
        BOOST_TEST( h.result() == r2 );
    }

    {
        H h( static_cast<boost::uint64_t>( 1 ) << 32 );
        BOOST_TEST( h.result() == r3 );
    }
}

template<class H> void test_copy_constructible()
{
    byte_type const seed[ 3 ] = { 0x01, 0x02, 0x03 };

    {
        H h1;
        H h2( h1 );

        BOOST_TEST( h1.result() == h2.result() );
    }

    {
        H h1( seed, 3 );
        H h2( h1 );

        BOOST_TEST( h1.result() == h2.result() );
    }

    {
        H h1;
        h1.update( seed, 3 );

        H h2( h1 );

        BOOST_TEST( h1.result() == h2.result() );
    }
}

template<class H> void test_update()
{
    byte_type const data[ 3 ] = {};

    {
        H h1;

        H h2;
        h2.update( data, 3 );

        BOOST_TEST( h1.result() != h2.result() );
    }

    {
        H h1;
        h1.update( data, 3 );

        H h2;
        h2.update( data, 3 );

        BOOST_TEST( h1.result() == h2.result() );
    }

    {
        H h1;
        h1.update( data, 3 );

        H h2;
        h2.update( data, 2 );

        BOOST_TEST( h1.result() != h2.result() );
    }

    {
        H h1;
        h1.update( data, 3 );

        H h2( h1 );

        h1.update( data, 3 );
        h2.update( data, 3 );

        BOOST_TEST( h1.result() == h2.result() );
    }
}

template<class H> void test_assignable()
{
    byte_type const seed[ 3 ] = { 0x01, 0x02, 0x03 };

    {
        H h1;
        H h2( h1 );

        typename H::result_type r1 = h1.result();

        BOOST_TEST( h1.result() != r1 );

        h1 = h2;
        BOOST_TEST( h1.result() == r1 );
    }

    {
        H h1( seed, 3 );
        H h2( h1 );

        typename H::result_type r1 = h1.result();

        BOOST_TEST( h1.result() != r1 );

        h1 = h2;
        BOOST_TEST( h1.result() == r1 );
    }

    {
        H h1;
        h1.update( seed, 3 );

        H h2( h1 );

        typename H::result_type r1 = h1.result();

        BOOST_TEST( h1.result() != r1 );

        h1 = h2;
        BOOST_TEST( h1.result() == r1 );
    }
}

template<class H> void test()
{
    test_result_type<H>();
    test_size_type<H>();
    test_default_constructible<H>();
    test_byte_seed_constructible<H>();
    test_integral_seed_constructible<H>();
    test_copy_constructible<H>();
    test_update<H>();
    test_assignable<H>();
}

int main()
{
    test<boost::hash2::fnv1a_32>();
    test<boost::hash2::fnv1a_64>();
    test<boost::hash2::murmur3_32>();
    test<boost::hash2::murmur3_128>();
    test<boost::hash2::xxhash_32>();
    test<boost::hash2::xxhash_64>();
    test<boost::hash2::spooky2_128>();
    test<boost::hash2::siphash_32>();
    test<boost::hash2::siphash_64>();
    test<boost::hash2::md5_128>();
    test<boost::hash2::sha1_160>();
    test<boost::hash2::hmac_md5_128>();
    test<boost::hash2::hmac_sha1_160>();

    return boost::report_errors();
}


#include <ctype.h>
#include <regex>
#include <algorithm>

#include <boost/log/trivial.hpp>

#include "lib/stats/relative_strength_index.h"
#include "lib/stats/exponential_moving_average.h"
#include "lib/stats/simple_moving_average.h"
#include "lib/stats/bollinger.h"
#include "lib/rsiscript.h"

/**
 * Parse a math-related script. There are several phases.
 *
 * 1. Replace variables.
 * 2. Identify sections in parenthesis. Process these first.
 * 3. Process the math equations/comparisons.
 *
 * @param const char *script [ex: (1+3)/(2 * (4 + 6))]
 * @return const char *result
 */
int script_max_paren_depth = 50;
std::string rsiscript::parse(const char* const script, const stockinfo &data) {
	std::string err = "0", repl, expr = script;
	std::size_t pos, lparen_pos, rparen_pos = 0;
	unsigned int lparens, rparens;
	bool found = false;

	last_variables_used.clear();
	last_variables.clear();
	if (script == nullptr)
		return err;

	BOOST_LOG_TRIVIAL(trace) << "Script: " << script;
	expr = replace_variables(expr, data);

	do {
		// Search for a ')'.
		pos = rparen_pos = expr.find(")", rparen_pos);
		found = (rparen_pos != std::string::npos);

		// Find the matching '('.
		if (found) {
			rparens = 1;
			lparens = 0;

			while (pos > 0) {
				pos--;

				if (expr[pos] == ')') // TODO: Should not be possible.
					rparens++;
				else if (expr[pos] == '(') {
					lparens++;

					// Will only happen with a '(' find.
					if (lparens == rparens) {
						lparen_pos = pos;
						break;
					}
				}
			}

			// ERROR: Mismatched parenthesis.
			if (rparens != lparens) {
				BOOST_LOG_TRIVIAL(error) << "Mismatched parenthesis: " << script;
				printf("ERROR: Mismatched parenthesis: %s\n", script);
				return err;
			}

			// Parse the substring. Do not include the current parenthesis set.
			repl = parse(expr.substr(lparen_pos + 1, rparen_pos - lparen_pos - 1).c_str(), data);
			repl = exec_script_calculate(repl);
			expr.replace(lparen_pos, rparen_pos - lparen_pos + 1, repl);

			rparen_pos -= (rparen_pos - lparen_pos - repl.length());
		}
	} while (found);

	expr = exec_script_calculate(expr);
	BOOST_LOG_TRIVIAL(info) << "End of run: " << expr;
	return expr;
}

/**
 * Find/replace the variables in our script.
 *
 * @return string
 */
std::string rsiscript::replace_variables(const std::string &script, const stockinfo &data) {
	std::string expr = script;
	std::string err = "0", repl;
	std::size_t pos, lparen_pos, rparen_pos = 0;
	unsigned int lparens, rparens;
	bool found = false;

	do {
		// Search for a ')'.
		pos = rparen_pos = expr.find("}", rparen_pos);
		found = (rparen_pos != std::string::npos);

		// Find the matching '('.
		if (found) {
			rparens = 1;
			lparens = 0;

			while (pos > 0) {
				pos--;

				if (expr[pos] == '}') // TODO: Should not be possible.
					rparens++;
				else if (expr[pos] == '{') {
					lparens++;

					// Will only happen with a '(' find.
					if (lparens == rparens) {
						lparen_pos = pos;
						break;
					}
				}

				/*if (lparens > script_max_paren_depth) {
					BOOST_LOG_TRIVIAL(error) << "Found too many opening parens: " << lparens;
					printf("Found too many opening parens: %iu\n", lparens);
					return nullptr;
				}*/
			}

			// ERROR: Mismatched parenthesis.
			if (rparens != lparens) {
				BOOST_LOG_TRIVIAL(error) << "Mismatched brackets: " << script;
				printf("ERROR: Mismatched brackets: %s\n", script.c_str());
				return err;
			}

			// Parse the substring. Do not include the current parenthesis set.
			repl = replace_variables(expr.substr(lparen_pos + 1, rparen_pos - lparen_pos - 1), data);
			BOOST_LOG_TRIVIAL(trace) << "Replacing: " << repl;
			repl = variables(repl, data);
			// TODO: Replace all instances of this same variable?
			expr.replace(lparen_pos, rparen_pos - lparen_pos + 1, repl);

			rparen_pos -= (rparen_pos - lparen_pos - repl.length());
		}
	} while (found);

	BOOST_LOG_TRIVIAL(trace) << "Replaced variables: " << expr;

	return expr;
}

/**
 * Parse script variables into values.
 *
 * Format: high
 * Format: rsi
 * Format: high:52wk
 * Format: rsi:52wk(28)
 *
 * @param string req The variable to process.
 * @param stockinfo data The stock data to use in processing req.
 * @return string The script without any more variables.
 */
std::string rsiscript::variables(const std::string &req, const stockinfo &data) {
	std::string ret = "0";
	std::vector<std::string> tokens;
	stockinfo *working_data = (stockinfo *)&data;
	stockinfo week_data;

	tokenize(req, tokens, ":", true);
	BOOST_LOG_TRIVIAL(trace) << "Variable tokens: " << tokens.size();

	// TODO: Parse options for stat variables. Weekly, monthly, RSI/SMA/EMA/BB/&c.
	std::string period;
	if (tokens.size() >= 2) {
		period = tokens[1];
	}
	if (period.length()) {
		int number;
		timeperiods tp;
		parse_period(period, number, tp);

		week_data = working_data->rollup(number, tp);
		working_data = &week_data;
	}

	if (!(*working_data).length()) {
		return ret;
	}

	// Process the requested variable.
	std::string var = tokens[0];
	if (var.compare("open") == 0) {
		ret = last_variable(req, (*working_data)[0]->open);
	}
	else if (var.compare("high") == 0) {
		ret = last_variable(req, (*working_data)[0]->high);
	}
	else if (var.compare("low") == 0) {
		ret = last_variable(req, (*working_data)[0]->low);
	}
	else if (var.compare("close") == 0) {
		ret = last_variable(req, (*working_data)[0]->close);
	}
	else if (var.compare("volume") == 0) {
		ret = last_variable(req, (*working_data)[0]->volume);
	}
	else if (var.compare("rsi") == 0) {
		relative_strength_index rsi;
		double *rsi_data = rsi.generate(*working_data, 14, (*working_data).length() - 26);
		ret = last_variable(req, *rsi_data);
		free(rsi_data);
	}
	else if (var.compare("sma") == 0) {
		simple_moving_average sma;
		double *sma_data = sma.generate(*working_data, 20, (*working_data).length() - 26);
		ret = last_variable(req, *sma_data);
		free(sma_data);
	}
	else if (var.compare("ema") == 0) {
		simple_moving_average ema;
		double *ema_data = ema.generate(*working_data, 20, (*working_data).length() - 26);
		ret = last_variable(req, *ema_data);
		free(ema_data);
	}
	else if (var.compare("bb_top") == 0) {
		simple_moving_average sma;
		bollinger bb;
		double *sma_data = sma.generate(*working_data, 20, (*working_data).length() - 26);
		double *bb_data = bb.bands(*working_data, 14, (*working_data).length() - 26);
		ret = last_variable(req, *sma_data + *bb_data);
		free(sma_data);
		free(bb_data);
	}
	else if (var.compare("bb_bottom") == 0) {
		simple_moving_average sma;
		bollinger bb;
		double *sma_data = sma.generate(*working_data, 20, (*working_data).length() - 26);
		double *bb_data = bb.bands(*working_data, 14, (*working_data).length() - 26);
		ret = last_variable(req, *sma_data - *bb_data);
		free(sma_data);
		free(bb_data);
	}

	return ret;
}

/**
 * Parse a string like "52weeks" roughly into "52" and "week." It would have been nice to use regex for this, but the
 * regex parser does not appear to work in this dev's compiler.
 *
 * @todo We could refactor this to place number & period into a struct.
 *
 * @param req String
 * @param number Return value.
 * @param period Return value.
 */
void rsiscript::parse_period(std::string req, int &number, timeperiods &period) {
	std::istringstream i(req);
	i >> number;

	if (!number) {
		number = 1;
	}

	if (req.compare("week"))
		period = week;
	else if (req.compare("month"))
		period = month;
	else if (req.compare("year"))
		period = year;
	else //if (req.compare("day"))
		period = day;

	return;
}

/**
 * Store name + value in last_variables.
 *
 * @param name
 * @param value
 * @return string value
 */
template<typename T>
std::string rsiscript::last_variable(const std::string &name, T value) {
	std::string ret = std::to_string(value);
	bool add = !last_variables_used.size();

	if (!add) {
		add = std::find(last_variables_used.begin(), last_variables_used.end(), name) == last_variables_used.end();
	}

	if (add) {
		if (last_variables.length()) {
			last_variables += ", ";
		}
		last_variables += name + " = " + ret;
		last_variables_used.push_back(name);
	}

	return ret;
}

/**
 * Direct the order of operator usage.
 *
 * Phase 1: ^
 * Phase 2: * / %
 * Phase 3: + -
 * Phase 4: > < =
 * Phase 5: | &
 *
 * @todo >= <=
 *
 * @return string The value calculated.
 */
const std::string rsiscript::exec_script_calculate(const std::string &script) {
	std::string ret;

	BOOST_LOG_TRIVIAL(trace) << "Script chunk: " << script;

	// Operational passes. The terminating 0's allow us to use string functions.
	const char first_pass[] = {'^', 0};
	const char second_pass[] = {'*', 'x', '/', '%', 0};
	const char third_pass[] = {'+', '-', 0};
	const char fourth_pass[] = {'>', '<', '=', 0};
	const char fifth_pass[] = {'|', '&', 0};

	// Pass over the string three times to give us proper order of operation.
	ret = exec_script_operations(script, first_pass);
	ret = exec_script_operations(ret, second_pass);
	ret = exec_script_operations(ret, third_pass);
	ret = exec_script_operations(ret, fourth_pass);
	ret = exec_script_operations(ret, fifth_pass);

	BOOST_LOG_TRIVIAL(trace) << "Script chunk reduced: " << ret;

	return ret;
}

/**
 * Find numbers and their operators for direct calculation.
 *
 * @return string The calculated value(s).
 */
std::string rsiscript::exec_script_operations(const std::string &script, const char *operators) {
	std::size_t sc_len = script.length();
	const char digits[] = "0123456789.";
	std::size_t x, start, operation, end;
	bool isdigit, calc_end, calc_start = false;
	long num1l, num2l;
	double num1d, num2d;
	std::string ret = script, result, num1str, num2str;

	for (x = 0; x < sc_len; x++) {
		/**
		 * This logic assumes well-formed expressions: 4+(*1+2) => 4+*3 => 4+3 => 7.
		 */

		// Ignore space characters.
		if (isspace(ret[x]))
			continue;

		// Mark the beginning of numbers as we work through the string.
		isdigit = (strchr(digits, ret[x]) != NULL);
		if (!calc_start && isdigit) {
			calc_start = true;
			start = x;
		}
		// After we find a number, find an operation.
		else if (calc_start && (strchr(operators, ret[x]) != NULL)) {
			operation = x;
			calc_end = false;

			// Is there another number after the operator?
			// TODO: Allow negative numbers.
			for (x++; x < sc_len; x++) {
				if (strchr(digits, ret[x]) != NULL) {
					calc_end = true;
					break;
				}
			}

			// If we found another number...
			if (calc_end) {
				// Find the end of the number;
				end = sc_len;
				for (x++; x < sc_len; x++) {
					if (strchr(digits, ret[x]) == NULL) {
						end = x - 1;
						break;
					}
				}

				// Pull this expression apart.
				num1str = ret.substr(start, operation - start);
				num2str = ret.substr(operation + 1, end - operation);

				BOOST_LOG_TRIVIAL(trace) << "Script operation: " << num1str << ", " << ret[operation] << ", " << num2str;

				// If this is a long/integer operation
				if ((num1str.find(".") == std::string::npos) && (num2str.find(".") == std::string::npos)) {
					num1l = atol(num1str.c_str());
					num2l = atol(num2str.c_str());

					result = exec_script_calculate_operation(num1l, num2l, ret[operation]);
				} else {
					num1d = atof(num1str.c_str());
					num2d = atof(num2str.c_str());

					result = exec_script_calculate_operation(num1d, num2d, ret[operation]);
				}

				ret.replace(start, end - start + 1, result);
				sc_len = ret.length();
				x = start; // Re-process this result since it might start the next calculation.

				BOOST_LOG_TRIVIAL(trace) << "Script operation result: " << ret;
			} // End calculation.

			//calc_start = false;
		} // End operator section.
		else if (!isdigit) {
			// If this is not a valid digit, a space, or in the current operations list, start over.
			calc_start = false;
		}
	} // Next character.

	return ret;
}

/**
 * Takes two numeric (int/double) values and a comparison operator. Performs the required operation.
 *
 * @return int/double
 */
template<typename T>
std::string rsiscript::exec_script_calculate_operation(const T val1, const T val2, char operation) {
	double res;
	T result;

	switch(operation) {
		case '*':
		case 'x':
			result = val1 * val2;
			break;
		case '/':
			res = (double)val1 / (double)val2;
			break;
		case '+':
			result = val1 + val2;
			break;
		case '-':
			result = val1 - val2;
			break;
		case '^':
			result = pow(val1, val2);
			break;
		case '%':
			result = remainder(val1, val2);
			break;
		case '>':
			result = (val1 > val2) ? 1 : 0;
			break;
		case '<':
			result = (val1 < val2) ? 1 : 0;
			break;
		case '=':
			result = (val1 == val2) ? 1 : 0;
			break;
		case '|':
			result = (val1 || val2) ? 1 : 0;
			break;
		case '&':
			result = (val1 && val2) ? 1 : 0;
			break;
		default:
			result = 0;
			BOOST_LOG_TRIVIAL(error) << "Unknown script operation: " << operation;
	}

	// Force (double) for division.
	return (operation == '/') ? std::to_string(res) : std::to_string(result);
}

/**
 * Split a string.
 *
 * @link https://stackoverflow.com/a/1493195/850782
 *
 * @param string str
 * @param tokens The return structure.
 * @param string delimiters The characters to split for (default: " ").
 * @param bool trimEmpty Should empty values be skipped (default: true)?
 * @return None. Use tokens.
 */
template<class ContainerT>
void rsiscript::tokenize(const std::string& str, ContainerT& tokens,
				const std::string& delimiters, bool trimEmpty)
{
	std::string::size_type pos, lastPos = 0, length = str.length();

	using value_type	= typename ContainerT::value_type;
	using size_type		= typename ContainerT::size_type;

	while(lastPos < length + 1)
	{
		pos = str.find_first_of(delimiters, lastPos);
		if(pos == std::string::npos)
		{
			pos = length;
		}

		if(pos != lastPos || !trimEmpty)
			tokens.push_back(value_type(str.data()+lastPos,
					(size_type)pos-lastPos ));

		lastPos = pos + 1;
	}
}


#include <bts/blockchain/market_records.hpp>
#include <fc/exception/exception.hpp>
#include <sstream>
#include <boost/algorithm/string.hpp>

namespace bts { namespace blockchain {

order_id_type market_order::get_id()const
{
    std::stringstream id_ss;
    id_ss << string( type )
          << string( market_index.order_price )
          << string( market_index.owner );
    return fc::ripemd160::hash( id_ss.str() );
}

string market_order::get_small_id()const
{
    string type_prefix = string( type );
    type_prefix = type_prefix.substr( 0, type_prefix.find( "_" ) );
    boost::to_upper( type_prefix );
    return type_prefix + "-" + string( get_id() ).substr( 0, 8 );
}

asset market_order::get_balance()const
{
  asset_id_type asset_id;
  switch( order_type_enum( type ) )
  {
     case bid_order:
        asset_id = market_index.order_price.quote_asset_id;
        break;
     case ask_order:
        asset_id = market_index.order_price.base_asset_id;
        break;
     case short_order:
        asset_id = market_index.order_price.base_asset_id;
        break;
     case cover_order:
        asset_id = market_index.order_price.quote_asset_id;
        break;
     default:
        FC_ASSERT( false, "Not Implemented" );
  }
  return asset( state.balance, asset_id );
}

price market_order::get_price()const
{
  return market_index.order_price;
}

price market_order::get_highest_cover_price()const
{ try {
  FC_ASSERT( type == cover_order );
  return asset( state.balance, market_index.order_price.quote_asset_id ) / asset( *collateral );
} FC_CAPTURE_AND_RETHROW() }

asset market_order::get_quantity()const
{
  switch( order_type_enum( type ) )
  {
     case bid_order:
     { // balance is in USD  divide by price
        return get_balance() * get_price();
     }
     case ask_order:
     { // balance is in USD  divide by price
        return get_balance();
     }
     case short_order:
     {
        return get_balance();
     }
     case cover_order:
     {
        return asset( (*collateral * 3)/4 );
     }
     default:
        FC_ASSERT( false, "Not Implemented" );
  }
  // NEVER GET HERE.....
  //return get_balance() * get_price();
}
asset market_order::get_quote_quantity()const
{
  switch( order_type_enum( type ) )
  {
     case bid_order:
     { // balance is in USD  divide by price
        return get_balance();
     }
     case ask_order:
     { // balance is in USD  divide by price
        return get_balance() * get_price();
     }
     case short_order:
     {
        return get_balance() * get_price();
     }
     case cover_order:
     {
        return get_balance();
     }
     default:
        FC_ASSERT( false, "Not Implemented" );
  }
  // NEVER GET HERE.....
 // return get_balance() * get_price();
}

} } // bts::blockchain