#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <cctbx/uctbx/fast_minimum_reduction.h>

namespace cctbx { namespace uctbx { namespace boost_python {

namespace {

  struct fast_minimum_reduction_wrappers
  {
    typedef fast_minimum_reduction<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t>("fast_minimum_reduction", no_init)
        .def(init<unit_cell const&,
                  optional<std::size_t, double, std::size_t> >((
          arg("unit_cell"),
          arg("iteration_limit")=100,
          arg("multiplier_significant_change_test")=16,
          arg("min_n_no_significant_change")=2)))
        .def("as_gruber_matrix", &w_t::as_gruber_matrix)
        .def("as_niggli_matrix", &w_t::as_niggli_matrix)
        .def("as_sym_mat3", &w_t::as_sym_mat3)
        .def("as_unit_cell", &w_t::as_unit_cell)
        .def("iteration_limit", &w_t::iteration_limit)
        .def("multiplier_significant_change_test",
             &w_t::multiplier_significant_change_test)
        .def("min_n_no_significant_change", &w_t::min_n_no_significant_change)
        .def("r_inv", &w_t::r_inv, ccr())
        .def("n_iterations", &w_t::n_iterations)
        .def("termination_due_to_significant_change_test",
             &w_t::termination_due_to_significant_change_test)
        .def("type", &w_t::type)
      ;
    }
  };

} // namespace <anoymous>

  void wrap_fast_minimum_reduction()
  {
    fast_minimum_reduction_wrappers::wrap();
  }

}}} // namespace cctbx::sgtbx::boost_python


#include "ucRollingModel.hpp"
#include "CbcLagrangeSolver.hpp"
#include "ClpRecourseSolver.hpp"
#include "conicBundleDriver.hpp"
#include <boost/scoped_ptr.hpp>

using namespace std;
using boost::scoped_ptr;

int main(int argc, char **argv) {

        MPI_Init(&argc, &argv);

        int mype;
        MPI_Comm_rank(MPI_COMM_WORLD,&mype);

        if (argc != 3) {
		printf("Usage: %s [num scenarios] [time horizon]\n",argv[0]);
                return 1;
        }

	int nscen = atoi(argv[1]);
	int T = atoi(argv[2]);
	ucRollingModel model("../../../apps/unitcommitment_rolling/Illinois",nscen,0,T);

        conicBundleDriver<CbcLagrangeSolver,ClpRecourseSolver>(model);

        MPI_Finalize();

        return 0;

}


/**
 ** Copyright (c) 2014 Illumina, Inc.
 **
 ** This file is part of Illumina's Enhanced Artificial Genome Engine (EAGLE),
 ** covered by the "BSD 2-Clause License" (see accompanying LICENSE file)
 **/

#include <iostream>
#include <string>
#include <vector>
#include <boost/assign.hpp>

using namespace std;
using boost::assign::list_of;

#include "Helpers.hh"

#include "RegistryName.hh"
#include "testPhred.hh"

using eagle::model::Phred;

CPPUNIT_TEST_SUITE_NAMED_REGISTRATION( TestPhred, registryName("Phred"));

void TestPhred::setUp()
{
}

void TestPhred::tearDown()
{
}

void TestPhred::testQualToProb()
{
    CPPUNIT_ASSERT_EQUAL( Phred::qualToProb(0), 1.0 );
    CPPUNIT_ASSERT_EQUAL( Phred::qualToProb(1), pow(10, -0.1) );
    CPPUNIT_ASSERT_EQUAL( Phred::qualToProb(10), 0.1 );
    CPPUNIT_ASSERT_EQUAL( Phred::qualToProb(20), 0.01 );
    CPPUNIT_ASSERT_EQUAL( Phred::qualToProb(30), 0.001 );
    CPPUNIT_ASSERT_EQUAL( Phred::qualToProb(40), 0.0001 );
    CPPUNIT_ASSERT_EQUAL( Phred::qualToProb(50), 0.00001 );
    CPPUNIT_ASSERT_THROW( Phred::qualToProb(51), eagle::common::EagleException );
}

void TestPhred::testProbToQual()
{
    CPPUNIT_ASSERT_EQUAL( Phred::probToQual(1.0),            0u );
    CPPUNIT_ASSERT_EQUAL( Phred::probToQual(pow(10, -0.1)),  1u );
    CPPUNIT_ASSERT_EQUAL( Phred::probToQual(0.1),           10u );
    CPPUNIT_ASSERT_EQUAL( Phred::probToQual(0.01),          20u );
    CPPUNIT_ASSERT_EQUAL( Phred::probToQual(0.001),         30u );
    CPPUNIT_ASSERT_EQUAL( Phred::probToQual(0.0001),        40u );
    CPPUNIT_ASSERT_EQUAL( Phred::probToQual(0.00001),       50u );
    CPPUNIT_ASSERT_EQUAL( Phred::probToQual(0.0),           51u );

    double between10and11 = (Phred::qualToProb(10) + Phred::qualToProb(11)) / 2;
    CPPUNIT_ASSERT_EQUAL( Phred::probToQual(between10and11), 11u );
}


//
// Created by cristobal on 07-04-18.
//

#include <iostream>
#include "FrameContainer.h"
#include <unistd.h>
#include <fstream>
#include <boost/progress.hpp>

FrameContainer::FrameContainer() {
}

FrameContainer::FrameContainer(cv::Mat &frame, int real_frame_position) : real_frame_position(real_frame_position) {
  cv::cvtColor(frame, frame, cv::COLOR_BGR2GRAY);
  cv::resize(frame, frame, cv::Size(256, 256));
  cv::Size s = frame.size();
  int rows = s.height;
  int cols = s.width;
  int rows_mini = rows / IMAGE_PARTS ;
  int cols_mini = cols / IMAGE_PARTS ;

  for (int i = 0; i < IMAGE_PARTS; i++){
    //vmat row_minis;
    vmat aux_hist;
    int y_start = i*rows_mini;
    int roi_height = std::min<int>(rows_mini, rows - y_start);

    for(int j = 0; j < IMAGE_PARTS; j++){
      int x_start = j*cols_mini;
      int roi_width = std::min<int>(cols_mini, cols - x_start);

      cv::Rect currentROI(x_start, y_start, roi_width, roi_height);
      cv::Mat cropped = frame(currentROI);
      cv::Mat detected_edges;

      cv::blur(cropped, detected_edges, cv::Size(3, 3));

      int ratio = 3;
      int kernel_size = 3;
      int lower_th = 30;
      cv::Canny(detected_edges, detected_edges, lower_th, lower_th*ratio, kernel_size);
      cv::Mat dst;
      cropped.copyTo(dst, detected_edges);
      cropped = dst;
      /*
      //sobelizado
      cv::Mat sobel_x, sobel_y, grad;

      int ddepth = CV_32F;
      cv::Sobel(cropped, sobel_x, ddepth, 1, 0);
      cv::Sobel(cropped, sobel_y, ddepth, 0, 1);
      cv::convertScaleAbs(sobel_x, sobel_x);
      cv::convertScaleAbs(sobel_y, sobel_y);
      cv::addWeighted(sobel_x, 0.5, sobel_y, 0.5, 0, cropped);
      // end sobelizado
      */

      cv::Mat histo_cropped = getHistogram(cropped);
      aux_hist.push_back(histo_cropped);
    }
    this->histograms.push_back(aux_hist);
  }
}

FrameContainer::FrameContainer(std::vector<vmat> &histograms, int real_frame_position) :
        histograms(histograms), real_frame_position(real_frame_position){

}

cv::Mat collapseHistogramXAxis(cv::Mat &original, int nextRange){
  cv::Mat out = cv::Mat::zeros(cv::Size(1, nextRange), original.type());
  int currentRange = original.rows;
  int step = currentRange/nextRange;
  float acc = original.at<float>(0, 0);
  int counter = 0;
  for(int i = 1; i < currentRange; i++){
    if(i % step == 0){
      out.at<float>(counter, 0) = acc/step;
      acc = 0;
      counter++;
    }
    acc += original.at<float>(i, 0);
  }
  if(acc > 0){
    out.at<float>(counter, 0) = acc/step;
  }
  return out;
}
cv::Mat FrameContainer::getHistogram(cv::Mat &whichImage) {
  cv::Mat toSave;
  int histSize = 256;
  float range[] = {0, 256};
  const float *histRange = {range};
  int channels[] = {0};
  cv::calcHist(&whichImage, 1, channels, cv::Mat(), toSave, 1, &histSize, &histRange, true, false);
  toSave = collapseHistogramXAxis(toSave, 2);
  cv::normalize(toSave, toSave);

  return toSave;
}

std::vector<vmat> &FrameContainer::getHistograms() {
  return this->histograms;
}

const int grab_times[] = {1, 10, 25};
const int FPS = 30;
vecFC getVecFC(cv::VideoCapture &videoCapture){
  vecFC output;
  cv::Mat frame;
  long counter = 0;
  int which = 0;
  long offset = 0;
  int frame_count = (int)(videoCapture.get(cv::CAP_PROP_FRAME_COUNT));

  int real_counter = -1;
  std::cout << "Beginning histogram calculation.. Total number of frames: " << frame_count << std::endl;
  boost::progress_display show_progress(frame_count);
  while(videoCapture.grab()){
    real_counter++;
    if(!videoCapture.retrieve(frame)){
      offset++;
      continue;
    }
    auto where  = (int) (counter % FPS);
    if(where == (grab_times[which] + offset) % FPS){
      FrameContainer frameContainer(frame, real_counter);
      output.push_back(frameContainer);

      which++;
      which %= 3;
    }
    if(counter % 5000 == 0){
      //std::cout << "progress: " << ((double)counter)*100/((double)frame_count) << "%" std::endl;
    }
    counter++;
    ++show_progress;
    offset = 0;
  }
  return output;
}

vecFC getVecFC(const std::string &storedSerialized) {
  std::ifstream ifs(storedSerialized);
  //std::cout << "trying to read filename: '" << storedSerialized << "'" << std::endl;
  if(!ifs){
    std::cout << "could not read ifs of " << storedSerialized << std::endl;
  }
  vecFC out;
  boost::archive::binary_iarchive ia(ifs);
  ia & out;
  return out;
}

void saveVecFC(const std::string &storedSerialized, vecFC &vecFc) {
  std::ofstream ofs(storedSerialized);
  boost::archive::binary_oarchive oa(ofs);
  oa << vecFc;
}

int FrameContainer::getRealFramePosition() {
  return this->real_frame_position;
}


//
//  NectStack.cpp
//  Nectarine
//
//  Created by Hossein Yousefi on 14/12/2018.
//  Copyright © 2018 Harbour Space. All rights reserved.
//

#include "NectStack.hpp"
#include <boost/assert.hpp>

namespace nectarine {
  int vmachine::execute(std::vector<int> const& code,
                        std::vector<int>::const_iterator pc,
                        std::vector<int>::iterator frame_ptr) {
    std::vector<int>::iterator stack_ptr = frame_ptr;

    while (pc != code.end()) {
      switch (*pc++) {
        case NEG:
          stack_ptr[-1] = -stack_ptr[-1];
          break;

        case NOT:
          stack_ptr[-1] = !bool(stack_ptr[-1]);
          break;

        case ADD:
          --stack_ptr;
          stack_ptr[-1] += stack_ptr[0];
          break;

        case SUB:
          --stack_ptr;
          stack_ptr[-1] -= stack_ptr[0];
          break;

        case MUL:
          --stack_ptr;
          stack_ptr[-1] *= stack_ptr[0];
          break;

        case DIV:
          --stack_ptr;
          stack_ptr[-1] /= stack_ptr[0];
          break;

        case EQ:
          --stack_ptr;
          stack_ptr[-1] = bool(stack_ptr[-1] == stack_ptr[0]);
          break;

        case NEQ:
          --stack_ptr;
          stack_ptr[-1] = bool(stack_ptr[-1] != stack_ptr[0]);
          break;

        case LT:
          --stack_ptr;
          stack_ptr[-1] = bool(stack_ptr[-1] < stack_ptr[0]);
          break;

        case LTE:
          --stack_ptr;
          stack_ptr[-1] = bool(stack_ptr[-1] <= stack_ptr[0]);
          break;

        case GT:
          --stack_ptr;
          stack_ptr[-1] = bool(stack_ptr[-1] > stack_ptr[0]);
          break;

        case GTE:
          --stack_ptr;
          stack_ptr[-1] = bool(stack_ptr[-1] >= stack_ptr[0]);
          break;

        case AND:
          --stack_ptr;
          stack_ptr[-1] = bool(stack_ptr[-1]) && bool(stack_ptr[0]);
          break;

        case OR:
          --stack_ptr;
          stack_ptr[-1] = bool(stack_ptr[-1]) || bool(stack_ptr[0]);
          break;

        case LOAD:
          *stack_ptr++ = frame_ptr[*pc++];
          break;

        case STORE:
          --stack_ptr;
          frame_ptr[*pc++] = stack_ptr[0];
          break;

        case INT:
          *stack_ptr++ = *pc++;
          break;

        case TRUE:
          *stack_ptr++ = true;
          break;

        case FALSE:
          *stack_ptr++ = false;
          break;

        case JMP:
          pc += *pc;
          break;

        case JIF:
          if (!bool(stack_ptr[-1]))
            pc += *pc;
          else
            ++pc;
          --stack_ptr;
          break;

        case ADJ:
          stack_ptr = stack.begin() + *pc++;
          break;

        case CALL: {
          int nargs = *pc++;
          int jump = *pc++;

          int r = execute(code,
                          code.begin() + jump,
                          stack_ptr - nargs);

          // cleanup after return from function
          stack_ptr[-nargs] = r;      //  get return value
          stack_ptr -= (nargs - 1);
        }
          break;

        case RET:
          return stack_ptr[-1];
      }
    }
    return -1;
  }
}


