/**
 * \ file SecondOrderSVFFilter.cpp
 */

#include <ATK/EQ/SecondOrderSVFFilter.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>

#define PROCESSSIZE (1024*64)

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

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  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.10017263357405178));
  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( SecondOrderSVFFilter_SVFLowPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  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, 1));
  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( SecondOrderSVFFilter_SVFLowPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  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.04987802660970978));
  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( SecondOrderSVFFilter_SVFLowPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  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.5266273548331198));
  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( SecondOrderSVFFilter_SVFHighPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  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.0024866672050896));
  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( SecondOrderSVFFilter_SVFHighPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  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, 1));
  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( SecondOrderSVFFilter_SVFHighPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  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.0006206013284984));
  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( SecondOrderSVFFilter_SVFHighPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);

  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, 1.0532789138212624));
  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(SecondOrderSVFFilter_SVFBandPassCoefficients_1k_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);

  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.3168938774681514));
  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(SecondOrderSVFFilter_SVFBandPassCoefficients_100_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);

  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, 1));
  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(SecondOrderSVFFilter_SVFBandPassCoefficients_2k_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);

  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.22340318032490636));
  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(SecondOrderSVFFilter_SVFBandPassPeakCoefficients_1k_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFBellCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);
  filter.set_gain(2);

  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.0093931154905567));
  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(SecondOrderSVFFilter_SVFBandPassPeakCoefficients_100_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFBellCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);
  filter.set_gain(2);

  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, 2));
  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(SecondOrderSVFFilter_SVFBandPassPeakCoefficients_2k_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024 * 64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFBellCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024 * 64);
  filter.set_output_sampling_rate(1024 * 64);
  filter.set_Q(1);
  filter.set_cut_frequency(100);
  filter.set_gain(2);

  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.0023373844708783));
  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( SecondOrderSVFFilter_SVFLowShelvingCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFLowShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_gain(.5);

  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.0018539205036439));
  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( SecondOrderSVFFilter_SVFLowShelvingCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFLowShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_gain(.5);

  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.949414461057972));
  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( SecondOrderSVFFilter_SVFLowShelvingCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFLowShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_gain(.5);

  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, 1.037173648834839));
  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( SecondOrderSVFFilter_SVFHighShelvingCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFHighShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(1000);
  filter.set_gain(.5);

  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.7905890936892349));
  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( SecondOrderSVFFilter_SVFHighShelvingCoefficients_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFHighShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(1000);
  filter.set_gain(.5);

  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(10000, 0.5062960600823273));
  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( SecondOrderSVFFilter_SVFHighShelvingCoefficients_500_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(500);

  ATK::SecondOrderSVFFilter<ATK::SecondOrderSVFHighShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(1000);
  filter.set_gain(.5);

  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(500, 0.9220383944187385));
  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);
}


/*
 * Copyright (C) 2019-2020 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

#include <ros/ros.h>
#include <string>
#include <algorithm>
#include <memory>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <lanelet2_core/geometry/Point.h>
#include <trajectory_utils/trajectory_utils.h>
#include <trajectory_utils/conversions/conversions.h>
#include <sstream>
#include <carma_utils/containers/containers.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <yield_plugin/yield_plugin.h>

using oss = std::ostringstream;

namespace yield_plugin
{
  YieldPlugin::YieldPlugin(carma_wm::WorldModelConstPtr wm, YieldPluginConfig config,
                                            PublishPluginDiscoveryCB plugin_discovery_publisher,
                                            MobilityResponseCB mobility_response_publisher)
    : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher), mobility_response_publisher_(mobility_response_publisher)
  {
    plugin_discovery_msg_.name = "YieldPlugin";
    plugin_discovery_msg_.versionId = "v1.0";
    plugin_discovery_msg_.available = true;
    plugin_discovery_msg_.activated = false;
    plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL;
    plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory";
  }

  void YieldPlugin::lookupECEFtoMapTransform()
  {
    tf2_listener_.reset(new tf2_ros::TransformListener(tf2_buffer_));
    tf2_buffer_.setUsingDedicatedThread(true);
    try
    {
      tf_ = tf2_buffer_.lookupTransform("earth", "map", ros::Time(0), ros::Duration(20.0)); //save to local copy of transform 20 sec timeout
    }
    catch (const tf2::TransformException &ex)
    {
      ROS_WARN("%s", ex.what());
    }
  }

  bool YieldPlugin::onSpin()
  {
    plugin_discovery_publisher_(plugin_discovery_msg_);
    return true;
  }

  void YieldPlugin::set_lanechange_status_publisher(const ros::Publisher& publisher)
  {
    lanechange_status_pub_ = publisher;
  }

  std::vector<std::pair<int, lanelet::BasicPoint2d>> YieldPlugin::detect_trajectories_intersection(std::vector<lanelet::BasicPoint2d> self_trajectory, std::vector<lanelet::BasicPoint2d> incoming_trajectory) const
  {
    std::vector<std::pair<int, lanelet::BasicPoint2d>> intersection_points;
    boost::geometry::model::linestring<lanelet::BasicPoint2d> self_traj;
    for (auto tpp:self_trajectory)
    {
      boost::geometry::append(self_traj, tpp);
    }
    // distance to consider trajectories colliding (chosen based on lane width and vehicle size)
    for (size_t i=0; i<incoming_trajectory.size(); i++)
    {
      double res = boost::geometry::distance(incoming_trajectory[i], self_traj);

      if (fabs(res) <= config_.intervehicle_collision_distance)
      {
         intersection_points.push_back(std::make_pair(i, incoming_trajectory[i]));
      }
    }
    return intersection_points;
  }

  std::vector<lanelet::BasicPoint2d> YieldPlugin::convert_eceftrajectory_to_mappoints(const cav_msgs::Trajectory& ecef_trajectory, const geometry_msgs::TransformStamped& tf) const
  {
    cav_msgs::TrajectoryPlan trajectory_plan;
    std::vector<lanelet::BasicPoint2d> map_points;

    tf2::Stamped<tf2::Transform> transform;
    tf2::fromMsg(tf, transform);

    lanelet::BasicPoint2d first_point = ecef_to_map_point(ecef_trajectory.location, transform);

    map_points.push_back(first_point);
    auto curr_point = ecef_trajectory.location;

    for (size_t i = 0; i<ecef_trajectory.offsets.size(); i++)
    {
      lanelet::BasicPoint2d offset_point;
      curr_point.ecef_x += ecef_trajectory.offsets[i].offset_x;
      curr_point.ecef_y += ecef_trajectory.offsets[i].offset_y;
      curr_point.ecef_z += ecef_trajectory.offsets[i].offset_z;

      offset_point = ecef_to_map_point(curr_point, transform);

      map_points.push_back(offset_point);
    }

    return map_points;
  }

  lanelet::BasicPoint2d YieldPlugin::ecef_to_map_point(const cav_msgs::LocationECEF& ecef_point, const tf2::Transform& map_in_earth) const
  {
    // convert input point to transform
    tf2::Transform point_in_earth;
    tf2::Quaternion no_rotation(0, 0, 0, 1);
    tf2::Vector3 input_point {(double)ecef_point.ecef_x/100.0, (double)ecef_point.ecef_y/100.0, (double)ecef_point.ecef_z/100.0}; //m to cm
    point_in_earth.setOrigin(input_point);
    point_in_earth.setRotation(no_rotation);
    // convert to map frame by (T_e_m)^(-1) * T_e_p
    auto point_in_map = map_in_earth.inverse() * point_in_earth;
    lanelet::BasicPoint2d output {
      point_in_map.getOrigin().getX(),
      point_in_map.getOrigin().getY()};

    return output;
  }

  cav_msgs::MobilityResponse YieldPlugin::compose_mobility_response(const std::string& resp_recipient_id, const std::string& req_plan_id, bool response) const
  {
    cav_msgs::MobilityResponse out_mobility_response;
    out_mobility_response.header.sender_id = config_.vehicle_id;
    out_mobility_response.header.recipient_id = resp_recipient_id;
    out_mobility_response.header.sender_bsm_id = host_bsm_id_;
    out_mobility_response.header.plan_id = req_plan_id;
    out_mobility_response.header.timestamp = ros::Time::now().toSec()*1000;

    if (config_.always_accept_mobility_request && response)
    {
      out_mobility_response.is_accepted = true;
    }
    else out_mobility_response.is_accepted = false;

    return out_mobility_response;
  }

  void YieldPlugin::mobilityrequest_cb(const cav_msgs::MobilityRequestConstPtr& msg)
  {
    cav_msgs::MobilityRequest incoming_request = *msg;
    cav_msgs::LaneChangeStatus lc_status_msg;
    if (incoming_request.strategy == "carma/cooperative-lane-change")
    {
      if (incoming_request.plan_type.type == cav_msgs::PlanType::CHANGE_LANE_LEFT || incoming_request.plan_type.type == cav_msgs::PlanType::CHANGE_LANE_RIGHT)
      {
        ROS_DEBUG_STREAM("Cooperative Lane Change Request Received");
        lc_status_msg.status = cav_msgs::LaneChangeStatus::REQUEST_RECEIVED;
        lc_status_msg.description = "Received lane merge request";
        if (incoming_request.header.recipient_id == config_.vehicle_id)
        {
          ROS_DEBUG_STREAM("CLC Request correctly received");
        }
        // extract mobility header
        std::string req_sender_id = incoming_request.header.sender_id;
        std::string req_plan_id = incoming_request.header.plan_id;
        // extract mobility request
        cav_msgs::LocationECEF ecef_location = incoming_request.location;
        cav_msgs::Trajectory incoming_trajectory = incoming_request.trajectory;
        std::string req_strategy_params = incoming_request.strategy_params;
        clc_urgency_ = incoming_request.urgency;
        ROS_DEBUG_STREAM("received urgency: " << clc_urgency_);

        // Parse strategy parameters
        using boost::property_tree::ptree;
        ptree pt;
        std::istringstream strstream(req_strategy_params);
        boost::property_tree::json_parser::read_json(strstream, pt);
        int req_traj_speed_full = pt.get<int>("s");
        int req_traj_fractional = pt.get<int>("f");
        int start_lanelet_id = pt.get<int>("sl");
        int end_lanelet_id = pt.get<int>("el");
        double req_traj_speed = (double)req_traj_speed_full + (double)(req_traj_fractional)/10.0;
        ROS_DEBUG_STREAM("req_traj_speed" << req_traj_speed);
        ROS_DEBUG_STREAM("start_lanelet_id" << start_lanelet_id);
        ROS_DEBUG_STREAM("end_lanelet_id" << end_lanelet_id);

        std::vector<lanelet::BasicPoint2d> req_traj_plan = {};

        req_traj_plan = convert_eceftrajectory_to_mappoints(incoming_trajectory, tf_);

        double req_expiration_sec = (double)incoming_request.expiration;
        double current_time_sec = ros::Time::now().toSec();

        bool response_to_clc_req = false;
        // ensure there is enough time for the yield
        double req_plan_time = req_expiration_sec - current_time_sec;
        double req_timestamp = (double)incoming_request.header.timestamp / 1000.0 - current_time_sec;
        set_incoming_request_info(req_traj_plan, req_traj_speed, req_plan_time, req_timestamp);

        if (req_expiration_sec - current_time_sec >= config_.tpmin && cooperative_request_acceptable_)
        {
          timesteps_since_last_req_ = 0;
          lc_status_msg.status = cav_msgs::LaneChangeStatus::REQUEST_ACCEPTED;
          lc_status_msg.description = "Accepted lane merge request";
          response_to_clc_req = true;
          ROS_DEBUG_STREAM("CLC accepted");
        }
        else
        {
          lc_status_msg.status = cav_msgs::LaneChangeStatus::REQUEST_REJECTED;
          lc_status_msg.description = "Rejected lane merge request";
          response_to_clc_req = false;
          ROS_DEBUG_STREAM("CLC rejected");
        }
        cav_msgs::MobilityResponse outgoing_response = compose_mobility_response(req_sender_id, req_plan_id, response_to_clc_req);
        mobility_response_publisher_(outgoing_response);
        lc_status_msg.status = cav_msgs::LaneChangeStatus::RESPONSE_SENT;
        ROS_DEBUG_STREAM("response sent");
      }
    }
    lanechange_status_pub_.publish(lc_status_msg);
    if (lanechange_status_pub_.isLatched())
    {
      lanechange_status_pub_.publish(lc_status_msg);
    }
    else
    {
      ROS_WARN("Lane Change Status Topic is not latched.");
    }

  }

  void YieldPlugin::set_incoming_request_info(std::vector <lanelet::BasicPoint2d> req_trajectory, double req_speed, double req_planning_time, double req_timestamp)
  {
    req_trajectory_points_ = req_trajectory;
    req_target_speed_ = req_speed;
    req_target_plan_time_ = req_planning_time;
    ROS_DEBUG_STREAM("req_target_plan_time_" << req_target_plan_time_);
    req_timestamp_ = req_timestamp;
  }

  void YieldPlugin::bsm_cb(const cav_msgs::BSMConstPtr& msg)
  {
    cav_msgs::BSMCoreData bsm_core_ = msg->core_data;
    host_bsm_id_ = bsmIDtoString(bsm_core_);
  }

  bool YieldPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req,
                                              cav_srvs::PlanTrajectoryResponse& resp)
  {
    if (req.initial_trajectory_plan.trajectory_points.size() < 2){
      throw std::invalid_argument("Empty Trajectory received by Yield");
    }
    cav_msgs::TrajectoryPlan original_trajectory = req.initial_trajectory_plan;
    cav_msgs::TrajectoryPlan yield_trajectory;

    // seperating cooperative yield with regular object detection for better performance.
    if (config_.enable_cooperative_behavior && clc_urgency_ > config_.acceptable_urgency)
    {
      ROS_DEBUG_STREAM("Only consider high urgency clc");
      if (timesteps_since_last_req_ < config_.acceptable_passed_timesteps)
      {
        ROS_DEBUG_STREAM("Yield for CLC. We haven't received an updated negotiation this timestep");
        yield_trajectory = update_traj_for_cooperative_behavior(original_trajectory, req.vehicle_state.longitudinal_vel);
        timesteps_since_last_req_++;
      }
      else
      {
        ROS_DEBUG_STREAM("unreliable CLC communication, switching to object avoidance");
        yield_trajectory = update_traj_for_object(original_trajectory, req.vehicle_state.longitudinal_vel); // Compute the trajectory
      }
    }
    else
    {
      ROS_DEBUG_STREAM("Yield for object avoidance");
      yield_trajectory = update_traj_for_object(original_trajectory, req.vehicle_state.longitudinal_vel); // Compute the trajectory
    }
    yield_trajectory.header.frame_id = "map";
    yield_trajectory.header.stamp = ros::Time::now();
    yield_trajectory.trajectory_id = original_trajectory.trajectory_id;
    yield_trajectory.initial_longitudinal_velocity = original_trajectory.initial_longitudinal_velocity;//copy the original trajectory's desired speed for now.

    resp.trajectory_plan = yield_trajectory;
    return true;
  }

  cav_msgs::TrajectoryPlan YieldPlugin::update_traj_for_cooperative_behavior(const cav_msgs::TrajectoryPlan& original_tp, double current_speed)
  {
    cav_msgs::TrajectoryPlan cooperative_trajectory;

    double initial_pos = 0;
    double goal_pos;
    double initial_velocity = current_speed;
    double goal_velocity = req_target_speed_;
    double planning_time = req_target_plan_time_;

    std::vector<lanelet::BasicPoint2d> host_traj_points = {};
    for (size_t i=0; i<original_tp.trajectory_points.size(); i++)
    {
      lanelet::BasicPoint2d traj_point;
      traj_point.x() = original_tp.trajectory_points[i].x;
      traj_point.y() = original_tp.trajectory_points[i].y;
      host_traj_points.push_back(traj_point);
    }

    std::vector<std::pair<int, lanelet::BasicPoint2d>> intersection_points = detect_trajectories_intersection(host_traj_points, req_trajectory_points_);
    if (!intersection_points.empty())
    {
      lanelet::BasicPoint2d intersection_point = intersection_points[0].second;
      double dx = original_tp.trajectory_points[0].x - intersection_point.x();
      double dy = original_tp.trajectory_points[0].y - intersection_point.y();
      goal_pos = sqrt(dx*dx + dy*dy) - config_.x_gap;
      ROS_DEBUG_STREAM("Goal position (goal_pos): " << goal_pos);
      double collision_time = req_timestamp_ + (intersection_points[0].first * ecef_traj_timestep_) - config_.safety_collision_time_gap;
      // planning_time = std::min(planning_time, collision_time);
      ROS_DEBUG_STREAM("req time stamp: " << req_timestamp_);
      ROS_DEBUG_STREAM("Collision time: " << collision_time);
      ROS_DEBUG_STREAM("intersection num: " << intersection_points[0].first);
      ROS_DEBUG_STREAM("Planning time: " << planning_time);
      // calculate distance traveled from beginning of trajectory to collision point
      double dx2 = intersection_point.x() - req_trajectory_points_[0].x();
      double dy2 = intersection_point.y() - req_trajectory_points_[0].y();
      // calculate incoming trajectory speed from time and distance between trajectory points
      double incoming_trajectory_speed = sqrt(dx2*dx2 + dy2*dy2)/(intersection_points[0].first * ecef_traj_timestep_);
      // calculate goal velocity from request trajectory
      goal_velocity = std::min(goal_velocity, incoming_trajectory_speed);
      double min_time = (initial_velocity - goal_velocity)/config_.yield_max_deceleration;

      ROS_DEBUG_STREAM("goal_velocity: " << goal_velocity);
      ROS_DEBUG_STREAM("incoming_trajectory_speed: " << incoming_trajectory_speed);

      if (planning_time > min_time)
      {
        cooperative_request_acceptable_ = true;
        cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time);
      }
      else
      {
        cooperative_request_acceptable_ = false;
        ROS_DEBUG_STREAM("The incoming requested trajectory is rejected, due to insufficient gap");
        cooperative_trajectory = original_tp;
      }

    }
    else
    {
      cooperative_request_acceptable_ = true;
      ROS_DEBUG_STREAM("The incoming requested trajectory does not overlap with host vehicle's trajectory");
      cooperative_trajectory = original_tp;
    }

    return cooperative_trajectory;
  }

  cav_msgs::TrajectoryPlan YieldPlugin::generate_JMT_trajectory(const cav_msgs::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time) const
  {
    cav_msgs::TrajectoryPlan jmt_trajectory;
    std::vector<cav_msgs::TrajectoryPlanPoint> jmt_trajectory_points;
    jmt_trajectory_points.push_back(original_tp.trajectory_points[0]);

    double initial_time = 0;
    double initial_accel = 0.0;
    double goal_accel = 0.0;

    double original_max_speed = max_trajectory_speed(original_tp.trajectory_points);
    ROS_DEBUG_STREAM("original_max_speed" << original_max_speed);
    std::vector<double> values = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos,
                                                                                goal_pos,
                                                                                initial_velocity,
                                                                                goal_velocity,
                                                                                initial_accel,
                                                                                goal_accel,
                                                                                initial_time,
                                                                                planning_time);

    std::vector<double> original_traj_downtracks = get_relative_downtracks(original_tp);

    for(size_t i = 1; i < original_tp.trajectory_points.size(); i++ )
      {
        double traj_target_time = i * planning_time / original_tp.trajectory_points.size();
        double dt_dist = polynomial_calc(values, traj_target_time);
        double dv = polynomial_calc_d(values, traj_target_time);
        cav_msgs::TrajectoryPlanPoint jmt_tpp;
        // the last moving trajectory point
        if (dv >= config_.max_stop_speed)
        {
          ROS_DEBUG_STREAM("target speed: " << dv);
          if (dv >= original_max_speed){
            dv = original_max_speed;
          }
          // if (dv >= initial_velocity){
          //   dv = std::max(config_.max_stop_speed, initial_velocity);
          // }
          // trajectory point is copied to move all the available information, then its target time is updated
          jmt_tpp = original_tp.trajectory_points[i];
          jmt_tpp.target_time = jmt_trajectory_points[i-1].target_time + ros::Duration(original_traj_downtracks[i]/dv);
          jmt_trajectory_points.push_back(jmt_tpp);
        }
        else
        {
          ROS_DEBUG_STREAM("target speed is zero");
          // if speed is zero, the vehicle will stay in previous location.
          jmt_tpp = jmt_trajectory_points[i-1];
          jmt_tpp.target_time = jmt_trajectory_points[0].target_time + ros::Duration(traj_target_time);
          jmt_trajectory_points.push_back(jmt_tpp);
        }
      }
    jmt_trajectory.header = original_tp.header;
    jmt_trajectory.trajectory_id = original_tp.trajectory_id;
    jmt_trajectory.trajectory_points = jmt_trajectory_points;
    return jmt_trajectory;
  }

  cav_msgs::TrajectoryPlan YieldPlugin::update_traj_for_object(const cav_msgs::TrajectoryPlan& original_tp, double initial_velocity)
  {

    cav_msgs::TrajectoryPlan update_tpp_vector;
    geometry_msgs::Twist current_velocity;
    current_velocity.linear.x = initial_velocity;

    std::vector<cav_msgs::RoadwayObstacle> rwol = wm_->getRoadwayObjects();
    cav_msgs::RoadwayObstacleList rwol2;
    rwol2.roadway_obstacles = rwol;
    host_vehicle_size.x = config_.vehicle_length;
    host_vehicle_size.y = config_.vehicle_width;
    host_vehicle_size.z = config_.vehicle_height;
    std::vector<cav_msgs::RoadwayObstacle> rwol_collision = carma_wm::collision_detection::WorldCollisionDetection(rwol2, original_tp, host_vehicle_size, current_velocity, config_.collision_horizon);

    ROS_DEBUG_STREAM("Roadway Object List (rwol) size: " << rwol.size());

    // correct the input types
    if(!rwol_collision.empty())
    {
      ROS_WARN_STREAM("Collision Detected!");

      // Distance from the original trajectory point to the lead vehicle/object
      double dist_x = rwol_collision[0].object.pose.pose.position.x - original_tp.trajectory_points[0].x;
      double dist_y = rwol_collision[0].object.pose.pose.position.y - original_tp.trajectory_points[0].y;
      double x_lead = sqrt(dist_x*dist_x + dist_y*dist_y);

      // roadway object position
      double gap_time = (x_lead - config_.x_gap)/initial_velocity;

      double collision_time = 0; //\TODO comming from carma_wm collision detection in future (CAR 4288)

      double goal_velocity = rwol_collision[0].object.velocity.twist.linear.x;
      // determine the safety inter-vehicle gap based on speed
      double safety_gap = std::max(goal_velocity * gap_time, config_.x_gap);
      if (config_.enable_adjustable_gap)
      {
        // check if a digital_gap is available
        double digital_gap = check_traj_for_digital_min_gap(original_tp);
        // if a digital gap is available, it is replaced as safety gap
        safety_gap = std::max(safety_gap, digital_gap);
      }
      // safety gap is implemented
      double goal_pos = x_lead - safety_gap;

      if (goal_velocity <= config_.min_obstacle_speed){
        ROS_WARN_STREAM("The obstacle is not moving");
      }

      double initial_time = 0;
      double initial_pos = 0.0; //relative initial position (first trajectory point)

      double initial_accel = 0;
      double goal_accel = 0;

      double delta_v_max = fabs(rwol_collision[0].object.velocity.twist.linear.x - max_trajectory_speed(original_tp.trajectory_points));
      // reference time, is the maximum time available to perform object avoidance (length of a trajectory)
      double t_ref = (original_tp.trajectory_points[original_tp.trajectory_points.size() - 1].target_time.toSec() - original_tp.trajectory_points[0].target_time.toSec());
      // time required for comfortable deceleration
      double t_ph = config_.acceleration_adjustment_factor * delta_v_max / config_.yield_max_deceleration;

      // planning time for object avoidance
      double tp = 0;

      if(t_ph > config_.tpmin && t_ref > t_ph)
      {
        tp = t_ph;
      }
      else if(t_ph < config_.tpmin)
      {
        tp = config_.tpmin;
      }
      else
      {
        tp = t_ref;
      }

      ROS_DEBUG_STREAM("Object avoidance planning time: " << tp);

      update_tpp_vector = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, tp);

      return update_tpp_vector;
    }
    ROS_DEBUG_STREAM("No collision detection, so trajectory not modified.");
    return original_tp;
  }

  std::vector<double> YieldPlugin::get_relative_downtracks(const cav_msgs::TrajectoryPlan& trajectory_plan) const
  {
    std::vector<double> downtracks;
    downtracks.reserve(trajectory_plan.trajectory_points.size());
    // relative downtrack distance of the fist point is 0.0
    downtracks[0] = 0.0;
    for (size_t i=1; i < trajectory_plan.trajectory_points.size(); i++){
      double dx = trajectory_plan.trajectory_points[i].x - trajectory_plan.trajectory_points[i-1].x;
      double dy = trajectory_plan.trajectory_points[i].y - trajectory_plan.trajectory_points[i-1].y;
      downtracks[i] = sqrt(dx*dx + dy*dy);
    }
    return downtracks;
  }

  double YieldPlugin::polynomial_calc(std::vector<double> coeff, double x) const
  {
    double result = 0;
    for (size_t i = 0; i < coeff.size(); i++)
    {
      double value = coeff[i] * pow(x, (int)(coeff.size() - 1 - i));
      result = result + value;
    }
    return result;
  }

  double YieldPlugin::polynomial_calc_d(std::vector<double> coeff, double x) const
  {
    double result = 0;
    for (size_t i = 0; i < coeff.size()-1; i++)
    {
      double value = (int)(coeff.size() - 1 - i) * coeff[i] * pow(x, (int)(coeff.size() - 2 - i));
      result = result + value;
    }
    return result;
  }

  double YieldPlugin::max_trajectory_speed(const std::vector<cav_msgs::TrajectoryPlanPoint>& trajectory_points) const
  {
    double max_speed = 0;
    for(size_t i = 0; i < trajectory_points.size() - 2; i++ )
    {
      double dx = trajectory_points[i + 1].x - trajectory_points[i].x;
      double dy = trajectory_points[i + 1].y - trajectory_points[i].y;
      double d = sqrt(dx*dx + dy*dy);
      double t = (trajectory_points[i + 1].target_time.toSec() - trajectory_points[i].target_time.toSec());
      double v = d/t;
      if(v > max_speed)
      {
        max_speed = v;
      }
    }
    return max_speed;
  }

  double YieldPlugin::check_traj_for_digital_min_gap(const cav_msgs::TrajectoryPlan& original_tp) const
  {
    double desired_gap = 0;

    for (size_t i = 0; i < original_tp.trajectory_points.size(); i++)
    {
      lanelet::BasicPoint2d veh_pos(original_tp.trajectory_points[i].x, original_tp.trajectory_points[i].y);
      auto llts = wm_->getLaneletsFromPoint(veh_pos, 1);
      if (llts.empty())
      {
        ROS_WARN_STREAM("Trajectory point: " << original_tp.trajectory_points[i]);
        throw std::invalid_argument("Trajectory point is not on a valid lanelet.");
      }
      auto digital_min_gap = llts[0].regulatoryElementsAs<lanelet::DigitalMinimumGap>(); //Returns a list of these elements)
      if (!digital_min_gap.empty())
      {
        double digital_gap = digital_min_gap[0]->getMinimumGap(); // Provided gap is in meters
        ROS_DEBUG_STREAM("Digital Gap found with value: " << digital_gap);
        desired_gap = std::max(desired_gap, digital_gap);
      }
    }
    return desired_gap;
  }

}  // namespace yield_plugin


#include "Scenario.hpp"
#include <numeric/Combinatorics.hpp>
#include <numeric/LimitedCombination.hpp>
#include <iostream>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
#include "../ModelPool.hpp"

using namespace numeric;

namespace multiagent {
namespace ccf {

Scenario::Scenario()
{
    mInterfaceCompatibilityTypes.push_back('m');
    mInterfaceCompatibilityTypes.push_back('f');
}

Scenario Scenario::fromConsole()
{
    Scenario scenario;

    size_t numberOfActorTypes;
    std::cout << "How many different actor types are being used:" << std::endl;
    std::cin >> numberOfActorTypes;

    char currentActorType = 'a';
    std::cout << "Please provide information about the actor types: " << std::endl;

    for(size_t i = 0; i < numberOfActorTypes; ++i)
    {
        std::cout << "The available number of items per type:" << std::endl;

        size_t numberPerActorType;
        std::cout << currentActorType << ": ";
        std::cin >> numberPerActorType;

        ActorDescription actorDescription;

        size_t maleEmiCount;
        std::cout << "The available number of EMI+ for this type:" << std::endl;
        std::cout << "Male EMI: " << std::endl;
        std::cin >> maleEmiCount;
        actorDescription.setInterfaceCount('m',maleEmiCount);

        size_t femaleEmiCount;
        std::cout << "The available number of EMI- for this type:" << std::endl;
        std::cout << "Female EMI: " << std::endl;
        std::cin >> femaleEmiCount;
        actorDescription.setInterfaceCount('f',femaleEmiCount);
        scenario.mActorTypes.push_back(currentActorType);

        uint8_t localActorId = 0;
        // Add interfaces per actor
        for(size_t i = 0; i < numberPerActorType; ++i)
        {
            Actor actor(currentActorType, localActorId++);
            scenario.mActors[actor] = actorDescription;
        }

        scenario.setModelCount(currentActorType, numberPerActorType);
        ++currentActorType;
    }

    return scenario;
}

Scenario Scenario::fromFile(const std::string& filename)
{
    Scenario spec;
    std::ifstream specFile(filename);
    if(specFile.is_open())
    {
        std::string line;
        std::string delimiter = " ";
        size_t actorType = 0;
        while(getline(specFile, line))
        {
            // if line start with # then this is a comment
            if(line.find("#") == 0 || line.empty())
            {
                continue;
            }

            // column is
            // <# of instances> <# of male interfaces> <# of female interfaces>
            boost::char_separator<char> sep(" ");
            boost::tokenizer< boost::char_separator<char> > tokens(line, sep);

            std::vector<std::string> columns(tokens.begin(), tokens.end());
            if(columns.size() != 3)
            {
                std::invalid_argument("moreorg::ccf::Scenario: invalid column: '" + line + "' in spec");
            }

            size_t numberOfInstances = boost::lexical_cast<size_t>(columns.at(0));
            size_t numberOfMaleInterfaces = boost::lexical_cast<size_t>(columns.at(1));
            size_t numberOfFemaleInterfaces = boost::lexical_cast<size_t>(columns.at(2));

            ActorDescription actorDescription;
            actorDescription.setInterfaceCount('m', numberOfMaleInterfaces);
            actorDescription.setInterfaceCount('f', numberOfFemaleInterfaces);

            char actorTypeId = 'a' + static_cast<char>(actorType);
            spec.mActorTypes.push_back(actorTypeId);

            for(size_t i = 0; i < numberOfInstances; ++i)
            {
                Actor actor(actorTypeId, i);
                spec.mActors[actor] = actorDescription;
            }
            spec.setModelCount(actorTypeId, numberOfInstances);

            ++actorType;
        }
        specFile.close();
    } else {
        throw std::runtime_error("moreorg::ccf::Scenario: failed to load spec from: " + filename );
    }

    return spec;
}

void Scenario::compute(size_t maxCoalitionSize)
{
    mMaxCoalitionSize = maxCoalitionSize;

    std::map<Actor, ActorDescription>::const_iterator ait = mActors.begin();
    for(; ait != mActors.end(); ++ait)
    {
        Actor actor = ait->first;
        ActorDescription actorDescription = ait->second;

        LocalInterfaceId localInterfaceId = 0;

        // Collect all interfaces (actor, localinterface, interface type)
        // Add interfaces per actor
        std::vector<CompatibilityType>::const_iterator iit = mInterfaceCompatibilityTypes.begin();
        for(; iit != mInterfaceCompatibilityTypes.end(); ++iit)
        {
            // Initialize interfaces
            CompatibilityType interfaceType = *iit;
            for(uint8_t i = 0; i < actorDescription.getInterfaceCount(interfaceType); ++i)
            {
                Interface interface(actor,localInterfaceId++, interfaceType);
                mInterfaces.push_back(interface);
            }
        }

        mActorList.push_back(actor);
    }

    // Compute the maximum number of actor combinations
    Combination<Actor> combination(mActorList, mMaxCoalitionSize, MAX);
    mNumberOfActorCombinations = combination.numberOfCombinations();

    createLinks();
}

void Scenario::createLinks()
{
    std::vector<Link> links;

    // Using combination of links, automatically accounts for an undirected
    // link graph
    Combination<Interface> combinations(mInterfaces, 2, numeric::EXACT);

    // Compute all invalid and possibly(!) valid links
    do {
        std::vector<Interface> interfaceCombination = combinations.current();
        Link link(interfaceCombination[0], interfaceCombination[1] );
        // Same type is not compatible
        if(interfaceCombination[0].getCompatibilityType() == interfaceCombination[1].getCompatibilityType())
        {
            mInvalidLinks.push_back(link);
            mInvalidLinkTypes.insert(link.getType());
        } else {
            links.push_back( link );
        }
    } while(combinations.next());

    // Check possibly valid links and
    // consider only links as valid when they do not
    // violate general constraints, e.g., disallowing self-referencing
    for(const Link& link : links)
    {
        if(link.getFirstActor() == link.getSecondActor())
        {
            mInvalidLinks.push_back(link);
            mInvalidLinkTypes.insert(link.getType());
            continue;
        }

        // Valid link found
        mValidLinks.push_back(link);
        mValidLinkTypes[link.getType()]++;

        // Create link groups, i.e. map pair of connecting agents to the link
        mLinkGroupMap[link.getGroup()].insert(link);
        // cache link groups, i.e. pairs of linked agents
        mAvailableLinkGroups.insert(link.getGroup());

        // Create interface mapping: map interface to the link it is involved
        mInterfaceLinkMap[link.getFirstInterface()].insert(link);
        mInterfaceLinkMap[link.getSecondInterface()].insert(link);

        // Create actor mapping: map actor to the number of links
        mActorLinkMap[link.getFirstActor()].insert(link);
        mActorLinkMap[link.getSecondActor()].insert(link);
    }

    moreorg::ModelPool::Set agentSpaceModelPools;
    std::set< std::set<Link> > linkSpaceModelPools;

    int count = 0;
    numeric::LimitedCombination<char> linkCombinations(mModelPool, mMaxCoalitionSize, numeric::MAX);
    do {
        ++count;
        std::vector<char> agentTypes = linkCombinations.current();
        if(agentTypes.size() == 1)
        {
            continue;
        }

        // Create a representative actor combination for this type
        std::vector<Actor> actors = mActorList;
        std::vector<Actor> representativeActor;
        for(char agentType: agentTypes)
        {
            std::vector<Actor>::iterator it = std::find_if(actors.begin(), actors.end(), [agentType](const Actor& actor)
                    {
                        return actor.getType() == agentType;
                    });
            representativeActor.push_back(*it);
            actors.erase(it);
        }

        // Find all feasible combinations for this type in link space
     //   std::cout << "Collect: " << count << std::endl;
        size_t coalitionSize = std::min(mMaxCoalitionSize, representativeActor.size());
        Combination<Actor> actorCombination(representativeActor, coalitionSize, EXACT);
        do {
            std::vector<Actor> actors = actorCombination.current();
    //        std::cout << "Actors: " << actors.size() << std::endl;
            collectCombinations(actors, actors, std::set<Link>(), agentSpaceModelPools, linkSpaceModelPools);
        } while(actorCombination.next());
    } while(linkCombinations.next());

    mNumberOfActorTypesTheoreticalBound = count;
    mNumberOfActorTypesAgentSpace = agentSpaceModelPools.size() + mActorTypes.size();
    mNumberOfActorTypesLinkSpace = linkSpaceModelPools.size() + mActorTypes.size();

    std::cout << "Max: " << mMaxCoalitionSize << std::endl;
    std::cout << "ModelPool: Agents " << agentSpaceModelPools.size() << std::endl << moreorg::ModelPool::toString(agentSpaceModelPools) << std::endl;
    //std::cout << "ModelPool: Link " << linkSpaceModelPools << std::endl;
}

void Scenario::collectCombinations(
        const std::vector<Actor>& allowedActors,
        std::vector<Actor> actors, std::set<Link> links,
        moreorg::ModelPool::Set& agentSpaceModelPools,
        std::set< std::set<Link> >& linkSpaceSets
        )
{
    if(actors.size() == 1)
    {
        actors.clear();

        std::set<Actor> actors;
        for(const Link& l : links)
        {
            actors.insert(l.getFirstActor());
            actors.insert(l.getSecondActor());
        }

        if(actors.size() == links.size() + 1)
        {
            moreorg::ModelPool agentSpaceAgentType;
            moreorg::ModelPool linkSpaceAgentType;
            for(Actor actor : actors)
            {
                std::string id;
                {
                    id += (char) actor.getType();
                    agentSpaceAgentType["http://actor-type#" + id] += 1;
                }
            }
            agentSpaceModelPools.insert(agentSpaceAgentType);
            linkSpaceSets.insert(links);
        }
    } else {
        std::set<Link> actorLinks = mActorLinkMap[actors.back()];
        std::set<Link> allowedLinks;
        for(const Link& l : actorLinks)
        {
            Actor a0 = l.getFirstActor();
            Actor a1 = l.getSecondActor();

            if( actors.end() != std::find(allowedActors.begin(), allowedActors.end(), a0) )
            {
                if(actors.end() != std::find(allowedActors.begin(), allowedActors.end(), a1))
                {
                    allowedLinks.insert(l);
                }
            }
        }
        actors.pop_back();

        //std::cout << "ActorLinks: " << allowedLinks.size() << std::endl;
        //for(const Link& l : allowedLinks)
        //{
        //    std::cout << "    " << l << std::endl;
        //}

        for(const Link& link : allowedLinks)
        {
            std::set<Link> addedLink = links;
            addedLink.insert(link);

            collectCombinations(allowedActors, actors, addedLink, agentSpaceModelPools, linkSpaceSets);
        }
    }
}

std::vector<LinkType> Scenario::getValidLinkTypeList() const
{
    std::map<LinkType, size_t>::const_iterator cit = mValidLinkTypes.begin();
    std::vector<LinkType> linkTypes;
    for(; cit != mValidLinkTypes.end(); ++cit)
    {
        linkTypes.push_back(cit->first);
    }
    return linkTypes;
}

std::string Scenario::rowDescription() const
{
    std::stringstream ss;
    ss << "#";
    ss << " <max coalition size>";
    ss << " <actors>";
    ss << " <agent space: composite actors>";
    ss << " <interfaces>";
    ss << " <valid links>";
    ss << " <invalid links>";
    ss << " <link space: bound on composite actors>";
    ss << " <agent space: composite actor types>";
    ss << " <link space: composite actor types>";
    ss << " <agent space: bound on composite actor types>";
    ss << std::endl;
    return ss.str();
}

std::string Scenario::report() const
{
    std::stringstream ss;
    ss << mMaxCoalitionSize;
    ss << " " << mActors.size();
    ss << " " << mNumberOfActorCombinations;
    ss << " " << mInterfaces.size();
    ss << " " << mValidLinks.size();
    ss << " " << mInvalidLinks.size();
    ss << " " << mNumberOfLinkCombinations;
    ss << " " << mNumberOfActorTypesAgentSpace;
    ss << " " << mNumberOfActorTypesLinkSpace;
    ss << " " << mNumberOfActorTypesTheoreticalBound;
    ss << std::endl;
    return ss.str();
}

std::ostream& operator<<(std::ostream& os, const Scenario& scenario)
{
    os << "# actors:                   " << scenario.mActors.size() << std::endl;
    os << "# max actor combos:         " << scenario.mNumberOfActorCombinations << std::endl;
    os << "# interfaces:               " << scenario.mInterfaces.size() << std::endl;
    os << "# valid links:              " << scenario.mValidLinks.size() << std::endl;
    os << "# invalid links:            " << scenario.mInvalidLinks.size() << std::endl;
    os << "# valid link combos:        " << scenario.mNumberOfLinkCombinations << std::endl;
    os << "# valid link types:         " << scenario.mValidLinkTypes.size() << std::endl;
    os << "# valid agents (agent space)" << scenario.mNumberOfActorTypesAgentSpace << std::endl;
    os << "# valid agents (link space) " << scenario.mNumberOfActorTypesLinkSpace << std::endl;

    std::map<LinkType, size_t>::const_iterator cit = scenario.mValidLinkTypes.begin();
    for(; cit != scenario.mValidLinkTypes.end(); ++cit)
    {
        os << "    " << cit->first << " -- " << cit->second << std::endl;
    }
    os << "# invalid link types: " << scenario.mInvalidLinkTypes.size() << std::endl;
    return os;
}

} // end namespace caf
} // end namespace multiagent


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program 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 of the License, or
|    (at your option) any later version.
|
|    This program is distributed in the hope that it will be useful,
|    but WITHOUT ANY WARRANTY; without even the implied warranty of
|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include <cmath>
#include <iomanip>
#include <algorithm>
#include <map>
#include <fstream>
#include <stdexcept>

#define SHA1DETAIL_INLINE_IMPL
#include <hashclash/saveload_bz2.hpp>
#include <hashclash/sha1detail.hpp>
#include <hashclash/sha1differentialpath.hpp>
#include <hashclash/booleanfunction.hpp>
#include <hashclash/rng.hpp>
#include <hashclash/timer.hpp>
#include <hashclash/bestof.hpp>
#include <hashclash/progress_display.hpp>
#include <hashclash/sha1messagespace.hpp>

#include <boost/lexical_cast.hpp>

#include "main.hpp"

sha1differentialpath maindiffpath;
sha1messagespace mainmespace;

uint32 m_diff[80];

bool throw_at_t16 = false;
uint32 firstmsg[16];

vector< uint32 > Qtp1valsvec[30];

vector< vector< vector<uint32> > > bitrelations;
vector< vector<uint32> > pathbitrelations;
vector< vector< vector<uint32> > > pathbitrelationsmatrix;

vector< vector<uint32> > firstmsg_vec;

void filter_bitconditions(vector< vector<uint32> >& bitrels, unsigned tbegin, unsigned tend)
{
	sweep_matrix(bitrels,80*32);
	vector<uint32> triv(81,0), impos(81,0);
	impos[80] = ~uint32(0);
	unsigned i = 0;
	while (i < bitrels.size()) {
		if (bitrels[i] == triv) {
			swap(bitrels[i], bitrels.back());
			bitrels.pop_back();
			continue;
		}
		if (bitrels[i] == impos)
			throw std::runtime_error("filter_bitconditions: contradicting bitrelations");

		int lastt = -1;
		for (int j = 0; j < bitrels[i].size() && j < 80; ++j)
			if (bitrels[i][j])
				lastt = j;
		if (lastt == -1)
			throw std::runtime_error("filter_bitconditions: huh?!?");
		if (lastt < tbegin || lastt >= tend) {
			swap(bitrels[i], bitrels.back());
			bitrels.pop_back();
		} else
			++i;
	}
}

int collisionfinding(parameters_type& parameters)
{
	bool usetunnelbitconditions = parameters.usetunnelbitconditions;

	sha1messagespace tmpspace;
	vector< vector<uint32> > bitrels, tmpbitrel, tmpbitrel2;
	for (unsigned i = 0; i < parameters.rnd234_m_bitrelationfiles.size(); ++i) {
		try {
			cout << "Loading '" << parameters.rnd234_m_bitrelationfiles[i] << "'..." << flush;
			load_bz2(tmpspace, text_archive, parameters.rnd234_m_bitrelationfiles[i]);
			cout << "done" << flush;
			tmpspace.tobitrelations_80(tmpbitrel);
			bitrels.insert(bitrels.end(), tmpbitrel.begin(), tmpbitrel.end());
			cout << " (" << tmpbitrel.size() << " bitrels, new total: " << bitrels.size() << ")" << endl;
		} catch (std::exception& e) {
			cerr << "Exception:" << endl << e.what() << endl;
			return 1;
		}
	}
	vector< sha1differentialpath > paths;

	cout << "Loading round 1 paths from '" << parameters.rnd1_pathsfile << "'..." << flush;
	try {
		load_bz2(paths, text_archive, parameters.rnd1_pathsfile);
		cout << "done (" << paths.size() << " paths)." << endl;
	} catch (std::exception& e) {
		cerr << "Exception:" << endl << e.what() << endl;
		return 1;
	}
	if (paths.size() == 0) exit(0);

	vector< vector<uint32> > bitrelsbu = bitrels;
for (unsigned pi = 0; pi < paths.size(); ++pi) { try {
	bitrels = bitrelsbu;
	sha1differentialpath upperpath;
	cout << "Loading round 2,3,4 path from '" << parameters.rnd234_pathfile << "'..." << flush;
	try {
		load_bz2(upperpath, text_archive, parameters.rnd234_pathfile);
		cout << "done." << endl;
	} catch (std::exception& e) {
		cerr << "Exception:" << endl << e.what() << endl;
		return 1;
	}
	for (int t = paths[pi].tbegin(); t < paths[pi].tend(); ++t)
		upperpath[t] = paths[pi][t];
	for (unsigned t = 0; t < paths[pi].tend()-1; ++t)
		upperpath.getme(t) = paths[pi].getme(t);
	cleanup_path(upperpath);
	maindiffpath = upperpath;

	bool bad = false;
	for (int t = maindiffpath.tbegin()+4; t < maindiffpath.tend()-1; ++t) {
		if (maindiffpath.getme(t).mask != parameters.m_mask[t]) {
			uint32 dm = maindiffpath.getme(t).adddiff();
			uint32 mcur = 0;
			uint32 mmask = parameters.m_mask[t];
			uint32 madd = (~mmask)+1;
			sdr msdr;
			msdr.mask = mmask;
			do {
				mcur += madd; mcur &= mmask;
				msdr.sign = mcur;
				if (msdr.adddiff() == dm) {
					cout << "corrected: \t" << t << ":\t" << msdr << " == " << sdr(parameters.m_mask[t]) << endl;
					break;
				}
			} while (mcur != 0);
			if (msdr.adddiff() == dm) {
				maindiffpath.getme(t) = msdr;
			} else {
				bad = true;
				cout << "failed: \t" << t << ":\t" << maindiffpath.getme(t) << " != " << sdr(parameters.m_mask[t]) << endl;
			}
		}
	}
	if (bad) exit(0);
	show_path(maindiffpath);

	// remove bitrelations possibly limiting me[0],...,me[19]
	const unsigned tend_fix_me = parameters.tend_rnd1_me;
	cout << "Fixed me diffs for t=[0," << tend_fix_me << "): (" << bitrels.size() << "=>" << flush;
	filter_bitconditions(bitrels, tend_fix_me, 80);
	cout << bitrels.size() << ")" << endl;
	for (unsigned i = 0; i < bitrels.size(); ++i) {
		cout << " - ";
		bool firstone = true;
		for (unsigned t = 0; t < 80; ++t)
			for (unsigned b = 0; b < 32; ++b)
				if (bitrels[i][t] & (1<<b)) {
					if (firstone)
						firstone = false;
					else
						cout << " + ";
					cout << "M[" << t << "," << b << "]";
				}
		cout << " = " << (bitrels[i][80]&1) << endl;
	}

	tmpspace.clear();
	for (unsigned t = 0; t < 80; ++t) {
		if (t < tend_fix_me) {
			for (unsigned b = 0; b < 31; ++b) {
				int bit = maindiffpath.getme(t).get(b);
				if (bit == 0)
					tmpspace.buildbasis_addfreebit(t,b);
				else
					tmpspace.buildbasis_setbit(t,b,bit==-1);
			}
			tmpspace.buildbasis_addfreebit(t,31);
		} else {
			for (unsigned b = 0; b < 32; ++b)
				tmpspace.buildbasis_addfreebit(t,b);
		}
	}
	tmpspace.tobitrelations_80(tmpbitrel);
	bitrels.insert(bitrels.end(), tmpbitrel.begin(), tmpbitrel.end());
	cout << "Extra me [0," << tend_fix_me << ") bitrelations: " << tmpbitrel.size() << endl;
	{
		cout << "Extra tunnel me bitrelations: " << endl;
		ifstream ifs("tunnel_bitconditions.txt");
		read_message_bitconditions(ifs, tmpbitrel);
		for (unsigned i = 0; i < tmpbitrel.size(); ++i) {
			bool firstone = true;
			for (unsigned t = 0; t < 80; ++t)
				if (tmpbitrel[i][t]) {
					if (firstone) firstone = false;
					else cout << "+ ";
					cout << "m" << t << sdr(tmpbitrel[i][t]) << " ";
				}
			cout << "= " << (tmpbitrel[i][80]&1) << endl;
		}
		bitrels.insert(bitrels.end(), tmpbitrel.begin(), tmpbitrel.end());
	}
	tmpspace.frombitrelations_80(bitrels);
	mainmespace = tmpspace;
	tmpspace.tobitrelations_16(bitrels);
	cout << "Total bitrelations: " << bitrels.size() << endl;
#if 0
	cout << "Independent message bits: " << endl;
	uint32 meindep[16];
	for (unsigned i = 0; i < 16; ++i)
		meindep[16] = ~uint32(0);
	for (unsigned i = 0; i < bitrels.size(); ++i)
		for (unsigned t = 0; t < 16; ++t)
			meindep[t] &= ~bitrels[i][t];
	for (unsigned i = 0; i < 16; ++i) {
		cout << "m[" << i << "] indep:\t";
		for (int b = 31; b >= 0; --b)
			cout << ((meindep[i]&(1<<b))?"1":"0");
		cout << endl;
	}
#endif
	pathbitrelations = bitrels;
	pathbitrelationsmatrix.clear();
	pathbitrelationsmatrix.resize(16);
	for (unsigned i = 0; i < 16; ++i)
		pathbitrelationsmatrix[i].resize(32);
	for (unsigned i = 0; i < pathbitrelations.size(); ++i) {
		int lastcol = -1;
		for (int col = 16*32-1; col >= 0; --col)
			if (pathbitrelations[i][col>>5]&(1<<(col&31))) {
				lastcol = col;
				unsigned t = lastcol>>5;
				unsigned b = lastcol&31;
				pathbitrelationsmatrix[t][b] = pathbitrelations[i];
				break;
			}
		if (lastcol == -1) throw;
	}
#if 0
	//needs diffpath & pathbitrelationsmatrix
	analyze_indepsection_prob();
#endif
	if (parameters.tunnelfile.size()) {
		vector<sha1differentialpath> tunnels;
		cout << "Loading tunnels from '" << parameters.tunnelfile << "'..." << flush;
		try {
			load_bz2(tunnels, text_archive, parameters.tunnelfile);
		}
		catch (std::exception &e) { tunnels.clear(); cout << "failed:" << endl << e.what() << endl; }
		catch (...) { tunnels.clear(); cout << "failed." << endl; }
		if (tunnels.size())
			analyze_tunnels_diffpath(maindiffpath, bitrels, tunnels);
		exit(0);
	}
	if (!usetunnelbitconditions) {
		// if not using tunnel bit conditions then we'll assume we want to analyze tunnels
		analyze_tunnels_diffpath(maindiffpath, bitrels);
		exit(0);
	}
	// if we're using tunnel bit conditions we'll assume we want to generate the collision finding program

	generate_program();
	break;
} catch (std::exception& e) { cerr << "c: " << e.what() << endl; } catch (...) {}
} // for (unsigned pi = 0; pi < paths.size(); ++pi)
}


/**
 * Copyright (c) 2011, 2012
 * Claudio Kopper <claudio.kopper@icecube.wisc.edu>
 * and the IceCube Collaboration <http://www.icecube.wisc.edu>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 *
 * $Id: I3CLSimModuleHelper.cxx 178168 2019-12-19 21:40:33Z jvansanten $
 *
 * @file I3CLSimModuleHelper.cxx
 * @version $Revision: 178168 $
 * @date $Date: 2019-12-19 14:40:33 -0700 (Thu, 19 Dec 2019) $
 * @author Claudio Kopper
 */