/*
# Copyright 2018 HyphaROS Workshop.
# Developer: HaoChih, LIN (hypha.ros@gmail.com)
#
# 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 <iostream>
#include <map>
#include <math.h>

#include "ros/ros.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
// #include <tf/transform_datatypes.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
//#include <ackermann_msgs/AckermannDriveStamped.h>
#include <visualization_msgs/Marker.h>

#include "MPC.h"
#include <Eigen/Core>
#include <Eigen/QR>

using namespace std;
using namespace Eigen;

/********************/
/* CLASS DEFINITION */
/********************/
class MPCNode
{
    public:
        MPCNode();
        int get_thread_numbers();

    private:
        ros::NodeHandle _nh;
        ros::Subscriber _sub_odom, _sub_gen_path, _sub_path, _sub_goal, _sub_amcl;
        ros::Publisher _pub_globalpath,_pub_odompath, _pub_twist, _pub_mpctraj;
        ros::Publisher _pub_ackermann;
        ros::Timer _timer1;
        tf::TransformListener _tf_listener;

        geometry_msgs::Point _goal_pos;
        nav_msgs::Odometry _odom;
        nav_msgs::Path _odom_path, _mpc_traj;
        //ackermann_msgs::AckermannDriveStamped _ackermann_msg;
        geometry_msgs::Twist _twist_msg;

        string _globalPath_topic, _goal_topic;
        string _map_frame, _odom_frame, _car_frame;

        MPC _mpc;
        map<string, double> _mpc_params;
        double _mpc_steps, _ref_cte, _ref_etheta, _ref_vel, _w_cte, _w_etheta, _w_vel,
               _w_angvel, _w_accel, _w_angvel_d, _w_accel_d, _max_angvel, _max_throttle, _bound_value;

        //double _Lf;
        double _dt, _w, _throttle, _speed, _max_speed;
        double _pathLength, _goalRadius, _waypointsDist;
        int _controller_freq, _downSampling, _thread_numbers;
        bool _goal_received, _goal_reached, _path_computed, _pub_twist_flag, _debug_info, _delay_mode;

        double polyeval(Eigen::VectorXd coeffs, double x);
        Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);

        void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg);
        void pathCB(const nav_msgs::Path::ConstPtr& pathMsg);
        void desiredPathCB(const nav_msgs::Path::ConstPtr& pathMsg);
        void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg);
        void amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg);
        void controlLoopCB(const ros::TimerEvent&);
        void makeGlobalPath(const nav_msgs::Odometry odomMsg);

        //For making global planner
        nav_msgs::Path _gen_path;

}; // end of class

MPCNode::MPCNode()
{
    //Private parameters handler
    ros::NodeHandle pn("~");

    //Parameters for control loop
    pn.param("thread_numbers", _thread_numbers, 2); // number of threads for this ROS node
    pn.param("pub_twist_cmd", _pub_twist_flag, true);
    pn.param("debug_info", _debug_info, true);
    pn.param("delay_mode", _delay_mode, true);
    pn.param("max_speed", _max_speed, 0.50); // unit: m/s
    pn.param("waypoints_dist", _waypointsDist, -1.0); // unit: m
    pn.param("path_length", _pathLength, 8.0); // unit: m
    pn.param("goal_radius", _goalRadius, 0.5); // unit: m
    pn.param("controller_freq", _controller_freq, 10);
    //pn.param("vehicle_Lf", _Lf, 0.290); // distance between the front of the vehicle and its center of gravity
    _dt = double(1.0/_controller_freq); // time step duration dt in s

    //Parameter for MPC solver
    pn.param("mpc_steps", _mpc_steps, 20.0);
    pn.param("mpc_ref_cte", _ref_cte, 0.0);
    pn.param("mpc_ref_vel", _ref_vel, 1.0);
    pn.param("mpc_ref_etheta", _ref_etheta, 0.0);
    pn.param("mpc_w_cte", _w_cte, 5000.0);
    pn.param("mpc_w_etheta", _w_etheta, 5000.0);
    pn.param("mpc_w_vel", _w_vel, 1.0);
    pn.param("mpc_w_angvel", _w_angvel, 100.0);
    pn.param("mpc_w_angvel_d", _w_angvel_d, 10.0);
    pn.param("mpc_w_accel", _w_accel, 50.0);
    pn.param("mpc_w_accel_d", _w_accel_d, 10.0);
    pn.param("mpc_max_angvel", _max_angvel, 3.0); // Maximal angvel radian (~30 deg)
    pn.param("mpc_max_throttle", _max_throttle, 1.0); // Maximal throttle accel
    pn.param("mpc_bound_value", _bound_value, 1.0e3); // Bound value for other variables

    //Parameter for topics & Frame name
    pn.param<std::string>("global_path_topic", _globalPath_topic, "/move_base/TrajectoryPlannerROS/global_plan" );
    pn.param<std::string>("goal_topic", _goal_topic, "/move_base_simple/goal" );
    pn.param<std::string>("map_frame", _map_frame, "map" ); //*****for mpc, "odom"
    pn.param<std::string>("odom_frame", _odom_frame, "odom");
    pn.param<std::string>("car_frame", _car_frame, "base_footprint" );

    //Display the parameters
    cout << "\n===== Parameters =====" << endl;
    cout << "pub_twist_cmd: "  << _pub_twist_flag << endl;
    cout << "debug_info: "  << _debug_info << endl;
    cout << "delay_mode: "  << _delay_mode << endl;
    //cout << "vehicle_Lf: "  << _Lf << endl;
    cout << "frequency: "   << _dt << endl;
    cout << "mpc_steps: "   << _mpc_steps << endl;
    cout << "mpc_ref_vel: " << _ref_vel << endl;
    cout << "mpc_w_cte: "   << _w_cte << endl;
    cout << "mpc_w_etheta: "  << _w_etheta << endl;
    cout << "mpc_max_angvel: "  << _max_angvel << endl;

    //Publishers and Subscribers
    _sub_odom   = _nh.subscribe("/odom", 1, &MPCNode::odomCB, this);
    _sub_path   = _nh.subscribe( _globalPath_topic, 1, &MPCNode::pathCB, this);
    _sub_gen_path   = _nh.subscribe( "desired_path", 1, &MPCNode::desiredPathCB, this);
    _sub_goal   = _nh.subscribe( _goal_topic, 1, &MPCNode::goalCB, this);
    _sub_amcl   = _nh.subscribe("/amcl_pose", 5, &MPCNode::amclCB, this);
    _pub_globalpath  = _nh.advertise<nav_msgs::Path>("/global_path", 1); // Global path generated from another source
    _pub_odompath  = _nh.advertise<nav_msgs::Path>("/mpc_reference", 1); // reference path for MPC ///mpc_reference
    _pub_mpctraj   = _nh.advertise<nav_msgs::Path>("/mpc_trajectory", 1);// MPC trajectory output
    //_pub_ackermann = _nh.advertise<ackermann_msgs::AckermannDriveStamped>("/ackermann_cmd", 1);
    if(_pub_twist_flag)
        _pub_twist = _nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1); //for stage (Ackermann msg non-supported)

    //Timer
    _timer1 = _nh.createTimer(ros::Duration((1.0)/_controller_freq), &MPCNode::controlLoopCB, this); // 10Hz //*****mpc

    //Init variables
    _goal_received = false;
    _goal_reached  = false;
    _path_computed = false;
    _throttle = 0.0;
    _w = 0.0;
    _speed = 0.0;

    //_ackermann_msg = ackermann_msgs::AckermannDriveStamped();
    _twist_msg = geometry_msgs::Twist();
    _mpc_traj = nav_msgs::Path();

    //Init parameters for MPC object
    _mpc_params["DT"] = _dt;
    //_mpc_params["LF"] = _Lf;
    _mpc_params["STEPS"]    = _mpc_steps;
    _mpc_params["REF_CTE"]  = _ref_cte;
    _mpc_params["REF_ETHETA"] = _ref_etheta;
    _mpc_params["REF_V"]    = _ref_vel;
    _mpc_params["W_CTE"]    = _w_cte;
    _mpc_params["W_EPSI"]   = _w_etheta;
    _mpc_params["W_V"]      = _w_vel;
    _mpc_params["W_ANGVEL"]  = _w_angvel;
    _mpc_params["W_A"]      = _w_accel;
    _mpc_params["W_DANGVEL"] = _w_angvel_d;
    _mpc_params["W_DA"]     = _w_accel_d;
    _mpc_params["ANGVEL"]   = _max_angvel;
    _mpc_params["MAXTHR"]   = _max_throttle;
    _mpc_params["BOUND"]    = _bound_value;
    _mpc.LoadParams(_mpc_params);
}

// Public: return _thread_numbers
int MPCNode::get_thread_numbers()
{
    return _thread_numbers;
}

// Evaluate a polynomial.
double MPCNode::polyeval(Eigen::VectorXd coeffs, double x)
{
    double result = 0.0;
    for (int i = 0; i < coeffs.size(); i++)
    {
        result += coeffs[i] * pow(x, i);
    }
    return result;
}

// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
Eigen::VectorXd MPCNode::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order)
{
    assert(xvals.size() == yvals.size());
    assert(order >= 1 && order <= xvals.size() - 1);
    Eigen::MatrixXd A(xvals.size(), order + 1);

    for (int i = 0; i < xvals.size(); i++)
        A(i, 0) = 1.0;

    for (int j = 0; j < xvals.size(); j++)
    {
        for (int i = 0; i < order; i++)
            A(j, i + 1) = A(j, i) * xvals(j);
    }

    auto Q = A.householderQr();
    auto result = Q.solve(yvals);
    return result;
}

// CallBack: Update odometry
void MPCNode::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    _odom = *odomMsg;

}