#ifndef __STDC_FORMAT_MACROS
#define __STDC_FORMAT_MACROS
#endif
#include <inttypes.h>

#include "clsim/I3CLSimModuleHelper.h"

#include "clsim/function/I3CLSimFunctionConstant.h"
#include "clsim/function/I3CLSimFunctionFromTable.h"
#include "clsim/function/I3CLSimFunctionDeltaPeak.h"
#include "clsim/random_value/I3CLSimRandomValueInterpolatedDistribution.h"
#include "clsim/random_value/I3CLSimRandomValueWlenCherenkovNoDispersion.h"
#include "clsim/random_value/I3CLSimRandomValueConstant.h"

#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/variant/get.hpp>

#include "dataclasses/physics/I3MCTree.h"
#include "dataclasses/physics/I3MCTreeUtils.h"

namespace I3CLSimModuleHelper {

    namespace {
        double CherenkovYieldDistribution(double wlen, I3CLSimMediumPropertiesConstPtr mediumProperties, double beta=1.)
        {
            I3CLSimFunctionConstPtr nPhaseDist =
            mediumProperties->GetPhaseRefractiveIndex(0); // this assumes the refractive index does not change between layers

            if (!nPhaseDist->HasNativeImplementation())
                log_fatal("The refractive index distribution needs a native implementation to be usable!");

            const double nPhase = nPhaseDist->GetValue(wlen);

            return (2.*M_PI/(137.*(wlen*wlen)))*(1. - 1./ ( std::pow(beta*nPhase,2.) ) ); // dN/dxdwlen
        }

        // the normalization will not be correct here
        double CherenkovYieldDistributionNoDispersion(double wlen)
        {
            return 1./(wlen*wlen); // dN/dxdwlen
        }

    };

    I3CLSimRandomValueConstPtr
    makeWavelengthGenerator(I3CLSimFunctionConstPtr unbiasedSpectrum,
                            I3CLSimFunctionConstPtr wavelengthGenerationBias,
                            I3CLSimMediumPropertiesConstPtr mediumProperties)
    {
        {
            // special handling for delta peaks
            I3CLSimFunctionDeltaPeakConstPtr deltaPeak =
            boost::dynamic_pointer_cast<const I3CLSimFunctionDeltaPeak>(unbiasedSpectrum);
            if (deltaPeak) {
                const double peakPosition = deltaPeak->GetPeakPosition();

                return I3CLSimRandomValueConstantConstPtr
                (new I3CLSimRandomValueConstant(peakPosition));
            }
        }

        // if we get here, it's not a delta peak

        double minWlen = unbiasedSpectrum->GetMinWlen();
        double maxWlen = unbiasedSpectrum->GetMaxWlen();

        // check if the spectrum is from a tabulated distribution (instead of
        // a parameterized one)
        I3CLSimFunctionFromTableConstPtr unbiasedSpectrumFromTable;
        unbiasedSpectrumFromTable =
        boost::dynamic_pointer_cast<const I3CLSimFunctionFromTable>(unbiasedSpectrum);

        if (!unbiasedSpectrumFromTable) {
            // do not clip wavelengths if they are from a tabulated distribution. In that case,
            // re-use the entire table binning
            if (mediumProperties->GetMinWavelength() > minWlen) minWlen=mediumProperties->GetMinWavelength();
            if (mediumProperties->GetMaxWavelength() < maxWlen) maxWlen=mediumProperties->GetMaxWavelength();
        }

        const double wlenRange = maxWlen-minWlen;
        if (wlenRange <= 0.) log_fatal("Internal error, wavelength range <= 0!");

        if (wavelengthGenerationBias->GetMinWlen() > minWlen)
            log_fatal("wavelength generation bias has to have a wavelength range larger or equal to the spectrum wavelength range!");
        if (wavelengthGenerationBias->GetMaxWlen() < maxWlen)
            log_fatal("wavelength generation bias has to have a wavelength range larger or equal to the spectrum wavelength range!");

        // Check if the spectrum values are from a tabulated distribution.
        // If yes, use the table binning, if not make up a binning.
        if (unbiasedSpectrumFromTable)
        {
            std::size_t wlenPoints = unbiasedSpectrumFromTable->GetNumEntries();
            const double firstWlen = unbiasedSpectrumFromTable->GetFirstWavelength();

            std::vector<double> spectrum(wlenPoints, NAN);
            std::vector<double> wavelengths(wlenPoints, NAN);
            for (std::size_t i=0;i<wlenPoints;++i)
            {
                const double wavelength = unbiasedSpectrumFromTable->GetEntryWavelength(i);
                const double entry = unbiasedSpectrumFromTable->GetEntryValue(i);
                const double bias = wavelengthGenerationBias->GetValue(wavelength);

                spectrum[i] = bias * entry;
                wavelengths[i] = wavelength;
            }

            if (unbiasedSpectrumFromTable->GetInEqualSpacingMode()) {
                const double wlenStep = unbiasedSpectrumFromTable->GetWavelengthStepping();

                return I3CLSimRandomValueInterpolatedDistributionConstPtr
                (new I3CLSimRandomValueInterpolatedDistribution(firstWlen,
                                                                wlenStep,
                                                                spectrum));
            } else {
                // slightly less efficient if non-equally spaced
                return I3CLSimRandomValueInterpolatedDistributionConstPtr
                (new I3CLSimRandomValueInterpolatedDistribution(wavelengths,
                                                                spectrum));
            }
        }
        else
        {
            // use a pre-defined binning of 10ns (an arbitrary value..)

            std::size_t wlenPoints = static_cast<std::size_t>(wlenRange/(10.*I3Units::nanometer))+2;
            const double firstWlen = minWlen;
            const double wlenStep = wlenRange/static_cast<double>(wlenPoints-1);

            std::vector<double> spectrum(wlenPoints, NAN);
            for (std::size_t i=0;i<wlenPoints;++i)
            {
                const double wavelength = firstWlen + static_cast<double>(i)*wlenStep;
                const double entry = unbiasedSpectrum->GetValue(wavelength);
                const double bias = wavelengthGenerationBias->GetValue(wavelength);

                spectrum[i] = bias * entry;
            }

            return I3CLSimRandomValueInterpolatedDistributionConstPtr
            (new I3CLSimRandomValueInterpolatedDistribution(firstWlen,
                                                            wlenStep,
                                                            spectrum));
        }
    }