void MPCNode::makeGlobalPath(const nav_msgs::Odometry odomMsg)
{
    /*
    nav_msgs::Path global_path = nav_msgs::Path();
    geometry_msgs::PoseStamped startPose;
    geometry_msgs::PoseStamped tempPose;
    nav_msgs::Odometry odom = odomMsg;

    //Calculate the waypoint distance
    double gap_x = _gen_path.poses[1].pose.position.x - _gen_path.poses[0].pose.position.x;
    double gap_y = _gen_path.poses[1].pose.position.y - _gen_path.poses[0].pose.position.y;
    _waypointsDist = sqrt(gap_x*gap_x + gap_y*gap_y);

    static double pre_odom_x = odom.pose.pose.position.x;
    static double pre_odom_y = odom.pose.pose.position.y;
    static double odom_diff = 0;
    static int n_waypointMove = 0;
    int N = _gen_path.poses.size(); // Number of waypoints

    try
    {
        double total_length = 0.0;

        startPose.header.stamp = ros::Time::now();
        startPose.header.frame_id = _odom_frame;
        startPose.pose = odom.pose.pose;

        global_path.poses.push_back(startPose); // copy current odom position to the start position

        // Calculate the difference and save the odom x,y
        odom_diff = sqrt((odom.pose.pose.position.x - pre_odom_x)*(odom.pose.pose.position.x - pre_odom_x) + (odom.pose.pose.position.y-pre_odom_y)*(odom.pose.pose.position.y-pre_odom_y));
        pre_odom_x = odom.pose.pose.position.x;
        pre_odom_y = odom.pose.pose.position.y;
        n_waypointMove += (int)(odom_diff / _waypointsDist);
        cout << "n_waypointMove: " << n_waypointMove << endl;

        if(n_waypointMove > N)
        {
            n_waypointMove = 0;
        }
        // Append the part of the generated path
        for(int i = n_waypointMove; i < N ; i++)
        {
            _tf_listener.transformPose(_odom_frame, ros::Time(0) ,
                                            _gen_path.poses[i], _map_frame, tempPose);
            global_path.poses.push_back(tempPose);
            total_length = total_length + _waypointsDist;
        }

        // publish global_path
        global_path.header.frame_id = _odom_frame;
        global_path.header.stamp = ros::Time::now();
        _pub_globalpath.publish(global_path);

        //desiredPathCB(global_path); // for cutting and down sampling
    }
    catch(tf::TransformException &ex)
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
    }
    */

}
// CallBack: Update generated path (conversion to odom frame)
void MPCNode::desiredPathCB(const nav_msgs::Path::ConstPtr& totalPathMsg)
{
    /*
    _gen_path = *totalPathMsg;

    //For plan the global path about desired path
    makeGlobalPath(_odom);

    _goal_received = true;
    _goal_reached = false;
    nav_msgs::Path mpc_path = nav_msgs::Path();   // For generating mpc reference path
    geometry_msgs::PoseStamped tempPose;
    nav_msgs::Odometry odom = _odom;

    try
    {
        double total_length = 0.0;
        _pathLength = 6;
        //find waypoints distance
        if(_waypointsDist <= 0.0)
        {
            double gap_x = totalPathMsg->poses[1].pose.position.x - totalPathMsg->poses[0].pose.position.x;
            double gap_y = totalPathMsg->poses[1].pose.position.y - totalPathMsg->poses[0].pose.position.y;
            _waypointsDist = sqrt(gap_x*gap_x + gap_y*gap_y);
        }

        // Find the nearst point for robot position
        int min_idx = 0;
        int min_val = 100; // why double is wrong?
        int N = totalPathMsg->poses.size(); // Number of waypoints
        const double px = odom.pose.pose.position.x; //pose: odom frame
        const double py = odom.pose.pose.position.y;
        const double ptheta = odom.pose.pose.position.y;

        double dx, dy; // difference distance
        double pre_yaw = 0;
        double roll, pitch, yaw = 0;

        for(int i = 0; i < N; i++)
        {
            dx = totalPathMsg->poses[i].pose.position.x - px;
            dy = totalPathMsg->poses[i].pose.position.y - py;

            tf::Quaternion q(
                totalPathMsg->poses[i].pose.orientation.x,
                totalPathMsg->poses[i].pose.orientation.y,
                totalPathMsg->poses[i].pose.orientation.z,
                totalPathMsg->poses[i].pose.orientation.w);
            tf::Matrix3x3 m(q);
            m.getRPY(roll, pitch, yaw);

            if(abs(pre_yaw - yaw) > 5)
            {
                cout << "abs(pre_yaw - yaw)" << abs(pre_yaw - yaw) << endl;
                pre_yaw = yaw;
            }

            if(min_val > sqrt(dx*dx + dy*dy))
            {
                min_val = sqrt(dx*dx + dy*dy);
                min_idx = i;

                if(i < N * 0.02)
                    min_idx = N - 100; //for smoothing about init position
            }
        }

        for(int i = min_idx; i < N ; i++)
        {
            if(total_length > _pathLength)
                break;

            _tf_listener.transformPose(_odom_frame, ros::Time(0) ,
                                            totalPathMsg->poses[i], _map_frame, tempPose);
            mpc_path.poses.push_back(tempPose);
            total_length = total_length + _waypointsDist;
        }

        // Connect the end of path to the front
        if(total_length < _pathLength )
        {
            for(int i = 0; i < N ; i++)
            {
                if(total_length > _pathLength)
                    break;
                _tf_listener.transformPose(_odom_frame, ros::Time(0) ,
                                                totalPathMsg->poses[i], _map_frame, tempPose);
                mpc_path.poses.push_back(tempPose);
                total_length = total_length + _waypointsDist;
            }
        }

        if(mpc_path.poses.size() >= _pathLength )
        {
            _odom_path = mpc_path; // Path waypoints in odom frame
            _path_computed = true;
            // publish odom path
            mpc_path.header.frame_id = _odom_frame;
            mpc_path.header.stamp = ros::Time::now();
            _pub_odompath.publish(mpc_path);
        }
        else
        {
            cout << "Failed to path generation" << endl;
            _waypointsDist = -1;
        }
    }
    catch(tf::TransformException &ex)
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
    }
    */
}

// CallBack: Update path waypoints (conversion to odom frame)
void MPCNode::pathCB(const nav_msgs::Path::ConstPtr& pathMsg)
{

    if(_goal_received && !_goal_reached)
    {
        cout << "PathCB condition" << endl;
        nav_msgs::Path odom_path = nav_msgs::Path();
        try
        {
            double total_length = 0.0;
            int sampling = _downSampling;

            //find waypoints distance
            if(_waypointsDist <=0.0)
            {
                double dx = pathMsg->poses[1].pose.position.x - pathMsg->poses[0].pose.position.x;
                double dy = pathMsg->poses[1].pose.position.y - pathMsg->poses[0].pose.position.y;
                _waypointsDist = sqrt(dx*dx + dy*dy);
                _downSampling = int(_pathLength/10.0/_waypointsDist);
            }

            // Cut and downsampling the path
            for(int i =0; i< pathMsg->poses.size(); i++)
            {
                if(total_length > _pathLength)
                    break;

                if(sampling == _downSampling)
                {
                    geometry_msgs::PoseStamped tempPose;
                    _tf_listener.transformPose(_odom_frame, ros::Time(0) , pathMsg->poses[i], _map_frame, tempPose);
                    odom_path.poses.push_back(tempPose);
                    sampling = 0;
                }
                total_length = total_length + _waypointsDist;
                sampling = sampling + 1;
            }

            if(odom_path.poses.size() >= 6 )
            {
                _odom_path = odom_path; // Path waypoints in odom frame
                _path_computed = true;
                // publish odom path
                odom_path.header.frame_id = _odom_frame;
                odom_path.header.stamp = ros::Time::now();
                _pub_odompath.publish(odom_path);
            }
            else
            {
                cout << "Failed to path generation" << endl;
                _waypointsDist = -1;
            }
            //DEBUG
            //cout << endl << "N: " << odom_path.poses.size() << endl
            //<<  "Car path[0]: " << odom_path.poses[0];
            // << ", path[N]: " << _odom_path.poses[_odom_path.poses.size()-1] << endl;
        }
        catch(tf::TransformException &ex)
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
        }
    }

}

// CallBack: Update goal status
void MPCNode::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg)
{
    _goal_pos = goalMsg->pose.position;
    _goal_received = true;
    _goal_reached = false;
    ROS_INFO("Goal Received :goalCB!");
}

// Callback: Check if the car is inside the goal area or not
void MPCNode::amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg)
{
    if(_goal_received)
    {
        double car2goal_x = _goal_pos.x - amclMsg->pose.pose.position.x;
        double car2goal_y = _goal_pos.y - amclMsg->pose.pose.position.y;
        double dist2goal = sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y);
        if(dist2goal < _goalRadius)
        {
            _goal_received = false;
            _goal_reached = true;
            _path_computed = false;
            ROS_INFO("Goal Reached !");
        }
    }
}

// Timer: Control Loop (closed loop nonlinear MPC)
void MPCNode::controlLoopCB(const ros::TimerEvent&)
{
    if(_goal_received && !_goal_reached && _path_computed ) //received goal & goal not reached
    {
        nav_msgs::Odometry odom = _odom;
        nav_msgs::Path odom_path = _odom_path;

        // Update system states: X=[x, y, theta, v]
        const double px = odom.pose.pose.position.x; //pose: odom frame
        const double py = odom.pose.pose.position.y;
        tf::Pose pose;
        tf::poseMsgToTF(odom.pose.pose, pose);
        const double theta = tf::getYaw(pose.getRotation());
        const double v = odom.twist.twist.linear.x; //twist: body fixed frame
        // Update system inputs: U=[w, throttle]
        const double w = _w; // steering -> w
        //const double steering = _steering;  // radian
        const double throttle = _throttle; // accel: >0; brake: <0
        const double dt = _dt;
        //const double Lf = _Lf;

        // Waypoints related parameters
        const int N = odom_path.poses.size(); // Number of waypoints
        const double costheta = cos(theta);
        const double sintheta = sin(theta);

        // Convert to the vehicle coordinate system
        VectorXd x_veh(N);
        VectorXd y_veh(N);
        for(int i = 0; i < N; i++)
        {
            const double dx = odom_path.poses[i].pose.position.x - px;
            const double dy = odom_path.poses[i].pose.position.y - py;
            x_veh[i] = dx * costheta + dy * sintheta;
            y_veh[i] = dy * costheta - dx * sintheta;
        }

        // Fit waypoints
        auto coeffs = polyfit(x_veh, y_veh, 3);

        const double cte  = polyeval(coeffs, 0.0);
        const double etheta = atan(coeffs[1]);

        VectorXd state(6);
        if(_delay_mode)
        {
            // Kinematic model is used to predict vehicle state at the actual moment of control (current time + delay dt)
            const double px_act = v * dt;
            const double py_act = 0;
            const double theta_act = w * dt; //(steering) theta_act = v * steering * dt / Lf;
            const double v_act = v + throttle * dt; //v = v + a * dt

            const double cte_act = cte + v * sin(etheta) * dt;
            const double etheta_act = etheta - theta_act;

            state << px_act, py_act, theta_act, v_act, cte_act, etheta_act;
        }
        else
        {
            state << 0, 0, 0, v, cte, etheta;
        }

        // Solve MPC Problem
        vector<double> mpc_results = _mpc.Solve(state, coeffs);

        // MPC result (all described in car frame), output = (acceleration, w)
        _w = mpc_results[0]; // radian/sec, angular velocity
        _throttle = mpc_results[1]; // acceleration
        _speed = v + _throttle*dt;  // speed
        if (_speed >= _max_speed)
            _speed = _max_speed;
        if(_speed <= 0.0)
            _speed = 0.0;

        if(_debug_info)
        {
            cout << "\n\nDEBUG" << endl;
            cout << "theta: " << theta << endl;
            cout << "V: " << v << endl;
            //cout << "odom_path: \n" << odom_path << endl;
            //cout << "x_points: \n" << x_veh << endl;
            //cout << "y_points: \n" << y_veh << endl;
            cout << "coeffs: \n" << coeffs << endl;
            cout << "_w: \n" << _w << endl;
            cout << "_throttle: \n" << _throttle << endl;
            cout << "_speed: \n" << _speed << endl;
        }

        // Display the MPC predicted trajectory
        _mpc_traj = nav_msgs::Path();
        _mpc_traj.header.frame_id = _car_frame; // points in car coordinate
        _mpc_traj.header.stamp = ros::Time::now();
        for(int i=0; i<_mpc.mpc_x.size(); i++)
        {
            geometry_msgs::PoseStamped tempPose;
            tempPose.header = _mpc_traj.header;
            tempPose.pose.position.x = _mpc.mpc_x[i];
            tempPose.pose.position.y = _mpc.mpc_y[i];
            tempPose.pose.orientation.w = 1.0;
            _mpc_traj.poses.push_back(tempPose);
        }
        // publish the mpc trajectory
        _pub_mpctraj.publish(_mpc_traj);

    }
    else
    {
        _throttle = 0.0;
        _speed = 0.0;
        _w = 0;
        if(_goal_reached && _goal_received)
            cout << "Goal Reached: control loop !" << endl;
    }

    // publish ankermann cmd_vel
    /*
    _ackermann_msg.header.frame_id = _car_frame;
    _ackermann_msg.header.stamp = ros::Time::now();
    _ackermann_msg.drive.steering_angle = _steering;
    _ackermann_msg.drive.speed = _speed;
    _ackermann_msg.drive.acceleration = _throttle;
    _pub_ackermann.publish(_ackermann_msg);
    */

    // publish general cmd_vel
    if(_pub_twist_flag)
    {
        _twist_msg.linear.x  = _speed;
        _twist_msg.angular.z = _w;
        _pub_twist.publish(_twist_msg);
    }

}

/*****************/
/* MAIN FUNCTION */
/*****************/
int main(int argc, char **argv)
{
    //Initiate ROS
    ros::init(argc, argv, "MPC_Node");
    MPCNode mpc_node;

    ROS_INFO("Waiting for global path msgs ~");
    ros::AsyncSpinner spinner(mpc_node.get_thread_numbers()); // Use multi threads
    spinner.start();
    ros::waitForShutdown();
    return 0;
}