    I3CLSimRandomValueConstPtr
    makeCherenkovWavelengthGenerator(I3CLSimFunctionConstPtr wavelengthGenerationBias,
                                     bool generateCherenkovPhotonsWithoutDispersion,
                                     I3CLSimMediumPropertiesConstPtr mediumProperties)
    {
        const double minWlen = mediumProperties->GetMinWavelength();
        const double maxWlen = mediumProperties->GetMaxWavelength();
        const double wlenRange = maxWlen-minWlen;
        if (wlenRange <= 0.) log_fatal("Internal error, wavelength range <= 0!");

        if (wavelengthGenerationBias->GetMinWlen() > minWlen)
            log_fatal("wavelength generation bias has to have a wavelength range larger or equal to the medium property range!");
        if (wavelengthGenerationBias->GetMaxWlen() < maxWlen)
            log_fatal("wavelength generation bias has to have a wavelength range larger or equal to the medium property range!");

        bool noBias=false;
        //bool biasIsConstant=false;

        {
            I3CLSimFunctionConstantConstPtr wavelengthGenerationBiasConstant =
            boost::dynamic_pointer_cast<const I3CLSimFunctionConstant>(wavelengthGenerationBias);

            if (wavelengthGenerationBiasConstant)
            {
                //biasIsConstant=true;

                if ( std::abs(wavelengthGenerationBiasConstant->GetValue((minWlen+maxWlen)/2.)-1.) < 1e-10 )
                    noBias=true;
            }
        }

        I3CLSimFunctionFromTableConstPtr wavelengthGenerationBiasFromTable;
        wavelengthGenerationBiasFromTable =
        boost::dynamic_pointer_cast<const I3CLSimFunctionFromTable>(wavelengthGenerationBias);

        if ((!noBias) && (generateCherenkovPhotonsWithoutDispersion))
        {
            log_warn("**********");
            log_warn(" Using the \"GenerateCherenkovPhotonsWithoutDispersion\" option");
            log_warn(" with a biased photon spectrum generation does not yield a performance");
            log_warn(" increase. You might consider turning this option off to get a better");
            log_warn(" approximation of the Cherenkov spectrum.");
            log_warn("**********");
        }

        // Check if the bias values are from a tabulated distribution.
        // If yes, use the table binning, if not make up a binning.
        if (wavelengthGenerationBiasFromTable)
        {
            std::size_t wlenPoints = wavelengthGenerationBiasFromTable->GetNumEntries();
            const double firstWlen = wavelengthGenerationBiasFromTable->GetFirstWavelength();

            std::vector<double> spectrum(wlenPoints, NAN);
            std::vector<double> wavelengths(wlenPoints, NAN);
            for (std::size_t i=0;i<wlenPoints;++i)
            {
                const double wavelength = wavelengthGenerationBiasFromTable->GetEntryWavelength(i);
                const double bias = wavelengthGenerationBiasFromTable->GetEntryValue(i);

                if (generateCherenkovPhotonsWithoutDispersion)
                {
                    spectrum[i] =
                    bias * CherenkovYieldDistributionNoDispersion(wavelength);
                }
                else
                {
                    spectrum[i] =
                    bias * CherenkovYieldDistribution(wavelength, mediumProperties);
                }

                wavelengths[i] = wavelength;
            }

            if (wavelengthGenerationBiasFromTable->GetInEqualSpacingMode()) {
                const double wlenStep = wavelengthGenerationBiasFromTable->GetWavelengthStepping();

                return I3CLSimRandomValueInterpolatedDistributionConstPtr
                (new I3CLSimRandomValueInterpolatedDistribution(firstWlen,
                                                                wlenStep,
                                                                spectrum));
            } else {
                // slightly less efficient if non-equally spaced
                return I3CLSimRandomValueInterpolatedDistributionConstPtr
                (new I3CLSimRandomValueInterpolatedDistribution(wavelengths,
                                                                spectrum));
            }
        }
        else if ((noBias) && (generateCherenkovPhotonsWithoutDispersion))
        {
            return I3CLSimRandomValueWlenCherenkovNoDispersionConstPtr
            (new I3CLSimRandomValueWlenCherenkovNoDispersion(minWlen, maxWlen));
        }
        else
        {
            std::size_t wlenPoints = static_cast<std::size_t>(wlenRange/(10.*I3Units::nanometer))+2;
            const double firstWlen = minWlen;
            const double wlenStep = wlenRange/static_cast<double>(wlenPoints-1);

            std::vector<double> spectrum(wlenPoints, NAN);
            for (std::size_t i=0;i<wlenPoints;++i)
            {
                const double wavelength = firstWlen + static_cast<double>(i)*wlenStep;
                const double bias = wavelengthGenerationBias->GetValue(wavelength);

                if (generateCherenkovPhotonsWithoutDispersion)
                {
                    spectrum[i] =
                    bias * CherenkovYieldDistributionNoDispersion(wavelength);
                }
                else
                {
                    spectrum[i] =
                    bias * CherenkovYieldDistribution(wavelength, mediumProperties);
                }
            }

            return I3CLSimRandomValueInterpolatedDistributionConstPtr
            (new I3CLSimRandomValueInterpolatedDistribution(firstWlen,
                                                            wlenStep,
                                                            spectrum));
        }

    }