//---------------------------------------------------------------------------
//
//    FCST: Fuel Cell Simulation Toolbox
//
//    Copyright (C) 2006-13 by Energy Systems Design Laboratory, University of Alberta
//
//    This software is distributed under the MIT License.
//    For more information, see the README file in /doc/LICENSE
//
//    - Class:
//    - Description:
//    - Developers:
//    - $Id: porous_layer_test.cc 2605 2014-08-15 03:36:44Z secanell $
//
//---------------------------------------------------------------------------

#include <porous_layer_test.h>
#include <boost/config/no_tr1/utility.hpp>

//---------------------------------------------------------------------------
void PorousLayerTest::setup(){

    ParameterHandler param;
    // Note since I cannot create a parent, I will test the functions of the parent in one of the children:
    FuelCellShop::Layer::GasDiffusionLayer<dim>::declare_GasDiffusionLayer_parameters("Sample Gas Diffusion Layer", param);

    param.enter_subsection("Fuel cell data");
    {
        param.enter_subsection("Sample Gas Diffusion Layer");
        {
            param.set("Gas diffusion layer type", "DesignFibrousGDL");
            {
                param.enter_subsection("Generic data");
                {
                    param.set("Knudsen pore radius [um]",5e-8);
                }
                param.leave_subsection();
            }
            param.leave_subsection();
        }
        param.leave_subsection();
    }

    layer = FuelCellShop::Layer::GasDiffusionLayer<dim>::create_GasDiffusionLayer("Sample Gas Diffusion Layer", param);

}

//---------------------------------------------------------------------------
  /*
void PorousLayerTest::molecular_diffusion_test(){

  double pressure(1.0); //atm
  double temperature(273); //K
  FuelCellShop::Layer::PorousLayer<dim> test("test");
  FuelCellShop::Material::Oxygen oxygen;
  FuelCellShop::Material::Nitrogen nitrogen;
  FcstUtilities::log<<"Computing diffusion coefficient of: "<<oxygen.name_material()<<" in "<<nitrogen.name_material()<<" at "<<pressure<<" atm and "<<temperature<<" Kelvin"<<std::endl;
  std::vector<FuelCellShop::Material::PureGas*> gases;
  gases.push_back(&oxygen);
  gases.push_back(&nitrogen);
  std::vector<VariableNames> deriv_flags;
  deriv_flags.push_back(total_pressure);
  deriv_flags.push_back(temperature_of_REV);
  test.set_derivative_flags(deriv_flags);
  test.set_gases_and_compute(gases, pressure, temperature);
  Table<2, double> D_eff;
  test.gas_diffusion_coefficients(D_eff);
  std::vector<Table< 2, double > > dD_dx;
  test.derivative_gas_diffusion_coefficients(dD_dx);

  FcstUtilities::log<<"The diffusion coefficients are: D_11="<<D_eff(0,0)<<" D_12 = "<<D_eff(0,1)<<"m2/s"<<std::endl;
  FcstUtilities::log<<"The diffusion coefficients are: D_21="<<D_eff(1,0)<<" D_22 = "<<D_eff(1,1)<<"m2/s"<<std::endl;
  FcstUtilities::log<<"The experimental value at 273K and 1atm is: 0.181 cm2/s"<<std::endl;

  FcstUtilities::log<<"The diffusion coefficients are: dD_11_dp="<<dD_dx[0](0,0)<<" dD_12_dp = "<<dD_dx[0](0,1)<<"m2/s"<<std::endl;
  FcstUtilities::log<<"The diffusion coefficients are: dD_21_dp="<<dD_dx[0](1,0)<<" dD_22_dp = "<<dD_dx[0](1,1)<<"m2/s"<<std::endl;

  FcstUtilities::log<<"The diffusion coefficients are: dD_11_dT="<<dD_dx[1](0,0)<<" dD_12_dT = "<<dD_dx[1](0,1)<<"m2/s"<<std::endl;
  FcstUtilities::log<<"The diffusion coefficients are: dD_21_dT="<<dD_dx[1](1,0)<<" dD_22_dT = "<<dD_dx[1](1,1)<<"m2/s"<<std::endl;

}
 */
//---------------------------------------------------------------------------
void PorousLayerTest::Knudsen_diffusion(){

    std::vector<double> T_dummy(8, 298.0);
    FuelCellShop::SolutionVariable sols(T_dummy,temperature_of_REV);

    double D_k_check(1.480150e-5);

    std::vector<double> D_k;
    FuelCellShop::Material::Oxygen oxygen;

    layer->Knudsen_diffusion(&oxygen, sols, D_k);

    //Final check
    std::ostringstream streamOut;

    for (unsigned int i=0; i<T_dummy.size(); ++i){
        streamOut <<"The value of the D_k is: "<<D_k[i]<<". The expected value is: "<<D_k_check<<std::endl;
        streamOut <<"Knudsen_diffusion!"<<std::endl;
        TEST_ASSERT_MSG(std::fabs((D_k_check - D_k[i])/D_k_check)<1e-6, streamOut.str().c_str());
        streamOut.flush();
    }
}

//---------------------------------------------------------------------------
void PorousLayerTest::Knudsen_diffusion_derivatives(){

    double T(298.0);
    std::vector<double> T_dummy(8, T);
    FuelCellShop::SolutionVariable sols(T_dummy,temperature_of_REV);

    double D_k_check(1.480150e-5);
    double dD_k_dT_check(0.5*D_k_check*(1/T));

    std::vector<double> D_k;
    std::vector<double> dD_k_dT;
    FuelCellShop::Material::Oxygen oxygen;

    layer->Knudsen_diffusion(&oxygen, sols, D_k, dD_k_dT);

    //Final check
    std::ostringstream streamOut;
    streamOut <<"The value of the D_k is: "<<D_k[0]<<". The expected value is: "<<D_k_check<<std::endl;
    streamOut <<"Knudsen_diffusion_derivatives!"<<std::endl;
    TEST_ASSERT_MSG(std::fabs((D_k_check - D_k[0])/D_k_check)<1e-6, streamOut.str().c_str());
    streamOut.flush();

    streamOut <<"The value of the dD_k_dT is: "<<dD_k_dT[0]<<". The expected value is: "<<dD_k_dT_check<<std::endl;
    streamOut <<"Knudsen_diffusion_derivatives!"<<std::endl;
    TEST_ASSERT_MSG(std::fabs((dD_k_dT_check - dD_k_dT[0])/dD_k_dT_check)<1e-6, streamOut.str().c_str());
    streamOut.flush();

}


/*
Copyright (c) 2012, Matthew H. Reilly (kb1vc)
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

    Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
    Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in
    the documentation and/or other materials provided with the
    distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "Spectrogram.hxx"
#include <math.h>
#include <iostream>
#include <boost/format.hpp>
#include <stdexcept>

SoDa::Spectrogram::Spectrogram(unsigned int fftlen)
{
  // remember how long the output will be
  fft_len = fftlen;

  // allocate the internal buffers
  win_samp = (std::complex<float>*) fftwf_alloc_complex(fft_len);
  fft_out = (std::complex<float>*) fftwf_alloc_complex(fft_len);
  result = new float[fft_len];

  // create the FFT plan
  fftplan = fftwf_plan_dft_1d(fft_len,
			      (fftwf_complex *) win_samp,
			      (fftwf_complex *) fft_out,
			      FFTW_FORWARD, FFTW_ESTIMATE);

  // setup the blackman harris window.
  window = initBlackmanHarris();
}

float * SoDa::Spectrogram::initBlackmanHarris()
{
  unsigned int i;
  float a0 = 0.35875;
  float a1 = 0.48829;
  float a2 = 0.14128;
  float a3 = 0.01168;

  float * w = new float[fft_len];
  float anginc = 2.0 * M_PI / ((float) fft_len - 1);
  float ang = 0.0;
  for(i = 0; i < fft_len; i++) {
    ang = anginc * ((float) i);
    w[i] = a0 - a1 * cos(ang) + a2 * cos(2.0 * ang) -a3 * cos(3.0 * ang);
  }

  return w;
}

void SoDa::Spectrogram::apply_common(std::complex<float> * invec,
				     unsigned int inveclen)
{
  unsigned int i, j;
  float repl_count;

  repl_count = floor(((float) inveclen) / ((float) fft_len));
  float gain_adj = 1.0 / repl_count;

  // zero the result accumulate buffer.
  for(j = 0; j < fft_len; j++) result[j] = 0.0;
  if(fft_len > inveclen) {
    throw std::runtime_error((boost::format("inveclen %d less than fftlen %d\n") % inveclen % fft_len).str());
  }
  // accumulate FFT results over the length of the buffer.
  for(i = 0; i < (inveclen + 1 - fft_len); i += (fft_len / 2)) {
    std::complex<float> *v = &(invec[i]);

    // window the input
    for(j = 0; j < fft_len; j++) {
      win_samp[j] = v[j] * window[j];
    }

    // do the fft
    fftwf_execute(fftplan);

    // accumulate the magnitude squared result
    for(j = 0; j < fft_len; j++) {
      float re, im;
      re = fft_out[j].real();
      im = fft_out[j].imag();
      result[j] += gain_adj * (re * re + im * im);
    }
  }
}

void SoDa::Spectrogram::apply_acc(std::complex<float> * invec,
				  unsigned int inveclen,
				  float * outvec,
				  float accumulation_gain)
{

  if(inveclen < fft_len) {
    std::cerr << "Input vector is shorter than FFT buffer " <<
      inveclen << " less than " << fft_len << std::endl;
    return;
  }

  // do the front end FFT
  apply_common(invec, inveclen);

  // now copy to the output buffer
  // this is mag^2 divided by the square of of the
  // number of segments we FFTd.
  unsigned int j, k;

  for(j = 0, k = (fft_len / 2); j < (fft_len / 2); j++, k++) {
    outvec[j] = result[k] * (1.0 - accumulation_gain) +
      outvec[j] * accumulation_gain;
  }

  for(j = (fft_len / 2), k = 0; j < fft_len; j++, k++) {
    outvec[j] = result[k] * (1.0 - accumulation_gain) +
      outvec[j] * accumulation_gain;
  }
}

void SoDa::Spectrogram::apply_max(std::complex<float> * invec,
				  unsigned int inveclen,
				  float * outvec,
				  bool first)
{
  // do the front end FFT
  apply_common(invec, inveclen);

  // now copy to the output buffer
  // this is mag^2 divided by the square of of the
  // number of segments we FFTd.
  unsigned int j, k;

  for(j = 0, k = (fft_len / 2); j < (fft_len / 2); j++, k++) {
    if(first) outvec[j] = result[k];
    else {
      outvec[j] = (result[k] > outvec[j]) ? result[k] : outvec[j];
    }
  }

  for(j = (fft_len / 2), k = 0; j < fft_len; j++, k++) {
    if(first) outvec[j] = result[k];
    else {
      outvec[j] = (result[k] > outvec[j]) ? result[k] : outvec[j];
    }
  }
}


#pragma once

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/negative_binomial_distribution.hpp>

#include "util.hpp"

#ifndef DRAW_H
#define DRAW_H

/* Random number generation with Boost */

class Draw {
public:

    Draw( );
    ~Draw( );
    long seed;

    void initialize(const long seed);
    double runif( );
    int rnegbin(const int r, const double p);
    int runif_int(const int min, const int max);
    double rnorm(const double mu, const double var);
    double rtrnorm(const double mu, const double var, const double bnd);
    double rinvgam(const double shape, const double scale);
private:

    boost::mt19937 randgen;
    template<class T>
    double randraw(T &generator) { return generator( ); }

};

#endif


/*
Copyright (C) 2016 Quaternion Risk Management Ltd
All rights reserved.

This file is part of ORE, a free-software/open-source library
for transparent pricing and risk analysis - http://opensourcerisk.org

ORE is free software: you can redistribute it and/or modify it
under the terms of the Modified BSD License.  You should have received a
copy of the license along with this program.
The license is also available online at <http://opensourcerisk.org>

This program is distributed on the basis that it will form a useful
contribution to risk analytics and model standardisation, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ored/marketdata/marketimpl.hpp>
#include <ored/portfolio/builders/yoycapfloor.hpp>
#include <ored/portfolio/capfloor.hpp>
#include <ored/portfolio/enginedata.hpp>
#include <ored/portfolio/portfolio.hpp>
#include <ored/utilities/indexparser.hpp>
#include <ored/utilities/log.hpp>
#include <oret/toplevelfixture.hpp>
#include <ql/indexes/inflation/euhicp.hpp>
#include <ql/instruments/inflationcapfloor.hpp>
#include <ql/pricingengines/inflation/inflationcapfloorengines.hpp>
#include <ql/termstructures/inflation/inflationhelpers.hpp>
#include <ql/termstructures/volatility/inflation/yoyinflationoptionletvolatilitystructure.hpp>
#include <ql/termstructures/yield/discountcurve.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <qle/indexes/inflationindexwrapper.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace std;
using namespace ore::data;

// FX Swap test
class TestMarket : public MarketImpl {
public:
    TestMarket(Date asof) {
        // valuation date

        dc = ActualActual();
        bdc = Following;
        cal = TARGET();

        // Add EUR notional curve
        Handle<YieldTermStructure> nominalTs(boost::make_shared<FlatForward>(0, cal, 0.005, dc));
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "EUR")] = nominalTs;

        // Add EUHICPXT YoY Inflation Curve
        boost::shared_ptr<ZeroInflationIndex> zcindex = boost::shared_ptr<EUHICPXT>(new EUHICPXT(false));
        ;
        boost::shared_ptr<YoYInflationIndex> index =
            boost::make_shared<QuantExt::YoYInflationIndexWrapper>(zcindex, zcindex->interpolated());

        std::vector<Date> datesZCII = {asof + 1 * Years, asof + 2 * Years, asof + 5 * Years, asof + 10 * Years,
                                       asof + 20 * Years};
        std::vector<Rate> ratesZCII = {1.1625, 1.23211, 1.36019, 1.51199, 1.74773};

        std::vector<boost::shared_ptr<YoYInflationTraits::helper>> instruments;
        for (Size i = 0; i < datesZCII.size(); i++) {
            Handle<Quote> quote(boost::shared_ptr<Quote>(new SimpleQuote(ratesZCII[i] / 100)));
            boost::shared_ptr<YoYInflationTraits::helper> anInstrument =
                boost::make_shared<YearOnYearInflationSwapHelper>(quote, Period(3, Months), datesZCII[i], cal, bdc, dc,
                                                                  index, nominalTs);
            instruments.push_back(anInstrument);
        };
        boost::shared_ptr<YoYInflationTermStructure> yoyTs = boost::shared_ptr<PiecewiseYoYInflationCurve<Linear>>(
            new PiecewiseYoYInflationCurve<Linear>(asof, TARGET(), Actual365Fixed(), Period(3, Months), Monthly,
                                                   index->interpolated(), ratesZCII[0] / 100, instruments));

        yoyInflationIndices_[make_pair(Market::defaultConfiguration, "EUHICPXT")] =
            Handle<YoYInflationIndex>(boost::make_shared<QuantExt::YoYInflationIndexWrapper>(
                parseZeroInflationIndex("EUHICPXT", false), false, Handle<YoYInflationTermStructure>(yoyTs)));

        // Add EUHICPXT yoy volatility term structure
        boost::shared_ptr<QuantLib::ConstantYoYOptionletVolatility> volSurface =
            boost::make_shared<QuantLib::ConstantYoYOptionletVolatility>(0.01, 0, cal, bdc, dc, Period(3, Months),
                                                                         Monthly, index->interpolated(), -1.0, 100.0,
                                                                         VolatilityType::Normal);

        yoyCapFloorVolSurfaces_[make_pair(Market::defaultConfiguration, "EUHICPXT")] =
            Handle<QuantExt::YoYOptionletVolatilitySurface>(volSurface);
    }

    BusinessDayConvention bdc;
    DayCounter dc;
    Calendar cal;
};

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(InflationCapFloorTests)

BOOST_AUTO_TEST_CASE(testYoYCapFloor) {

    BOOST_TEST_MESSAGE("Testing YoY Cap Price...");

    // build market
    Date today(18, July, 2016);
    Settings::instance().evaluationDate() = today;

    boost::shared_ptr<TestMarket> market = boost::make_shared<TestMarket>(today);
    // Test if EUR yoy inflation curve is empty
    Handle<YoYInflationIndex> infidx = market->yoyInflationIndex("EUHICPXT");
    QL_REQUIRE(!infidx.empty(), "EUHICPXT inflation index not found");

    // envelope
    Envelope env("CP");

    // Start/End date
    Date startDate = today;
    Date endDate = today + 5 * Years;

    // date 2 string
    std::ostringstream oss;
    oss << io::iso_date(startDate);
    string start(oss.str());
    oss.str("");
    oss.clear();
    oss << io::iso_date(endDate);
    string end(oss.str());

    // Schedules
    string conv = "F";
    string rule = "Forward";
    ScheduleData scheduleYY(ScheduleRules(start, end, "1Y", "TARGET", conv, conv, rule));

    // Leg variables
    string dc = "ACT/ACT";
    vector<Real> notional(1, 10000000);
    string paymentConvention = "F";

    // EUR YoY Leg
    bool isPayerYY = false;
    string indexYY = "EUHICPXT";
    string lag = "3M";
    LegData legYY(boost::make_shared<YoYLegData>(indexYY, lag, 0), isPayerYY, "EUR", scheduleYY, dc, notional,
                  vector<string>(), paymentConvention, false, true);

    std::vector<double> caps(1, 0.009);
    // Build capfloor trades
    boost::shared_ptr<Trade> yyCap(new ore::data::CapFloor(env, "Long", legYY, caps, std::vector<double>()));

    // engine data and factory
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("YYCapFloor") = "YYCapModel";
    engineData->engine("YYCapFloor") = "YYCapEngine";
    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);
    engineFactory->registerBuilder(boost::make_shared<YoYCapFloorEngineBuilder>());

    // build capfloor and portfolio
    boost::shared_ptr<Portfolio> portfolio(new Portfolio());
    yyCap->id() = "YoY_Cap";

    portfolio->add(yyCap);
    portfolio->build(engineFactory);

    // check YoY cap NPV against pure QL pricing
    Schedule schedule(startDate, endDate, 1 * Years, TARGET(), Following, Following, DateGeneration::Forward, false);
    Handle<YieldTermStructure> nominalTs = market->discountCurve("EUR");
    Leg yyLeg =
        yoyInflationLeg(schedule, TARGET(), market->yoyInflationIndex("EUHICPXT").currentLink(), Period(3, Months))
            .withNotionals(10000000)
            .withPaymentDayCounter(ActualActual())
            .withPaymentAdjustment(Following)
            .withRateCurve(nominalTs);

    boost::shared_ptr<YoYInflationCapFloor> qlCap(new YoYInflationCap(yyLeg, caps));

    Handle<QuantLib::YoYOptionletVolatilitySurface> hovs = market->yoyCapFloorVol("EUHICPXT");
    auto dscEngine = boost::make_shared<YoYInflationBachelierCapFloorEngine>(
        market->yoyInflationIndex("EUHICPXT").currentLink(), hovs, nominalTs);
    qlCap->setPricingEngine(dscEngine);
    BOOST_CHECK_CLOSE(yyCap->instrument()->NPV(), qlCap->NPV(), 1E-8); // this is 1E-10 rel diff
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <iomanip>
#include <random>
#include <boost/program_options.hpp>
#include <boost/io/ios_state.hpp>

#include <vexcl/devlist.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/fft.hpp>

using namespace vex;

typedef stopwatch<> watch;

#ifdef VEXCL_HAVE_CUDA
#  include <cufft.h>
#  include <cuda_runtime.h>

void check(cudaError_t status, const char *msg) {
    if (status != cudaSuccess)
        throw std::runtime_error(msg);
}

void check(cufftResult status, const char *msg) {
    if (status != CUFFT_SUCCESS)
        throw std::runtime_error(msg);
}

watch test_cufft(cl_float2 *data, size_t n, size_t m, size_t runs, bool) {
    size_t dataSize = sizeof(cufftComplex) * n * m;

    cufftHandle plan;
    if(m == 1)
        check(cufftPlan1d(&plan, (int)n, CUFFT_C2C, 1), "cufftPlan1d");
    else
        check(cufftPlan2d(&plan, (int)n, (int)m, CUFFT_C2C), "cufftPlan2d");

    cufftComplex *inData;
    check(cudaMalloc((void **)(&inData), dataSize), "cudaMalloc");

    cufftComplex *outData;
    check(cudaMalloc((void **)(&outData), dataSize), "cudaMalloc");

    // Send X to device
    check(cudaMemcpy(inData, data, dataSize, cudaMemcpyHostToDevice), "cudaMemcpy");

    watch w;
    for(size_t i = 0 ; i < runs ; i++) {
        cudaDeviceSynchronize();
        w.tic();
        cufftExecC2C(plan, inData, outData, CUFFT_FORWARD);
        cudaDeviceSynchronize();
        w.toc();
    }

    cufftDestroy(plan);
    cudaFree(inData);
    cudaFree(outData);
    return w;
}
#else
watch test_cufft(cl_float2 *, size_t, size_t, size_t, bool) {
    return watch();
}
#endif

#ifdef VEXCL_HAVE_FFTW
#  ifdef _OPENMP
#    include <omp.h>
#  endif
#  include <fftw3.h>

watch test_fftw(cl_float2 *data, size_t n, size_t m, size_t runs, bool dump_plan) {
    int sz[2] = {(int)n, (int)m};
    fftwf_complex *out = reinterpret_cast<fftwf_complex *>(
        fftwf_malloc(sizeof(fftwf_complex) * n * m));
    fftwf_plan p1 = fftwf_plan_dft(m == 1 ? 1 : 2, sz,
        reinterpret_cast<fftwf_complex *>(data),
        out, FFTW_FORWARD, FFTW_MEASURE);

    watch w;
    for(size_t i = 0 ; i < runs ; i++) {
        w.tic();
        fftwf_execute(p1);
        w.toc();
    }

    if(dump_plan) fftwf_fprint_plan(p1, stderr);
    fftwf_destroy_plan(p1);
    fftwf_free(out);
    return w;
}
#else
watch test_fftw(cl_float2 *, size_t, size_t, size_t, bool) {
    return watch();
}
#endif

watch test_clfft(Context &ctx, cl_float2 *data, size_t n, size_t m, size_t runs, bool dump_plan) {
    vector<cl_float2> a(ctx, n * m, data);
    vector<cl_float2> b(ctx, n * m);
    std::vector<size_t> sz; sz.push_back(n); if(m > 1) sz.push_back(m);
    FFT<cl_float2> fft(ctx, sz);

    watch w;
    for(size_t i = 0 ; i < runs ; i++) {
        ctx.finish();
        w.tic();
        b = fft(a);
        ctx.finish();
        w.toc();
    }

    if(dump_plan) std::cerr << fft.plan;
    return w;
}

void info(const watch &w, size_t size, size_t dim) {
    boost::io::ios_all_saver stream_state(std::cout);

    // FFT is O(n log n)
    double ops = dim == 1
        ? size * std::log(static_cast<double>(size)) // O(n log n)
        : 2.0 * size * size * std::log(static_cast<double>(size)); // O(n log n)[1D fft] * n[rows] * 2[transposed]
    std::cout << '\t';
    if(w.tics() == 0)
        std::cout << '-';
    else
        std::cout << std::scientific << (ops / w.average());
}

int main(int argc, char **argv) {
    using namespace boost::program_options;
    options_description desc("Options");
    bool add_prime = false, dump_plan = false;
    size_t composite = 0, min, max, runs;
    desc.add_options()
        ("help,h", "show help")
        ("runs,r", value(&runs)->default_value(1000),
            "run each FFT multiple times")
        ("min", value(&min)->default_value(1 << 4),
            "minimum size")
        ("max", value(&max)->default_value(1 << 20),
            "maximum size (all powers of two between)")
        ("prime,p", bool_switch(&add_prime),
            "add nearest prime")
        ("composite,c", value(&composite)->implicit_value(23),
            "add nearest multiple of this number (for each power of two)")
        ("dump-plan", bool_switch(&dump_plan),
            "show each CLFFT plan on stderr");
    variables_map vm;
    store(parse_command_line(argc, argv, desc), vm);
    notify(vm);
    if(vm.count("help") || min > max) {
        std::cerr << desc << std::endl;
        return EXIT_FAILURE;
    }

#if defined(_OPENMP) && defined(VEXCL_HAVE_FFTW)
    fftwf_init_threads();
    fftwf_plan_with_nthreads(omp_get_max_threads());
#endif
    Context ctx(Filter::Env && Filter::Count(1));
    std::cerr << ctx << std::endl;

    // power of two sizes
    std::vector<size_t> twos, ns;
    for(size_t n = min ; n <= max ; n *= 2) {
        twos.push_back(n);
        ns.push_back(n);
    }

    if(add_prime) {
        vex::fft::prime_generator prime;
        size_t prev = 0, next = 0;
        for(auto n = twos.begin() ; n != twos.end() ; n++) {
            while(next <= *n) { prev = next; next = prime(); }
            const size_t p = *n - prev < next - *n ? prev : next;
            ns.push_back(p);
        }
    }

    if(composite != 0) {
        for(auto n = twos.begin() ; n != twos.end() ; n++) {
            const size_t m = static_cast<size_t>(*n / static_cast<double>(composite) + 0.5);
            if(m > 1) ns.push_back(composite * m);
        }
    }

    const size_t max_len = *std::max_element(ns.begin(), ns.end());

    // random data
#ifdef VEXCL_HAVE_FFTW
    cl_float2 *data = reinterpret_cast<cl_float2 *>(
        fftwf_malloc(sizeof(cl_float2) * max_len));
#else
    cl_float2 *data = new cl_float2[max_len];
#endif
    std::minstd_rand gen;
    std::uniform_real_distribution<float> dist(-1000, 1000);
    for(size_t i = 0 ; i < 2 * max_len ; i++)
        reinterpret_cast<float*>(data)[i] = dist(gen);

    // 1D
    std::cout << "# prints `n log n / time`\n";
    std::cout << "#n\tfftw^1\tclfft^1\tcufft^1" << std::endl;
    for(auto n = ns.begin() ; n != ns.end() ; n++) {
        std::cout << *n;
        info(test_fftw (     data, *n, 1, runs, dump_plan), *n, 1);
        info(test_clfft(ctx, data, *n, 1, runs, dump_plan), *n, 1);
        info(test_cufft(     data, *n, 1, runs, dump_plan), *n, 1);
        std::cout << std::endl;
    }
    std::cout << std::endl;

    // 2D
    std::cout << "# prints `2 n^2 log n / time`\n";
    std::cout << "#n\tfftw^2\tclfft^2\tcufft^2" << std::endl;
    for(auto n = ns.begin() ; n != ns.end() ; n++)
        if(*n * *n <= max_len) {
            std::cout << *n;
            info(test_fftw (     data, *n, *n, runs, dump_plan), *n, 2);
            info(test_clfft(ctx, data, *n, *n, runs, dump_plan), *n, 2);
            info(test_cufft(     data, *n, *n, runs, dump_plan), *n, 2);
            std::cout << std::endl;
        }

    return 0;
}


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/list.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/math/mean_and_variance.h>
#include <scitbx/array_family/boost_python/shared_wrapper.h>
#include <scitbx/examples/bevington/prototype_core.h>
#include <Eigen/Sparse>
#include <boost/python/return_internal_reference.hpp>

using namespace boost::python;
namespace scitbx{
namespace example{
namespace boost_python { namespace {

  void
  scaling_init_module() {
    using namespace boost::python;

    class_<linear_ls_eigen_wrapper>("linear_ls_eigen_wrapper", no_init)
      .def(init<int>(arg("n_parameters")))
      .def("n_parameters", &linear_ls_eigen_wrapper::n_parameters)
      .def("reset", &linear_ls_eigen_wrapper::reset)
      .def("right_hand_side", &linear_ls_eigen_wrapper::right_hand_side)
      .def("solve", &linear_ls_eigen_wrapper::solve)
      .def("solved", &linear_ls_eigen_wrapper::solved)
      .def("normal_matrix", &linear_ls_eigen_wrapper::normal_matrix)
      .def("solution", &linear_ls_eigen_wrapper::solution)
    ;

    typedef non_linear_ls_eigen_wrapper nllsew;
    boost::python::return_internal_reference<> rir;
    class_<nllsew,
           bases<scitbx::lstbx::normal_equations::non_linear_ls<double> > >(
           "non_linear_ls_eigen_wrapper", no_init)
      .def(init<int const&>(arg("n_parameters")))
      .def("reset", &nllsew::reset)
      .def("step_equations",&nllsew::step_equations, rir)
      .def("add_constant_to_diagonal",&nllsew::add_constant_to_diagonal)
      .def("show_eigen_summary",&nllsew::show_eigen_summary)
      .def("get_normal_matrix_diagonal",&nllsew::get_normal_matrix_diagonal)
      .def("get_normal_matrix",&nllsew::get_normal_matrix)
      .def("get_cholesky_lower", &nllsew::get_cholesky_lower)
      .def("get_cholesky_diagonal", &nllsew::get_cholesky_diagonal)
      .def("get_eigen_permutation_ordering", &nllsew::get_eigen_permutation_ordering)
      .def("add_equations", &nllsew::add_equations)
      .def("solved", &nllsew::solved)
      .def("get_normal_matrix_ncols", &nllsew::get_normal_matrix_ncols)
      .def("n_parameters", &nllsew::n_parameters)
      .def("get_normal_matrix_nnonZeros", &nllsew::get_normal_matrix_nnonZeros)
      .def("get_lower_cholesky_nnonZeros", &nllsew::get_lower_cholesky_nnonZeros)
    ;

    typedef bevington_silver silver;
    class_<silver>( "bevington_silver")
      .def(init<>())
      .def("set_cpp_data", &silver::set_cpp_data)
      .def("fvec_callable", &silver::fvec_callable)
      .def("gvec_callable", &silver::gvec_callable)
      .def("curvatures", &silver::curvatures)
      .def("functional", &silver::functional)
      .def("get_xobs", &silver::get_xobs)
      .def("get_wobs", &silver::get_wobs)
    ;

    typedef dense_base_class dbc;
    class_<dbc,bases<silver, scitbx::lstbx::normal_equations::non_linear_ls<double> > >( "dense_base_class", no_init)
      .def(init<int>(arg("n_parameters")))
      .def("access_cpp_build_up_directly_dense", &dbc::access_cpp_build_up_directly_dense,
        (arg("objective_only"),arg("current_values")))
    ;

    typedef eigen_base_class bev;
    class_<bev,bases<silver, nllsew> >( "eigen_base_class", no_init)
      .def(init<int>(arg("n_parameters")))
      .def("access_cpp_build_up_directly_eigen_eqn", &bev::access_cpp_build_up_directly_eigen_eqn,
        (arg("objective_only"),arg("current_values")))
    ;
  }

}
}}} // namespace xfel::boost_python::<anonymous>

BOOST_PYTHON_MODULE(scitbx_examples_bevington_ext)
{
  scitbx::example::boost_python::scaling_init_module();

}


#include <stdio.h>
#include <string>
#include <map>
#include <fstream>
#include "llvm/Support/raw_ostream.h"
#include <cstdio>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <array>
#include <regex>
#include "helper.h"
#include "klee/ExecutionState.h"
#include "klee/Internal/ADT/RNG.h"
#include <boost/math/distributions/binomial.hpp>
#include "Executor.h"
#include <cmath>
#include "klee/klee.h"

#include "BFHandler.h"

using namespace klee;
using namespace std;
using namespace llvm;
using boost::math::binomial;

namespace klee {
  extern RNG theRNG;
}

namespace boost {
   void throw_exception( std::exception const & e ) {

   }
}

BFHandler::BFHandler(Executor &_executor)
: executor(_executor)
{
   printf("new bf handler created!\n");
}

BFHandler::BFHandler(BFHandler &_handler)
: executor(_handler.executor)
{

}

BFHandler::~BFHandler()
{

}

BF::BF()
{
}

BF::BF(int _size):
   size(_size),
   cur_size(0)
{
   // create an empty table
   LOG(LOG_MASK_BF, "creating an empty hash table, size=%d", _size);
}

BF::BF(const BF &_table):
   size(_table.size),
   cur_size(_table.cur_size)
{
}

BF::~BF()
{
}

int BFHandler::bf_init(ExecutionState &cur, size_t sz)
{
   LOG(LOG_MASK_BF, "allocating bf, size=%ld", sz);
   cur.bf = new BF(sz);
   return 0;
}

// This function computes the probability that for a BF size >=3,
// a new accss would add n new dirty bits to the BF
double BFHandler::get_path_prob(BF* bf, int n)
{
   double ret = 0;
   assert(bf->cur_size >= 3);
   assert(n >= 0 && n <= 3);
   // uint64_t k = bf->cur_size;
   // uint64_t N = bf->size;
   // uint64_t base = N * N * N;
   double k = bf->cur_size;
   double N = bf->size;
   double base = N * N * N;

   if (n == 0) {
      ret = k * k * k / base;
   } else if (n == 1) {
      ret = (3 * k * k * (N - k) + 3 * k * (N - k) + (N - k)) / base;
   } else if (n == 2) {
      ret = (3 * k * (N - k) * (N - k - 1) + 3 * (N - k) * (N - k - 1)) / base;
   } else if (n == 3) {
      ret = (N - k) * (N - k - 1) * (N - k - 2) / base;
   }

   // LOG(LOG_MASK_BF, "n=%d, k=%f, N=%f, ret=%f", n, k, N, ret);
   return ret;
}

int BFHandler::bf_access(ExecutionState &cur,
                         vector<ExecutionState*> &states,
                         vector<int> &rets)
{
   BF *bf = cur.bf;
   assert(bf);
   LOG(LOG_MASK_BF, "accessing bf, cur size=%d", bf->cur_size);

   // It's impossible that BF size is 1 or 2
   assert(bf->cur_size == 0 || bf->cur_size >= 3);

   // When the BF is empty, can only produce "miss" result
   if (bf->cur_size == 0) {
      bf->cur_size = 3;
      states.push_back(&cur);
      rets.push_back(BF_ACCESS_MISS_ADD3);

      LOG(LOG_MASK_BF, "accessing an empty bf, returning one path (miss)");
      return 0;
   }

   // When BF.size >= 3, that will be 4 possible outputs
   executor.branchTableAccessNoCond(cur, 4, states);
   rets.push_back(BF_ACCESS_HIT);
   rets.push_back(BF_ACCESS_MISS_ADD1);
   rets.push_back(BF_ACCESS_MISS_ADD2);
   rets.push_back(BF_ACCESS_MISS_ADD3);

   // update path probs for child states
   // for the first branch, bf size doesn't change
   states[0]->updatePathProb(get_path_prob(bf, 0));

   states[1]->updatePathProb(get_path_prob(bf, 1));
   states[1]->bf->cur_size += 1;

   states[2]->updatePathProb(get_path_prob(bf, 2));
   states[2]->bf->cur_size += 2;

   states[3]->updatePathProb(get_path_prob(bf, 3));
   states[3]->bf->cur_size += 3;

   LOG(LOG_MASK_BF, "accessing a non-empty bf, returning 4 paths");
   return 0;
}

int BFHandler::bf_read(ExecutionState &cur,
                         vector<ExecutionState*> &states,
                         vector<int> &rets)
{
   BF *bf = cur.bf;
   assert(bf);
   LOG(LOG_MASK_BF, "reading bf, cur size=%d", bf->cur_size);

   // It's impossible that BF size is 1 or 2
   assert(bf->cur_size == 0 || bf->cur_size >= 3);

   // When the BF is empty, can only produce "miss" result
   if (bf->cur_size == 0) {
      states.push_back(&cur);
      rets.push_back(GREYBOX_MISS);
      LOG(LOG_MASK_BF, "bf is empty, read misses");
      return 0;
   }

   // When BF.size >= 3, that will be 2 possibilities: miss or hit
   double probHit = get_path_prob(bf, 0);
   double probMiss = 1 - probHit;
   executor.branchTableAccessNoCond(cur, 2, states);

   rets.push_back(GREYBOX_HIT);
   rets.push_back(GREYBOX_MISS);

   states[0]->updatePathProb(probHit);
   states[1]->updatePathProb(probMiss);

   LOG(LOG_MASK_BF, "bf is empty, read misses");

   return 0;
}


#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>


/*
Copyright 2009-2021 Nicolas Colombe

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include <core/randomsetwalk.hpp>
#include <core/random.hpp>

#include <boost/container/flat_set.hpp>

namespace eXl
{
  struct PrimeNumbers
  {
    PrimeNumbers()
    {
      m_Primes.insert(2);
      m_Primes.reserve(1000);
      AddUpTo(1000);
    }

    void AddUpTo(unsigned int iMin)
    {
      unsigned int start = *m_Primes.rbegin();
      unsigned int curMaxPrime = start;
      ++start;
      while (curMaxPrime <= iMin)
      {
        unsigned int limit = sqrt(double(start));
        boost::container::flat_set<unsigned int>::iterator iter = m_Primes.begin();
        boost::container::flat_set<unsigned int>::iterator iterEnd = m_Primes.end();
        for (; iter != iterEnd; ++iter)
        {
          if (*iter > limit)
          {
            iter = iterEnd;
            break;
          }
          if (start % *iter == 0)
          {
            break;
          }
        }
        if (iter == iterEnd)
        {
          //printf("%i\n", start);
          curMaxPrime = start;
          m_Primes.insert(start);
        }
        ++start;
      }
    }

    unsigned int Get(unsigned int iValue)
    {
      if (iValue >= *m_Primes.rbegin())
      {
        AddUpTo(iValue);
        return *m_Primes.rbegin();
      }
      else
      {
        return *m_Primes.upper_bound(iValue);
      }
    }

    boost::container::flat_set<unsigned int> m_Primes;
  };

  static PrimeNumbers s_Prime;

  RandomSetWalk::RandomSetWalk(unsigned int iSetSize, Random& iRand)
  {
    m_Size = iSetSize;
    m_Prime = s_Prime.Get(iSetSize);
    Reset(iRand);
  }

  bool RandomSetWalk::Next(unsigned int& oNum)
  {
    if (m_Step < m_Size)
    {
      oNum = m_Current;
      do
      {
        m_Current += m_Skip;
        m_Current %= m_Prime;
      }
      while(m_Current >= m_Size);
      ++m_Step;
      return true;
    }
    return false;
  }

  void RandomSetWalk::Reset(Random& iRand)
  {
    do
    {
      m_Skip = ((iRand.Generate() % (m_Prime - 1)) + 1) * m_Size * m_Size + ((iRand.Generate() % (m_Prime - 1)) + 1) * m_Size + ((iRand.Generate() % (m_Prime - 1)) + 1);
    }while(m_Skip % m_Prime == 0);
    m_Current = iRand.Generate() % m_Size;
    m_Step = 0;
  }
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/map_view.hpp>
#include <boost/numeric/mtl/matrix/hermitian_view.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/recursion/predefined_masks.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/mtl/operation/scale.hpp>
#include <boost/numeric/mtl/operation/hermitian.hpp>

using std::cout; using std::complex;

typedef complex<double> ct;

double value(double)
{
    return 7.0;
}

complex<double> value(complex<double>)
{
    return ct(7.0, 1.0);
}

template <typename MatrixA, typename MatrixB, typename MatrixC>
void test(MatrixA&, MatrixB&, MatrixC&, const char* name)
{
    MatrixA a(7, 7);
    MatrixB b(7, 7);
    MatrixC c(7, 7);

    set_to_zero(a);
    {
	typename MatrixA::value_type ref(0);
	mtl::mat::inserter<MatrixA>  ins(a);
	ins(2, 3) << value(ref);
	ins(4, 3) << value(ref) + 1.0;
	ins(2, 5) << value(ref) + 2.0;
    }
    std::complex<double> sum= a[4][3], diff= a[4][3];

    set_to_zero(b);
    {
	typename MatrixB::value_type ref(0);
	mtl::mat::inserter<MatrixB>  ins(b);
	ins(2, 2) << value(ref) + 3.0;
	ins(4, 3) << value(ref) + 4.0;
    }
    sum+= b[4][3];
    diff-= b[4][3];

    cout << "\n\n" << name << "\n";
    cout << "Original matrices:\nA=\n" << a << "B=\n" << b << "\n";

    c= a + b;

    cout << "C= A + B\n" << c << "\n";
    MTL_THROW_IF(c[4][3] != sum, mtl::runtime_error("wrong sum"));

    c= a + b + a + b;

    cout << "C= A + B + A + B\n" << c << "\n";
    MTL_THROW_IF(c[4][3] != 2.0*sum, mtl::runtime_error("wrong sum"));
    c+= a + b;

    cout << "C+= A + B\n" << c << "\n";
    MTL_THROW_IF(c[4][3] != 3.0*sum, mtl::runtime_error("wrong increment by sum"));

    cout << "A + B\n" << a+b << "\n";
    cout << "(A + B)[4][3] = " << (a+b)[4][3] << "\n\n";

    c-= a + b;
    cout << "C-= A + B\n" << c << "\n";
    MTL_THROW_IF(c[4][3] != 2.0*sum, mtl::runtime_error("wrong decrement by sum"));

    c= a - b;

    cout << "C= A - B\n" << c << "\n";
    MTL_THROW_IF(c[4][3] != diff, mtl::runtime_error("wrong difference"));

    c-= a - b;
    cout << "C-= A - B\n" << c << "\n";
    MTL_THROW_IF(c[4][3] != 0.0, mtl::runtime_error("wrong decrement by difference"));
}

int main(int argc, char* argv[])
{
    using namespace mtl;
    unsigned size= 7;
    if (argc > 1) size= atoi(argv[1]);

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);
    morton_dense<double, recursion::morton_z_mask>       mzd(size, size);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(size, size);
    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<complex<double> >                            drc(size, size);
    compressed2D<complex<double> >                       crc(size, size);

    test(dr, dr, dr, "Dense row major");
    test(dc, dr, dr, "Dense column major as sum of dense rows");
    test(dc, dr, dc, "Dense column major as sum of dense rows and column");

    test(mzd, mzd, mzd, "Morton Z-order");
    test(d2r, mzd, d2r, "Hybrid 2 row-major + Morton Z-order");

    test(cr, cr, cr, "Compressed row major");
    test(cc, cr, cc, "Compressed column major + row");

    test(drc, drc, drc, "Dense row major complex");
    test(drc, dc, drc, "Dense row major complex + column double");
    test(crc, crc, crc, "Compressed row major complex");
    test(crc, dc, crc, "Compressed row major complex + dense column major double");

    return 0;
}


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/bitfloating.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/bitincrement.hpp>

template <typename T, int N, typename Env>
void test(Env& $)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using p_t = bs::pack<T, N>;
  using fT = bd::as_floating_t<T>;
  using f_t =bs::pack<fT, N>;

  T a1[N];
  fT b[N];
  for(int i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::bitfloating(a1[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  f_t bb (&b[0], &b[0]+N);
  STF_IEEE_EQUAL(bs::bitfloating(aa1), bb);
}

STF_CASE_TPL("Check bitfloating on pack" , (uint32_t)(uint64_t)(int32_t)(int64_t))//STF_CONVERTIBLE_TYPES) // to define ?
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

STF_CASE_TPL (" bitfloating int_convert",  (int32_t)(int64_t))
{
  namespace bs = boost::simd;
  using bs::bitfloating;
  using p_t = bs::pack<T>;
  using r_t = decltype(bitfloating(p_t()));
  typedef  boost::dispatch::as_floating_t<p_t>  wished_r_t;

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

  // specific values tests
  STF_EQUAL(bitfloating(bs::One<p_t>()), bs::Bitincrement<r_t>());
  STF_EQUAL(bitfloating(bs::Mone<p_t>()), -bs::Bitincrement<r_t>());
  STF_EQUAL(bitfloating(bs::Zero<p_t>()), bs::Zero<r_t>());
}

STF_CASE_TPL (" bitfloating uint_convert",  (uint32_t)(uint64_t))
{
  namespace bs = boost::simd;
  using bs::bitfloating;
  using p_t = bs::pack<T>;
  using r_t = decltype(bitfloating(p_t()));
  typedef boost::dispatch::as_floating_t<p_t>  wished_r_t;

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

  // specific values tests
  STF_EQUAL(bitfloating(bs::One<p_t>()), bs::Bitincrement<r_t>());
  STF_EQUAL(bitfloating(bs::Zero<p_t>()), bs::Zero<r_t>());
}


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <tuple>
#include <NTL/BasicThreadPool.h>

#include <EncryptedArray.h>
#include <FHE.h>

#include <intraSlot.h>
#include <binaryArith.h>

#include "gtest/gtest.h"
#include "test_common.h"

#ifdef DEBUG_PRINTOUT
#include <debugging.h>
#endif
// define flags FLAG_PRINT_ZZX, FLAG_PRINT_POLY, FLAG_PRINT_VEC, functions
//        decryptAndPrint(ostream, ctxt, sk, ea, flags)
//        decryptAndCompare(ctxt, sk, ea, pa);

namespace {

struct Parameters {
    Parameters(long prm, long bitSize, long bitSize2, long outSize, bool bootstrap, long seed, long nthreads) :
        prm(prm),
        bitSize(bitSize),
        bitSize2(bitSize2),
        outSize(outSize),
        bootstrap(bootstrap),
        seed(seed),
        nthreads(nthreads)
    {};

    long prm;           // parameter size (0-tiny,...,7-huge)
    long bitSize;       // bitSize of input integers (<=32)
    long bitSize2;      // bitSize of 2nd input integer (<=32)
    long outSize;       // bitSize of output integers, as many as needed
    bool bootstrap;     // test multiplication with bootstrapping
    long seed;          // PRG seed
    long nthreads;      // number of threads

    friend std::ostream& operator<<(std::ostream& os, const Parameters& params) {
        return os << "{" <<
           "prm=" << params.prm << "," <<
           "bitSize=" << params.bitSize << "," <<
           "bitSize2=" << params.bitSize2 << "," <<
           "outSize=" << params.outSize << "," <<
           "bootstrap=" << params.bootstrap << "," <<
           "seed=" << params.seed << "," <<
           "nthreads=" << params.nthreads <<
           "}";
    };
};

class GTest_binaryArith : public ::testing::TestWithParam<std::tuple<Parameters, int>> {
    protected:
        static std::vector<zzX> unpackSlotEncoding;
        constexpr static long mValues[8][15] = {
            // { p, phi(m),   m,   d, m1, m2, m3,    g1,   g2,   g3, ord1,ord2,ord3, B,c}
            {  2,    48,   105, 12,   3, 35,  0,    71,    76,    0,   2,  2,   0, 25, 2},
            {  2 ,  600,  1023, 10,  11, 93,  0,   838,   584,    0,  10,  6,   0, 25, 2},
            {  2,  2304,  4641, 24,   7,  3,221,  3979,  3095, 3760,   6,  2,  -8, 25, 3},
            {  2,  5460,  8193, 26,8193,  0,  0,    46,     0,    0, 210,  0,   0, 25, 3},
            {  2,  8190,  8191, 13,8191,  0,  0,    39,     0,    0, 630,  0,   0, 25, 3},
            {  2, 10752, 11441, 48,  17,673,  0,  4712,  2024,    0,  16,-14,   0, 25, 3},
            {  2, 15004, 15709, 22,  23,683,  0,  4099, 13663,    0,  22, 31,   0, 25, 3},
            {  2, 27000, 32767, 15,  31,  7,151, 11628, 28087,25824,  30,  6, -10, 28, 4}
        };

        static long correctBitSize(long minimum, long oldBitSize)
        {
            long newBitSize;
            if(oldBitSize <= 0) newBitSize = minimum;
            else if(oldBitSize > 32) newBitSize = 32;
            else newBitSize = oldBitSize;
            return newBitSize;
        };

        // Validates the prm value, throwing if invalid
        static long validatePrm(long prm)
        {
            if(prm < 0 || prm >= 5)
                throw std::invalid_argument("prm must be in the interval [0, 4]");
            return prm;
        };

        static NTL::Vec<long> calculateMvec(const long *vals)
        {
            NTL::Vec<long> mvec;
            NTL::append(mvec, vals[4]);
            if (vals[5]>1) NTL::append(mvec, vals[5]);
            if (vals[6]>1) NTL::append(mvec, vals[6]);
            return mvec;
        };

        static std::vector<long> calculateGens(const long *vals)
        {
            std::vector<long> gens;
            gens.push_back(vals[7]);
            if (vals[8]>1) gens.push_back(vals[8]);
            if (vals[9]>1) gens.push_back(vals[9]);
            return gens;
        };

        static std::vector<long> calculateOrds(const long *vals)
        {
            std::vector<long> ords;
            ords.push_back(vals[10]);
            if (abs(vals[11])>1) ords.push_back(vals[11]);
            if (abs(vals[12])>1) ords.push_back(vals[12]);
            return ords;
        };

        static long calculateLevels(bool bootstrap, long outSize, long bitSize)
        {
            long L;
            if (bootstrap) L=900; // that should be enough
            else {
                double nBits =
                    (outSize>0 && outSize<2*bitSize)? outSize : (2*bitSize);
                double three4twoLvls = log(nBits/2) / log(1.5);
                double add2NumsLvls = log(nBits) / log(2.0);
                L = (5 + ceil(three4twoLvls + add2NumsLvls))*30;
            }
            return L;
        };

        // Returns a reference to the passed-in context once it has been modified to get it ready for test.
        // This is not static because it uses quite a lot of state of the object.
        FHEcontext& prepareContext(FHEcontext &context)
        {
            if (helib_test::verbose) {
                std::cout <<"input bitSizes="<<bitSize<<','<<bitSize2
                    <<", output size bound="<<outSize << std::endl;
                if (nthreads>1) std::cout << "  using "<<NTL::AvailableThreads()<<" threads\n";
                std::cout << "computing key-independent tables..." << std::flush;
            }
            buildModChain(context, L, c, /*willBeBootstrappable=*/bootstrap);
            if (bootstrap) {
                context.makeBootstrappable(mvec, /*t=*/0);
            }
            buildUnpackSlotEncoding(unpackSlotEncoding, *context.ea);
            if (helib_test::verbose) {
                std::cout << " done.\n";
                context.zMStar.printout();
            }
            return context;
        };

        void prepareSecKey(FHESecKey &secKey)
        {
            if(helib_test::verbose) {
                std::cout << " L="<<L<<", B="<<B<<std::endl;
                std::cout << "\ncomputing key-dependent tables..." << std::flush;
            }
            secKey.GenSecKey();
            addSome1DMatrices(secKey); // compute key-switching matrices
            addFrbMatrices(secKey);
            if (bootstrap) secKey.genRecryptData();
            if (helib_test::verbose) std::cout << " done\n";
        };

        const long prm;
        const long bitSize;
        const long bitSize2;
        const long outSize;
        const bool bootstrap;
        const long seed;
        const long nthreads;

        const long *vals;
        const long p;
        const long m;
        const NTL::Vec<long> mvec;
        const std::vector<long> gens;
        const std::vector<long> ords;
        const long B;
        const long c;
        const long L;
        FHEcontext context;
        FHESecKey secKey;

        GTest_binaryArith () :
            prm(validatePrm(std::get<0>(GetParam()).prm)),
            bitSize(correctBitSize(5, std::get<0>(GetParam()).bitSize)),
            bitSize2(correctBitSize(bitSize, std::get<0>(GetParam()).bitSize2)),
            outSize(std::get<0>(GetParam()).outSize),
            bootstrap(std::get<0>(GetParam()).bootstrap),
            seed(std::get<0>(GetParam()).seed),
            nthreads(std::get<0>(GetParam()).nthreads),
            vals(mValues[prm]),
            p(vals[0]),
            m(vals[2]),
            mvec(calculateMvec(vals)),
            gens(calculateGens(vals)),
            ords(calculateOrds(vals)),
            B(vals[13]),
            c(vals[14]),
            L(calculateLevels(bootstrap, outSize, bitSize)),
            context(m, p, /*r=*/1, gens, ords),
            secKey(prepareContext(context))
    {};

        virtual void SetUp ()
        {
            if (seed) NTL::SetSeed(NTL::ZZ(seed));
            if (nthreads>1) NTL::SetNumThreads(nthreads);

            prepareSecKey(secKey);

            activeContext = &context; // make things a little easier sometimes
#ifdef DEBUG_PRINTOUT
            dbgEa = (EncryptedArray*) context.ea;
            dbgKey = &secKey;
#endif
        }

        virtual void TearDown() override
        {
#ifdef DEBUG_PRINTOUT
            cleanupGlobals();
#endif
        }

        // This gets called once all of the tests have run
    public:
        static void TearDownTestCase ()
        {
            if (helib_test::verbose)
                printAllTimers(std::cout);
        };

};
constexpr long GTest_binaryArith::mValues[8][15];
std::vector<zzX> GTest_binaryArith::unpackSlotEncoding;