    I3CLSimStepToPhotonConverterOpenCLPtr initializeOpenCL(const I3CLSimOpenCLDevice &device,
                                                           I3RandomServicePtr rng,
                                                           I3CLSimSimpleGeometryConstPtr geometry,
                                                           I3CLSimMediumPropertiesConstPtr medium,
                                                           I3CLSimFunctionConstPtr wavelengthGenerationBias,
                                                           const std::vector<I3CLSimRandomValueConstPtr> &wavelengthGenerators,
                                                           bool enableDoubleBuffering,
                                                           bool doublePrecision,
                                                           bool stopDetectedPhotons,
                                                           bool saveAllPhotons,
                                                           double saveAllPhotonsPrescale,
                                                           double fixedNumberOfAbsorptionLengths,
                                                           double pancakeFactor,
                                                           uint32_t photonHistoryEntries,
                                                           uint32_t limitWorkgroupSize)
    {
        I3CLSimStepToPhotonConverterOpenCLPtr conv(new I3CLSimStepToPhotonConverterOpenCL(rng, device.GetUseNativeMath()));

        conv->SetDevice(device);

        conv->SetWlenGenerators(wavelengthGenerators);
        conv->SetWlenBias(wavelengthGenerationBias);

        conv->SetMediumProperties(medium);
        conv->SetGeometry(geometry);

        conv->SetEnableDoubleBuffering(enableDoubleBuffering);
        conv->SetDoublePrecision(doublePrecision);
        conv->SetStopDetectedPhotons(stopDetectedPhotons);
        conv->SetSaveAllPhotons(saveAllPhotons);
        conv->SetSaveAllPhotonsPrescale(saveAllPhotonsPrescale);

        conv->SetFixedNumberOfAbsorptionLengths(fixedNumberOfAbsorptionLengths);
        conv->SetDOMPancakeFactor(pancakeFactor);

        conv->SetPhotonHistoryEntries(photonHistoryEntries);

        conv->Compile();
        //log_trace("%s", conv.GetFullSource().c_str());

        std::size_t maxWorkgroupSize = conv->GetMaxWorkgroupSize();
        if (limitWorkgroupSize!=0) {
            maxWorkgroupSize = std::min(static_cast<std::size_t>(limitWorkgroupSize), maxWorkgroupSize);
        }

        conv->SetWorkgroupSize(maxWorkgroupSize);
        const std::size_t workgroupSize = conv->GetWorkgroupSize();

        // use approximately the given number of work items, convert to a multiple of the workgroup size
        std::size_t maxNumWorkitems = (static_cast<std::size_t>(device.GetApproximateNumberOfWorkItems())/workgroupSize)*workgroupSize;
        if (maxNumWorkitems==0) maxNumWorkitems=workgroupSize;

        conv->SetMaxNumWorkitems(maxNumWorkitems);

        log_info("Using OpenCL device: platform=%s device=%s isgpu=%d",
                  device.GetPlatformName().c_str(),
                  device.GetDeviceName().c_str(),
                  device.IsGPU());
        log_info("maximum workgroup size is %zu", maxWorkgroupSize);
        log_info("configured workgroup size is %zu", workgroupSize);
        if (maxNumWorkitems != device.GetApproximateNumberOfWorkItems()) {
            log_info("maximum number of work items is %zu (user configured was %" PRIu32 ")", maxNumWorkitems, device.GetApproximateNumberOfWorkItems());
        } else {
            log_debug("maximum number of work items is %zu (user configured was %" PRIu32 ")", maxNumWorkitems, device.GetApproximateNumberOfWorkItems());
        }

        conv->Initialize();

        return conv;
    }

}


// File: finetune_classifier.cc
// Author: Karl Moritz Hermann (mail@karlmoritz.com)
// Created: 29-01-2013
// Last Update: Thu 03 Oct 2013 11:26:25 AM BST

// STL
#include <iostream>
#include <algorithm>
#include <random>
#include <chrono>

// Boost
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/parsers.hpp>

// L-BFGS
#include <lbfgs.h>

// Local
#include "finetune_classifier.h"
#include "models.h"
#include "fast_math.h"

using namespace std;
namespace bpo = boost::program_options;

FinetuneClassifier::FinetuneClassifier(RecursiveAutoencoderBase& rae,
    TrainingCorpus& trainC, float lambdaF, float alpha,
    int dynamic_mode, int iterations) : lambda(lambdaF), alpha_rae(alpha),
  mode(dynamic_mode), iterations(iterations), it_count(0), num_batches(100),
  eta(0.1) {

  /***************************************************************************
   *             Define a couple of frequently needed variables              *
   ***************************************************************************/

  train_length = min(rae.config.num_sentences,int(trainC.size()));
  bool use_full_corpus = false;
  if (train_length == 0)
  {
    train_length = trainC.size();
    use_full_corpus = true;
  }

  label_width = rae.config.label_class_size;
  num_label_types = rae.config.num_label_types; // 1

  int multiplier = 1;
  if (mode == 0)
    multiplier = 2;
  if (mode == 3)
    multiplier = 3; // only works for compound test
  if (mode == 4)
    multiplier = 4;
  // Embedding: s1 + s2 + cos_sim(s1,s2) + len(s1) + len(s2) +
  // unigram_overlap(s1,s2) following Blacoe/Lapata 2012
  dynamic_embedding_size = multiplier * rae.config.word_representation_size;

  theta_size_ = dynamic_embedding_size * label_width * num_label_types + label_width * num_label_types;

  trainI_ = new Real[train_length * dynamic_embedding_size]();
  theta_  = new Real[theta_size_];
  WeightVectorType theta(theta_,theta_size_);
  theta.setZero();
  if (true) {
    std::random_device rd;
    std::mt19937 gen(rd());
    //std::mt19937 gen(0);
    float r = sqrt( 6.0 / dynamic_embedding_size);
    std::uniform_real_distribution<> dis(-r,r);
    for (int i=0; i<theta_size_; i++)
      theta(i) = dis(gen);
  }

  Real* ptr = trainI_;
  for (auto i=0; i<train_length; ++i) {
    int j = i;
    if ((not use_full_corpus) and (i%2 == 1))
      j = trainC.size() - i;
    trainData.push_back(VectorLabelPair(WeightVectorType(ptr, dynamic_embedding_size),trainC[j].value));
    mix.push_back(i);
    ptr += dynamic_embedding_size;
  }

  ptr = theta_;
  for (auto i=0; i<num_label_types; ++i) {
    Wcat.push_back(WeightMatrixType(ptr, label_width, dynamic_embedding_size));
    ptr += label_width * dynamic_embedding_size;
  }
  for (auto i=0; i<num_label_types; ++i) {
    Bcat.push_back(WeightVectorType(ptr, label_width));
    Bcat.back().setZero(); // discuss ..
    ptr += label_width;
  }

  /***************************************************************************
   *        Populate train input with forward Propagation and tricks         *
   ***************************************************************************/

#pragma omp parallel for schedule(dynamic)
  for (auto i = 0; i<train_length; ++i)
  {
    int j = i;
    if ((not use_full_corpus) and (i%2 == 1))
      j = trainC.size() - i;

    SinglePropBase* propagator = nullptr;

    Bools bools;
    if(rae.config.tree == TREE_CCG or rae.config.tree == TREE_STANFORD)
      propagator = rae.getSingleProp(trainC[j],0.5,bools);
    assert (propagator != nullptr);

    propagator->forwardPropagate(true);
    propagator->setDynamic(trainData[i].vector,mode);
    //cout << "C: " << trainData[i].vector[0] << endl;

    delete propagator;
  }
}