TEST_P(GTest_binaryArith, fifteenForFour)
{
    std::vector<Ctxt> inBuf(15, Ctxt(secKey));
    std::vector<Ctxt*> inPtrs(15, nullptr);

    std::vector<Ctxt> outBuf(5, Ctxt(secKey));

    long sum=0;
    std::string inputBits = "(";
    for (int i=0; i<15; i++) {
        if (NTL::RandomBnd(10)>0) { // leave empty with small probability
            inPtrs[i] = &(inBuf[i]);
            long bit = NTL::RandomBnd(2);  // a random bit
            secKey.Encrypt(inBuf[i], NTL::ZZX(bit));
            inputBits += std::to_string(bit) + ",";
            sum += bit;
        }
        else inputBits += "-,";
    }
    inputBits += ")";

    // Add these bits
    if (helib_test::verbose) {
        std::cout << std::endl;
        CheckCtxt(inBuf[lsize(inBuf)-1], "b4 15for4");
    }
    long numOutputs
        = fifteenOrLess4Four(CtPtrs_vectorCt(outBuf), CtPtrs_vectorPt(inPtrs));
    if (helib_test::verbose)
        CheckCtxt(outBuf[lsize(outBuf)-1], "after 15for4");

    // Check the result
    long sum2=0;
    for (int i=0; i<numOutputs; i++) {
        NTL::ZZX poly;
        secKey.Decrypt(poly, outBuf[i]);
        sum2 += to_long(ConstTerm(poly)) << i;
    }
    EXPECT_EQ(sum, sum2) << "inputs = "<< inputBits << std::endl;
    if (helib_test::verbose) {
        std::cout << "15to4 succeeded, sum"<<inputBits<<"="<<sum2<<std::endl;
    }
};

TEST_P(GTest_binaryArith, product)
{
    const EncryptedArray& ea = *context.ea;
    long mask = (outSize? ((1L<<outSize)-1) : -1);

    // Choose two random n-bit integers
    long pa = NTL::RandomBits_long(bitSize);
    long pb = NTL::RandomBits_long(bitSize2);

    // Encrypt the individual bits
    NTL::Vec<Ctxt> eProduct, enca, encb;

    resize(enca, bitSize, Ctxt(secKey));
    for (long i=0; i<bitSize; i++) {
        secKey.Encrypt(enca[i], NTL::ZZX((pa>>i)&1));
        if (bootstrap) { // put them at a lower level
            enca[i].bringToSet(context.getCtxtPrimes(5));
        }
    }
    resize(encb, bitSize2, Ctxt(secKey));
    for (long i=0; i<bitSize2; i++) {
        secKey.Encrypt(encb[i], NTL::ZZX((pb>>i)&1));
        if (bootstrap) { // put them at a lower level
            encb[i].bringToSet(context.getCtxtPrimes(5));
        }
    }
    if (helib_test::verbose) {
        std::cout << "\n  bits-size "<<bitSize<<'+'<<bitSize2;
        if (outSize>0) std::cout << "->"<<outSize;
        CheckCtxt(encb[0], "b4 multiplication");
    }
    // Test positive multiplication
    std::vector<long> slots;
    {CtPtrs_VecCt eep(eProduct);  // A wrappers around the output vector
        multTwoNumbers(eep,CtPtrs_VecCt(enca),CtPtrs_VecCt(encb),/*negative=*/false,
                outSize, &unpackSlotEncoding);
        decryptBinaryNums(slots, eep, secKey, ea);
    } // get rid of the wrapper
    if (helib_test::verbose)
        CheckCtxt(eProduct[lsize(eProduct)-1], "after multiplication");
    long pProd = pa*pb;
    EXPECT_EQ(slots[0], ((pa*pb)&mask)) <<
        "Positive product error: pa="<<pa<<", pb="<<pb
        << ", but product="<<slots[0]
        << " (should be "<<pProd<<'&'<<mask<<'='<<(pProd&mask)<<")\n";

    if (helib_test::verbose) {
        std::cout << "positive product succeeded: ";
        if (outSize) std::cout << "bottom "<<outSize<<" bits of ";
        std::cout << pa<<"*"<<pb<<"="<<slots[0]<<std::endl;
    }
    // Test negative multiplication
    secKey.Encrypt(encb[bitSize2-1], NTL::ZZX(1));
    decryptBinaryNums(slots, CtPtrs_VecCt(encb), secKey, ea, /*negative=*/true);
    pb = slots[0];
    eProduct.kill();
    {CtPtrs_VecCt eep(eProduct);  // A wrappers around the output vector
        multTwoNumbers(eep,CtPtrs_VecCt(enca),CtPtrs_VecCt(encb),/*negative=*/true,
                outSize, &unpackSlotEncoding);
        decryptBinaryNums(slots, eep, secKey, ea, /*negative=*/true);
    } // get rid of the wrapper
    if (helib_test::verbose)
        CheckCtxt(eProduct[lsize(eProduct)-1], "after multiplication");
    pProd = pa*pb;
    EXPECT_EQ((slots[0]&mask), (pProd&mask)) <<
        "Negative product error: pa="<<pa<<", pb="<<pb
        << ", but product="<<slots[0]
        << " (should be "<<pProd<<'&'<<mask<<'='<<(pProd&mask)<<")\n";
    if (helib_test::verbose) {
        std::cout << "negative product succeeded: ";
        if (outSize) std::cout << "bottom "<<outSize<<" bits of ";
        std::cout << pa<<"*"<<pb<<"="<<slots[0]<<std::endl;
    }

#ifdef DEBUG_PRINTOUT
    const Ctxt* minCtxt = nullptr;
    long minLvl=1000;
    for (const Ctxt& c: eProduct) {
        long lvl = c.findBaseLevel();
        if (lvl < minLvl) {
            minCtxt = &c;
            minLvl = lvl;
        }
    }
    decryptAndPrint((std::cout<<" after multiplication: "), *minCtxt, secKey, ea,0);
    std::cout << std::endl;
#endif
};