void FinetuneClassifier::evaluate()
{

  vector<VectorLabelPair>& data = trainData;
  int length = train_length;

  int right = 0;
  int wrong = 0;
  int tp = 0;
  int fp = 0;
  int tn = 0;
  int fn = 0;

#pragma omp parallel for schedule(dynamic)
  for (auto i = 0; i<length; ++i)
  {

    // Encode input
    ArrayReal label_vec = (
        Wcat[0] * data[i].vector + Bcat[0]
        ).unaryExpr(std::ptr_fun(getSigmoid)).array();

    ArrayReal lbl_sm = data[i].label - label_vec;

#pragma omp critical
    {
      if(abs(lbl_sm.sum()) > 0.5)
      {
        wrong += 1;
        if(data[i].label == 0)  ++fp;
        else                    ++fn;
      }
      else
      {
        right += 1;
        if(data[i].label == 0)  ++tn;
        else                    ++tp;
      }
    }
  }
    cout << right << "/" << right + wrong << "  ";
    Real precision = 1.0 * tp / (tp + fp);
    Real recall    = 1.0 * tp / (tp + fn);
    Real accuracy  = 1.0 * (tp + tn) / (tp + tn + fp + fn);
    Real f1score   = 2.0 * (precision * recall) / (precision + recall);
    cout << "Acc/F1: " << accuracy << " " << f1score << endl;
}

void FinetuneClassifier::trainLbfgs(LineSearchType linesearch)
{
  batch_from = 0;
  batch_to = train_length;

  lbfgs_parameter_t param;
  lbfgs_parameter_init(&param);
  param.linesearch = linesearch;
  param.max_iterations = iterations;
  //param.epsilon = 0.00000001;
  param.m = 25;

  const int n = theta_size_;
  auto vars = theta_;
  Real error = 0.0;

  int tries = 0;

  while (tries < 3 and it_count < 250)
  {
    int ret = lbfgs(n, vars, &error, lbfgs_evaluate_, lbfgs_progress_, this, &param);
    cout << "L-BFGS optimization terminated with status code = " << ret << endl;
    cout << "fx=" << error << endl;
    ++tries;
  }
}

void FinetuneClassifier::trainAdaGrad()
{
  auto vars = theta_;
  int number_vars = theta_size_;

  WeightArrayType theta(vars,number_vars);

  Real* Gt_d = new Real[number_vars];
  Real* Ginv_d = new Real[number_vars];
  WeightArrayType Gt(Gt_d,number_vars);
  WeightArrayType Ginv(Ginv_d,number_vars);
  Gt.setZero();

  Real* data1 = new Real[number_vars];

  int batchsize = (train_length / num_batches) + 1;
  cout << "Batch size: " << batchsize << endl;

  for (auto iteration = 0; iteration < iterations; ++iteration)
  {
    //unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    random_shuffle (mix.begin(), mix.end()); //, std::default_random_engine(seed));

    for (auto batch = 0; batch < num_batches; ++batch)
    {
      batch_from = batch*batchsize;
      batch_to = min((batch+1)*batchsize,train_length);

      if (batch_to - batch_from > 0)
      {
        WeightArrayType grad(data1,number_vars);

        //float err =
        finetuneCostAndGrad_(theta_,data1,number_vars);
        //grad /= (batch_to - batch_from);
        Gt += grad*grad;
        for (int i=0;i<number_vars;i++) {
          if (Gt_d[i] == 0)
            Ginv_d[i] = 0;
          else
            Ginv_d[i] = sqrt(1/Gt_d[i]);
        }

        grad *= Ginv;
        grad *= eta;

        //cout << theta.abs().sum() << " vs " << grad.abs().sum() << endl;
        theta -= grad;
      }
    }

    evaluate();
  }

  delete [] data1;
  delete [] Gt_d;
  delete [] Ginv_d;

}

lbfgsfloatval_t FinetuneClassifier::lbfgs_evaluate_(
    void *instance,
    const lbfgsfloatval_t *x,
    lbfgsfloatval_t *g,
    const int n,
    const lbfgsfloatval_t step
    )
{
  return reinterpret_cast<FinetuneClassifier*>(instance)->finetuneCostAndGrad_(x, g, n);
}

int FinetuneClassifier::lbfgs_progress_(
    void *instance,
    const lbfgsfloatval_t *x,
    const lbfgsfloatval_t *g,
    const lbfgsfloatval_t fx,
    const lbfgsfloatval_t xnorm,
    const lbfgsfloatval_t gnorm,
    const lbfgsfloatval_t step,
    int n,
    int k,
    int ls
    )
{
  cout << "N: " << n << endl;
  printf("Iteration %d:\n", k);
  printf("  fx = %f, x[0] = %f, x[1] = %f %f\n", fx, x[0], x[1], x[2]);
  printf("  fx = %f, g[0] = %f, g[1] = %f %f\n", fx, g[0], g[1], g[2]);
  printf("  xnorm = %f, gnorm = %f, step = %f\n", xnorm, gnorm, step);
  printf("\n");

  reinterpret_cast<FinetuneClassifier*>(instance)->it_count++;
  reinterpret_cast<FinetuneClassifier*>(instance)->evaluate();
  return 0;
}

// ForwardPropagates and returns error and gradient on self
lbfgsfloatval_t FinetuneClassifier::finetuneCostAndGrad_(
    const lbfgsfloatval_t *x,
    lbfgsfloatval_t *gradient_location,
    const int n)
{

  WeightVectorType  grad(gradient_location,theta_size_);
  grad.setZero();

  WeightMatricesType Wcatgrad;
  WeightVectorsType  Bcatgrad;

  Real* ptr = gradient_location;
  for (auto i=0; i<num_label_types; ++i) {
    Wcatgrad.push_back(WeightMatrixType(ptr, label_width, dynamic_embedding_size));
    ptr += label_width * dynamic_embedding_size;
  }
  for (auto i=0; i<num_label_types; ++i) {
    Bcatgrad.push_back(WeightVectorType(ptr, label_width));
    ptr += label_width;
  }

  assert(ptr == theta_size_ + gradient_location);

  /***************************************************************************
   *        Populate train input with forward Propagation and tricks         *
   ***************************************************************************/
  Real cost = 0.0;
  int right = 0;
  int wrong = 0;
  int tp = 0;
  int fp = 0;
  int tn = 0;
  int fn = 0;

#pragma omp parallel for schedule(dynamic)
  for (auto k = batch_from; k<batch_to; ++k)
  {
    auto i = mix[k];

    // Encode input
    ArrayReal label_vec = (
        Wcat[0] * trainData[i].vector + Bcat[0]
        ).unaryExpr(std::ptr_fun(getSigmoid)).array();

    ArrayReal lbl_sm = label_vec - trainData[i].label;
    ArrayReal delta = lbl_sm * (label_vec) * (1 - label_vec);

#pragma omp critical
    {
      //cout << "D/D2" << delta << " " << delta2 << endl;
      cost += 0.5 * (lbl_sm * lbl_sm).sum();
      Wcatgrad[0] += delta.matrix() * trainData[i].vector.transpose();
      Bcatgrad[0] += delta.matrix();

      if(abs(lbl_sm.sum()) > 0.5)
      {
        wrong += 1;
        if(trainData[i].label == 0)
          ++fp;
        else
          ++fn;
      }
      else
      {
        right += 1;
        if(trainData[i].label == 0)
          ++tn;
        else
          ++tp;
      }
    }
  }

  /*
   *cout << "Correct: " << right << "/" << right + wrong << "   ";
   *cout << "Zero: " << tp << "/" << tp+fn << " ";
   *cout << "One:  " << tn << "/" << tn+fp << endl;
   */

  float lambda_partial = lambda * (batch_to - batch_from) / train_length;
  Wcatgrad[0] += lambda_partial*Wcat[0];
  cost += 0.5*lambda_partial*(Wcat[0].cwiseProduct(Wcat[0])).sum();

  Wcatgrad[0] /= (batch_to - batch_from);
  cost /= (batch_to - batch_from);

  return cost;
}

FinetuneClassifier::~FinetuneClassifier()
{
  delete [] trainI_;
  delete [] theta_;

}