TEST_P(GTest_binaryArith, add)
{
    const EncryptedArray& ea = *context.ea;
    long mask = (outSize? ((1L<<outSize)-1) : -1);

    // Choose two random n-bit integers
    long pa = NTL::RandomBits_long(bitSize);
    long pb = NTL::RandomBits_long(bitSize2);

    // Encrypt the individual bits
    NTL::Vec<Ctxt> eSum, enca, encb;

    resize(enca, bitSize, Ctxt(secKey));
    for (long i=0; i<bitSize; i++) {
        secKey.Encrypt(enca[i], NTL::ZZX((pa>>i)&1));
        if (bootstrap) { // put them at a lower level
            enca[i].bringToSet(context.getCtxtPrimes(5));
        }
    }
    resize(encb, bitSize2, Ctxt(secKey));
    for (long i=0; i<bitSize2; i++) {
        secKey.Encrypt(encb[i], NTL::ZZX((pb>>i)&1));
        if (bootstrap) { // put them at a lower level
            encb[i].bringToSet(context.getCtxtPrimes(5));
        }
    }
    if (helib_test::verbose) {
        std::cout << "\n  bits-size "<<bitSize<<'+'<<bitSize2;
        if (outSize>0) std::cout << "->"<<outSize;
        std::cout <<std::endl;
        CheckCtxt(encb[0], "b4 addition");
    }

    // Test addition
    std::vector<long> slots;
    {CtPtrs_VecCt eep(eSum);  // A wrapper around the output vector
        addTwoNumbers(eep, CtPtrs_VecCt(enca), CtPtrs_VecCt(encb),
                outSize, &unpackSlotEncoding);
        decryptBinaryNums(slots, eep, secKey, ea);
    } // get rid of the wrapper
    if (helib_test::verbose) CheckCtxt(eSum[lsize(eSum)-1], "after addition");
    long pSum = pa+pb;
    EXPECT_EQ(slots[0], ((pa+pb)&mask)) <<
        "addTwoNums error: pa="<<pa<<", pb="<<pb
        << ", but pSum="<<slots[0]
        << " (should be ="<<(pSum&mask)<<")\n";
    if (helib_test::verbose) {
        std::cout << "addTwoNums succeeded: ";
        if (outSize) std::cout << "bottom "<<outSize<<" bits of ";
        std::cout << pa<<"+"<<pb<<"="<<slots[0]<<std::endl;
    }

#ifdef DEBUG_PRINTOUT
    const Ctxt* minCtxt = nullptr;
    long minLvl=1000;
    for (const Ctxt& c: eSum) {
        long lvl = c.findBaseLevel();
        if (lvl < minLvl) {
            minCtxt = &c;
            minLvl = lvl;
        }
    }
    decryptAndPrint((std::cout<<" after addition: "), *minCtxt, secKey, ea,0);
    std::cout << std::endl;
#endif
};

INSTANTIATE_TEST_SUITE_P(small_parameter_sizes_repeated, GTest_binaryArith, ::testing::Combine(
            ::testing::Values(
                //SLOW
                Parameters(1, 5, 0, 0, false, 0, 1)
                //FAST
                //Parameters(0, 5, 0, 0, false, 0, 1)), ::testing::Range(0,2)

                ), ::testing::Range(0,2)
            ) // The range is for repeats
        );

} // anonymous namespace