// Copyright (C) 2017  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "opaque_types.h"
#include <dlib/python.h>
#include <dlib/global_optimization.h>
#include <dlib/matrix.h>

using namespace dlib;
using namespace std;
namespace py = pybind11;

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

std::vector<bool> list_to_bool_vector(
    const py::list& l
)
{
    std::vector<bool> result(len(l));
    for (long i = 0; i < result.size(); ++i)
    {
        result[i] = l[i].cast<bool>();
    }
    return result;
}

matrix<double,0,1> list_to_mat(
    const py::list& l
)
{
    matrix<double,0,1> result(len(l));
    for (long i = 0; i < result.size(); ++i)
        result(i) = l[i].cast<double>();
    return result;
}

py::list mat_to_list (
    const matrix<double,0,1>& m
)
{
    py::list l;
    for (long i = 0; i < m.size(); ++i)
        l.append(m(i));
    return l;
}

size_t num_function_arguments(py::object f)
{
    if (hasattr(f,"func_code"))
        return f.attr("func_code").attr("co_argcount").cast<std::size_t>();
    else
        return f.attr("__code__").attr("co_argcount").cast<std::size_t>();
}

double call_func(py::object f, const matrix<double,0,1>& args)
{
    const auto num = num_function_arguments(f);
    DLIB_CASSERT(num == args.size(),
        "The function being optimized takes a number of arguments that doesn't agree with the size of the bounds lists you provided to find_max_global()");
    DLIB_CASSERT(0 < num && num < 15, "Functions being optimized must take between 1 and 15 scalar arguments.");

#define CALL_WITH_N_ARGS(N) case N: return dlib::gopt_impl::_cwv(f,args,typename make_compile_time_integer_range<N>::type()).cast<double>();
    switch (num)
    {
        CALL_WITH_N_ARGS(1)
        CALL_WITH_N_ARGS(2)
        CALL_WITH_N_ARGS(3)
        CALL_WITH_N_ARGS(4)
        CALL_WITH_N_ARGS(5)
        CALL_WITH_N_ARGS(6)
        CALL_WITH_N_ARGS(7)
        CALL_WITH_N_ARGS(8)
        CALL_WITH_N_ARGS(9)
        CALL_WITH_N_ARGS(10)
        CALL_WITH_N_ARGS(11)
        CALL_WITH_N_ARGS(12)
        CALL_WITH_N_ARGS(13)
        CALL_WITH_N_ARGS(14)
        CALL_WITH_N_ARGS(15)

        default:
            DLIB_CASSERT(false, "oops");
            break;
    }
}

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

py::tuple py_find_max_global (
    py::object f,
    py::list bound1,
    py::list bound2,
    py::list is_integer_variable,
    unsigned long num_function_calls,
    double solver_epsilon = 0
)
{
    DLIB_CASSERT(len(bound1) == len(bound2));
    DLIB_CASSERT(len(bound1) == len(is_integer_variable));

    auto func = [&](const matrix<double,0,1>& x)
    {
        return call_func(f, x);
    };

    auto result = find_max_global(func, list_to_mat(bound1), list_to_mat(bound2),
        list_to_bool_vector(is_integer_variable), max_function_calls(num_function_calls),
        solver_epsilon);

    return py::make_tuple(mat_to_list(result.x),result.y);
}

py::tuple py_find_max_global2 (
    py::object f,
    py::list bound1,
    py::list bound2,
    unsigned long num_function_calls,
    double solver_epsilon = 0
)
{
    DLIB_CASSERT(len(bound1) == len(bound2));

    auto func = [&](const matrix<double,0,1>& x)
    {
        return call_func(f, x);
    };

    auto result = find_max_global(func, list_to_mat(bound1), list_to_mat(bound2), max_function_calls(num_function_calls), solver_epsilon);

    return py::make_tuple(mat_to_list(result.x),result.y);
}

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

py::tuple py_find_min_global (
    py::object f,
    py::list bound1,
    py::list bound2,
    py::list is_integer_variable,
    unsigned long num_function_calls,
    double solver_epsilon = 0
)
{
    DLIB_CASSERT(len(bound1) == len(bound2));
    DLIB_CASSERT(len(bound1) == len(is_integer_variable));

    auto func = [&](const matrix<double,0,1>& x)
    {
        return call_func(f, x);
    };

    auto result = find_min_global(func, list_to_mat(bound1), list_to_mat(bound2),
        list_to_bool_vector(is_integer_variable), max_function_calls(num_function_calls),
        solver_epsilon);

    return py::make_tuple(mat_to_list(result.x),result.y);
}

py::tuple py_find_min_global2 (
    py::object f,
    py::list bound1,
    py::list bound2,
    unsigned long num_function_calls,
    double solver_epsilon = 0
)
{
    DLIB_CASSERT(len(bound1) == len(bound2));

    auto func = [&](const matrix<double,0,1>& x)
    {
        return call_func(f, x);
    };

    auto result = find_min_global(func, list_to_mat(bound1), list_to_mat(bound2), max_function_calls(num_function_calls), solver_epsilon);

    return py::make_tuple(mat_to_list(result.x),result.y);
}

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

void bind_global_optimization(py::module& m)
{
    /*!
        requires
            - len(bound1) == len(bound2) == len(is_integer_variable)
            - for all valid i: bound1[i] != bound2[i]
            - solver_epsilon >= 0
            - f() is a real valued multi-variate function.  It must take scalar real
              numbers as its arguments and the number of arguments must be len(bound1).
        ensures
            - This function performs global optimization on the given f() function.
              The goal is to maximize the following objective function:
                 f(x)
              subject to the constraints:
                min(bound1[i],bound2[i]) <= x[i] <= max(bound1[i],bound2[i])
                if (is_integer_variable[i]) then x[i] is an integer.
            - find_max_global() runs until it has called f() num_function_calls times.
              Then it returns the best x it has found along with the corresponding output
              of f().  That is, it returns (best_x_seen,f(best_x_seen)).  Here best_x_seen
              is a list containing the best arguments to f() this function has found.
            - find_max_global() uses a global optimization method based on a combination of
              non-parametric global function modeling and quadratic trust region modeling
              to efficiently find a global maximizer.  It usually does a good job with a
              relatively small number of calls to f().  For more information on how it
              works read the documentation for dlib's global_function_search object.
              However, one notable element is the solver epsilon, which you can adjust.

              The search procedure will only attempt to find a global maximizer to at most
              solver_epsilon accuracy.  Once a local maximizer is found to that accuracy
              the search will focus entirely on finding other maxima elsewhere rather than
              on further improving the current local optima found so far.  That is, once a
              local maxima is identified to about solver_epsilon accuracy, the algorithm
              will spend all its time exploring the function to find other local maxima to
              investigate.  An epsilon of 0 means it will keep solving until it reaches
              full floating point precision.  Larger values will cause it to switch to pure
              global exploration sooner and therefore might be more effective if your
              objective function has many local maxima and you don't care about a super
              high precision solution.
            - Any variables that satisfy the following conditions are optimized on a log-scale:
                - The lower bound on the variable is > 0
                - The ratio of the upper bound to lower bound is > 1000
                - The variable is not an integer variable
              We do this because it's common to optimize machine learning models that have
              parameters with bounds in a range such as [1e-5 to 1e10] (e.g. the SVM C
              parameter) and it's much more appropriate to optimize these kinds of
              variables on a log scale.  So we transform them by applying log() to
              them and then undo the transform via exp() before invoking the function
              being optimized.  Therefore, this transformation is invisible to the user
              supplied functions.  In most cases, it improves the efficiency of the
              optimizer.
    !*/
    {
    m.def("find_max_global", &py_find_max_global,
"requires \n\
    - len(bound1) == len(bound2) == len(is_integer_variable) \n\
    - for all valid i: bound1[i] != bound2[i] \n\
    - solver_epsilon >= 0 \n\
    - f() is a real valued multi-variate function.  It must take scalar real \n\
      numbers as its arguments and the number of arguments must be len(bound1). \n\
ensures \n\
    - This function performs global optimization on the given f() function. \n\
      The goal is to maximize the following objective function: \n\
         f(x) \n\
      subject to the constraints: \n\
        min(bound1[i],bound2[i]) <= x[i] <= max(bound1[i],bound2[i]) \n\
        if (is_integer_variable[i]) then x[i] is an integer. \n\
    - find_max_global() runs until it has called f() num_function_calls times. \n\
      Then it returns the best x it has found along with the corresponding output \n\
      of f().  That is, it returns (best_x_seen,f(best_x_seen)).  Here best_x_seen \n\
      is a list containing the best arguments to f() this function has found. \n\
    - find_max_global() uses a global optimization method based on a combination of \n\
      non-parametric global function modeling and quadratic trust region modeling \n\
      to efficiently find a global maximizer.  It usually does a good job with a \n\
      relatively small number of calls to f().  For more information on how it \n\
      works read the documentation for dlib's global_function_search object. \n\
      However, one notable element is the solver epsilon, which you can adjust. \n\
 \n\
      The search procedure will only attempt to find a global maximizer to at most \n\
      solver_epsilon accuracy.  Once a local maximizer is found to that accuracy \n\
      the search will focus entirely on finding other maxima elsewhere rather than \n\
      on further improving the current local optima found so far.  That is, once a \n\
      local maxima is identified to about solver_epsilon accuracy, the algorithm \n\
      will spend all its time exploring the function to find other local maxima to \n\
      investigate.  An epsilon of 0 means it will keep solving until it reaches \n\
      full floating point precision.  Larger values will cause it to switch to pure \n\
      global exploration sooner and therefore might be more effective if your \n\
      objective function has many local maxima and you don't care about a super \n\
      high precision solution. \n\
    - Any variables that satisfy the following conditions are optimized on a log-scale: \n\
        - The lower bound on the variable is > 0 \n\
        - The ratio of the upper bound to lower bound is > 1000 \n\
        - The variable is not an integer variable \n\
      We do this because it's common to optimize machine learning models that have \n\
      parameters with bounds in a range such as [1e-5 to 1e10] (e.g. the SVM C \n\
      parameter) and it's much more appropriate to optimize these kinds of \n\
      variables on a log scale.  So we transform them by applying log() to \n\
      them and then undo the transform via exp() before invoking the function \n\
      being optimized.  Therefore, this transformation is invisible to the user \n\
      supplied functions.  In most cases, it improves the efficiency of the \n\
      optimizer."
        ,
	py::arg("f"), py::arg("bound1"), py::arg("bound2"), py::arg("is_integer_variable"), py::arg("num_function_calls"), py::arg("solver_epsilon")=0
    );
    }

    {
    m.def("find_max_global", &py_find_max_global2,
        "This function simply calls the other version of find_max_global() with is_integer_variable set to False for all variables.",
	py::arg("f"), py::arg("bound1"), py::arg("bound2"), py::arg("num_function_calls"), py::arg("solver_epsilon")=0
    );
    }

    {
    m.def("find_min_global", &py_find_min_global,
      "This function is just like find_max_global(), except it performs minimization rather than maximization."
        ,
	py::arg("f"), py::arg("bound1"), py::arg("bound2"), py::arg("is_integer_variable"), py::arg("num_function_calls"), py::arg("solver_epsilon")=0
    );
    }

    {
    m.def("find_min_global", &py_find_min_global2,
        "This function simply calls the other version of find_min_global() with is_integer_variable set to False for all variables.",
	py::arg("f"), py::arg("bound1"), py::arg("bound2"), py::arg("num_function_calls"), py::arg("solver_epsilon")=0
    );
    }

}


/*
 * ----------------- BEGIN LICENSE BLOCK ---------------------------------
 *
 * Copyright (C) 2019-2021 Intel Corporation
 *
 * SPDX-License-Identifier: MIT
 *
 * ----------------- END LICENSE BLOCK -----------------------------------
 */

#include "opendrive/geometry/CenterLine.hpp"
#include <boost/array.hpp>
#include <boost/math/tools/rational.hpp>
#include <cmath>
#include <iostream>
#include <spdlog/spdlog.h>
#include "opendrive/geometry/GeometryGenerator.hpp"
#include "opendrive/geometry/LaneUtils.hpp"

namespace opendrive {
namespace geometry {

double laneHeight(ElevationProfileSet const &elevationSet, double s)
{
  return evalPoly3(elevationSet, s);
}

geometry::DirectedPoint CenterLine::eval(double s, bool applyLateralOffset) const
{
  if (s < 0.)
  {
    // as the curve is only defined betwen the range [0 , length], we return the line evaluated at s = 0
    return eval(0.);
  }

  for (auto it = geometry.rbegin(); it != geometry.rend(); it++)
  {
    if (s >= (*it)->GetStartOffset())
    {
      auto directedPoint = (*it)->PosFromDist(s - (*it)->GetStartOffset());

      // This is parametrized as the landmarks positions do not take into account the lateral offset
      if (applyLateralOffset)
      {
        directedPoint.ApplyLateralOffset(calculateOffset(s));
      }

      directedPoint.location.z = laneHeight(elevation, s);
      return directedPoint;
    }
  }

  auto &lastGeometry = geometry.back();
  auto directed_point = lastGeometry->PosFromDist(s - lastGeometry->GetStartOffset());
  directed_point.location.z = laneHeight(elevation, s);
  return directed_point;
}

std::list<double> CenterLine::samplingPoints() const
{
  std::list<double> points;

  points.push_back(length);

  for (auto it = geometry.rbegin(); it != geometry.rend(); it++)
  {
    if ((*it)->GetType() == ::opendrive::GeometryType::ARC)
    {
      auto arc = static_cast<::opendrive::geometry::GeometryArc *>((*it).get());
      double theta = fabs(arc->GetCurvature()) * (*it)->GetLength();

      if (theta < 1e-2)
      {
        points.insert(points.begin(), (*it)->GetStartOffset() + 0.5 * (*it)->GetLength());
      }
      else
      {
        double c = fabs(arc->GetCurvature());
        double dsmax = 2.0 / c * acos(1.0 - c * Geometry::cMaxSamplingError);
        int segments = static_cast<int>((*it)->GetLength() / dsmax);
        for (auto k = segments - 1; k > 0; --k)
        {
          double r = static_cast<double>(k) / static_cast<double>(segments);
          points.insert(points.begin(), (*it)->GetStartOffset() + r * (*it)->GetLength());
        }
      }
    }
    else
    {
      // only start and end point for now
    }

    points.insert(points.begin(), (*it)->GetStartOffset());
  }

  auto const maxErrorSquared = Geometry::cMaxSamplingError * Geometry::cMaxSamplingError;
  // now refine sampling points according to actual points influenced by lateralOffset and elevation
  auto iter = points.begin();
  while (iter != points.end())
  {
    auto nextIter = iter;
    nextIter++;
    if (nextIter == points.end())
    {
      break;
    }
    auto const startPosition = *iter;
    auto const endPosition = *nextIter;
    if (endPosition - startPosition < Geometry::cMaxSamplingError)
    {
      // sampling below the maxError longitudinal distance makes no sense
      ++iter;
      continue;
    }
    auto const centerPosition = 0.5 * startPosition + 0.5 * endPosition;

    auto const start = eval(startPosition).location;
    auto const end = eval(endPosition).location;
    auto const centerLinearInterpolated = 0.5 * start + 0.5 * end;
    auto const centerActual = eval(centerPosition).location;
    auto errorSquared = (centerActual - centerLinearInterpolated).normSquared();
    if (errorSquared > maxErrorSquared)
    {
      points.insert(nextIter, centerPosition);
    }
    else
    {
      iter++;
    }
  }

  return points;
}

double CenterLine::calculateOffset(double s) const
{
  return evalPoly3(laneOffsetSet, s);
}

bool generateCenterLine(RoadInformation &roadInfo, CenterLine &centerLine)
{
  bool ok = true;

  centerLine.geometry.clear();
  centerLine.length = roadInfo.attributes.length;
  centerLine.laneOffsetSet = roadInfo.lanes.lane_offset;
  centerLine.elevation = roadInfo.road_profiles.elevation_profile;

  // Add geometry information
  for (auto &geometry_attribute : roadInfo.geometry_attributes)
  {
    Point start(
      geometry_attribute->start_position_x, geometry_attribute->start_position_y, geometry_attribute->start_position_z);

    try
    {
      switch (geometry_attribute->type)
      {
        case ::opendrive::GeometryType::ARC:
        {
          auto arc = static_cast<GeometryAttributesArc *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(std::make_unique<geometry::GeometryArc>(
            arc->start_position, arc->length, arc->heading, start, arc->curvature));
          break;
        }
        case ::opendrive::GeometryType::LINE:
        {
          auto line = static_cast<GeometryAttributesLine *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(
            std::make_unique<geometry::GeometryLine>(line->start_position, line->length, line->heading, start));
          break;
        }
        break;
        case ::opendrive::GeometryType::SPIRAL:
        {
          // currently not yet supported
          spdlog::error("generateCenterLine() spirals are currently not supported yet");
          ok = false;
          break;
        }
        break;
        case ::opendrive::GeometryType::POLY3:
        {
          auto poly3 = static_cast<GeometryAttributesPoly3 *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(std::make_unique<geometry::GeometryPoly3>(
            poly3->start_position, poly3->length, poly3->heading, start, poly3->a, poly3->b, poly3->c, poly3->d));
          break;
        }
        break;
        case ::opendrive::GeometryType::PARAMPOLY3:
        {
          auto paramPoly3 = static_cast<GeometryAttributesParamPoly3 *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(
            std::make_unique<geometry::GeometryParamPoly3>(paramPoly3->start_position,
                                                           paramPoly3->length,
                                                           paramPoly3->heading,
                                                           start,
                                                           paramPoly3->aU,
                                                           paramPoly3->bU,
                                                           paramPoly3->cU,
                                                           paramPoly3->dU,
                                                           paramPoly3->aV,
                                                           paramPoly3->bV,
                                                           paramPoly3->cV,
                                                           paramPoly3->dV,
                                                           paramPoly3->p_range == "normalized"));
          break;
        }
        break;
        default:
          break;
      }
    }
    catch (...)
    {
      spdlog::error("generateCenterLine() Invalid geometry definition");
      ok = false;
    }
  }

  return ok;
}
} // namespace geometry
} // namespace opendrive


//Header for this file
#include "modetrack.h"
//C System-Headers
#include <termios.h>  /* POSIX terminal control definitions */
#include <sys/ioctl.h>
#include <fcntl.h>//fopen(),fclose()
#include <unistd.h>//read(), write()
//C++ System headers
#include <vector>//vector
#include <string>//string
#include <fstream>//iss*
#include <chrono>// timing functions
#include <cmath>//sqrt, abs
#include <iostream>//cout
#include <typeinfo>//typeid
#include <algorithm> // transform, find
#include <functional> // plus/minus
#include <utility>//std::make_pair
#include <map>//std::map
//Boost Headers
#include <boost/algorithm/string.hpp>//split() and is_any_of for parsing .csv files
#include <boost/lexical_cast.hpp>//lexical cast (unsurprisingly)
//Miscellaneous Headers
#include <omp.h>//OpenMP pragmas
//Project specific Headers
//

ModeTrack::ModeTrack() {

    //initialize best fit curves
    PopulateBestFitCurves();
}

ModeTrack::~ModeTrack() {}

inline void SubtractBackground(std::vector<double>& data,std::vector<double>& background) {

    data.resize(background.size());
    std::transform(data.begin(), data.end(), background.begin(), data.begin(),std::minus<double>());
}

std::vector< data_triple<double> > SubtractBackground(const std::vector< data_triple<double> >& data,
        const std::vector<double>& background) {

    std::vector< data_triple<double> > new_signal;
    new_signal.reserve( background.size() );

    for( uint i = 0; i < background.size() ; i++ ) {

        double length = data[i].cavity_length;
        double frequency = data[i].frequency_MHz;
        double power = data[i].power_dBm - background[i];

        new_signal.push_back( data_triple< double >( length, frequency, power ) );
    }

    return new_signal;
}

double ModeTrack::GetPeaks( const std::vector<data_triple<double> >& data_str, int mode_number, Method filter_method ) {

    std::vector< data_triple< double > > clean_signal = SubtractBackground( data_str, background );

    std::vector<double> power_list = data_triples_to_power( clean_signal );

    auto peak_list = FindPeaks( power_list, filter_method) ;
    std::cout << "Number of peaks identified: " << peak_list.size() << std::endl;

    //check to see if any mode were identified
    //if not return the default value of 'zero'

    if( peak_list.size() >= 1 ) {

        auto identified_peaks = CompareAndFill(peak_list, clean_signal);

        //check to see if requested mode number has been identified
        //if it has return the frequency at which it was found
        //if not return zero, which will be interpreted as 'not found'
        if ( identified_peaks.count(mode_number) == 1 ) {
            return identified_peaks[mode_number];
        } else {
            throw mode_track_failure( "Requested mode was not identified." );
        }
    } else {
        throw mode_track_failure( "No modes were identified." );
    }
}

double ModeTrack::GetPeaksGauss( const std::vector<data_triple<double> >& data_str, int mode_number ) {

    try {
        return GetPeaks( data_str, mode_number, Gauss );
    } catch (const mode_track_failure& e) {
        throw mode_track_failure( "Could not identify requested mode in data set" );
    }
//    return GetPeaks( data_str, mode_number, Gauss );
}

double ModeTrack::GetPeaksBiLat( const std::vector<data_triple<double> >& data_str, int mode_number) {

    try {
        return GetPeaks( data_str, mode_number, BiLat );
    } catch (const mode_track_failure& e) {
        throw mode_track_failure( "Could not identify requested mode in data set" );
    }

//    return GetPeaks( data_str, mode_number, BiLat );
}

void ModeTrack::SetLowerBound( double frequency ) {
    upper_bound = frequency;
}

void ModeTrack::SetUpperBound( double frequency ) {
    lower_bound = frequency;
}

double ModeTrack::GetMaxPeak( const std::vector< data_triple<double> >& data_list ) {

    std::vector<double> power_list = data_triples_to_power( data_list );

    uint peak_index = 0;
    FindMaxima( power_list );

    try {
        peak_index = FindMaxima( power_list );
    } catch (const mode_track_failure& e) {
        throw mode_track_failure( "No maxima peak found in data set." );
    }

    data_triple<double> data_entry = data_list.at( peak_index );
    return data_entry.frequency_MHz;
}

void ModeTrack::SetBackground( const std::vector<data_triple< double > >& background_list ) {

    background = data_triples_to_power( background_list );

}

std::vector<double> ModeTrack::Derivative( const std::vector<double>& data_list ) {

    std::vector<double> der_list;

    std::deque<double> data_deque (data_list.begin(),data_list.end());
    //Pad data_list with zeros at front and back
    for ( unsigned int i=0; i < 2; i++ ) {
        data_deque.push_front(0.0);
        data_deque.push_back(0.0);
    }

    #pragma omp parallel for ordered schedule(dynamic)
    for( unsigned int n = 2; n < data_list.size() + 2; n++ ) {

        double der = ( -data_deque.at( n + 2 ) + 8.0*data_deque.at( n + 1 ) - 8*data_deque.at( n - 1 )+data_deque.at( n - 2 ) ) / 12.0;

        #pragma omp ordered
        der_list.push_back(der);
    }
    return(der_list);
}

//sum all enteries in a vector, with optional parameter of raising each entry to a power
//used by Standard Deviation function
inline double sum ( const std::vector< double >& data_list, double exponent ) {

    double tot = 0.0;

    for ( const auto& val : data_list ) {
        tot += std::pow( val, exponent );
    }

    return tot;
}

//define a gaussian function with standard deviation sigma and a mean value of zero
//used in the construction of gaussian kernels
inline double gaussian ( double x, double sigma ) {
    return 1.0 / ( std::sqrt(M_PI_2)*sigma )*std::exp( -0.5 * std::pow( x/sigma, 2.0 ) );
}

//generate a gaussian kernel of radius 'r', suitable for convolutions
//kernel will have a standard deviation of r/2.
std::vector<double> ModeTrack::GaussKernel( int r ) {

    double sigma = static_cast<double>( r )/2.0;

    std::vector<double> vals;

    for( int i = -r; i<= r ; i ++ ) {
        vals.push_back( gaussian( i, sigma ) );
    }

    //normalize kernel before returning
    return Normalize( vals );
}

double ModeTrack::StdDev( const std::vector<double>& data_list ) {

    //compute mean value of data set
    double sum_x = sum( data_list, 1.0 );
    double n = static_cast<double>( data_list.size() );
    double mean = sum_x/n;

    //compute variance taking into account Bessel's correction eg n/(n-1)
    double sum_x2 = sum( data_list, 2.0 );
    double sigma_sqr = sum_x2/( n - 1.0 ) - n/( n - 1.0 )*std::pow( mean, 2.0 );

    //return square root of variance
    return std::sqrt( sigma_sqr );
}

std::vector<double> ModeTrack::Normalize( const std::vector<double>& data_list ) {

    double norm_factor = std::sqrt( sum( data_list, 2.0 ) );
    auto normalized_data = data_list;

    std::transform( normalized_data.begin(), \
                    normalized_data.end(), \
                    normalized_data.begin(), \
                    std::bind1st(std::divides<float>(), norm_factor) );

    return normalized_data;
}

std::vector< double > ModeTrack::Convolve( const std::vector<double>& signal, const std::vector<double>& kernel ) {

    int kernel_size = kernel.size();
    int half_k_size = (kernel_size - 1 )/2;

    int signal_size = signal.size();
    int signal_max_index = signal_size - 1;

    std::vector< double > output( signal.size(), 0.0 );

    #pragma omp parallel for
    for ( int i = 0 ; i < signal_size ; i++ ) {

        double conv_elem = 0.0;

        int k_max = ( ( i + half_k_size ) > signal_max_index )?( signal_max_index + half_k_size - i ):(kernel_size);
        int k_min = ( ( i - half_k_size ) < 0 )?( half_k_size - i ):(0);

        for ( int j = k_min ; j < k_max ; j++ ) {

            conv_elem += signal[ i + j - half_k_size]*kernel[ j ];
        }

        for ( int j = 0 ; j < k_min ; j++ ) {

            conv_elem += signal[ -i - j + half_k_size ]*kernel[ j ];
        }

        for ( int j = k_max ; j < kernel_size ; j++ ) {

            conv_elem += signal[ 2*signal_max_index - i - j + half_k_size ]*kernel[ j ];

        }

        output[i] = conv_elem;

    }

    return output;
}

//Convolve the input list 'data_list' with a gaussian kernel of radius 15
//serves as a low-pass filter that surpressed noise.
std::vector<double> ModeTrack::GaussBlur( const std::vector<double>& data_list ) {

    auto gauss_matrix = GaussKernel( 15 );
    return Convolve( data_list, gauss_matrix );
}

struct RetrieveVal {
    template <typename T>
    typename T::first_type operator()(T keyValuePair) const {
        return keyValuePair.second;
    }
};

//copy values stored in std::map to std::vec
//std::transform(map.begin(), map.end(), std::back_inserter(vec), RetrieveVal());

std::vector<double> ModeTrack::FindPeaks( const std::vector<double>& data_list, const ModeTrack::Method method ) {

    auto filtered_list = ( method == Gauss )? GaussBlur( data_list ):BilateralFilter( data_list, 10, 2 );
    auto f_prime = Derivative( filtered_list );

    int f_prime_size = f_prime.size() - 1;
    int max_size = filtered_list.size();

    double sigma = StdDev( f_prime );

    std::vector< double > peak_list;

    #pragma omp parallel for ordered schedule(dynamic)
    for( int i = 0; i < f_prime_size; i++ ) {

        bool stat_signifigant = ( f_prime.at(i) < 3*sigma && f_prime.at(i+1) > 3*sigma );
        bool away_from_edges = ( i+1 < max_size - 4 && i+1 > 4 );

        if( stat_signifigant && away_from_edges ) {

            #pragma omp ordered
            peak_list.push_back( static_cast<double>( i + 1 ) );

        }
    }

    return peak_list;
}

uint ModeTrack::FindMaxima( const std::vector<double>& data_list ) {

    auto g_blur = GaussBlur( data_list );
    auto f_prime = Derivative( g_blur );

    uint length = data_list.size();

    double sigma = StdDev( f_prime );

    auto result = std::min_element( data_list.begin(), data_list.end() );
    double min_power = static_cast<double>(*result);

    uint peak_index = 0;
    double peak_power = min_power;

    bool peak_identified = false;

    for( unsigned int i = 0; i < f_prime.size() - 1; i++ ) {

        //Establish criteria for a maxima peak;

        bool maxima = ( f_prime.at(i) > 0*sigma && f_prime.at(i+1) < 0*sigma );
        bool away_from_edges = ( i+1 < (length - 5) && i+1 > 5 );

        if( maxima && away_from_edges ) {

            peak_identified |= true;

            std::cout << "Found peak at: " << i <<std::endl;

            uint found_index = i;
            double found_power = data_list.at(i);

            std::cout << "Power was: " << found_power <<std::endl;

            //check to see if peak index is already an element in the map
            //if duplicate is found check which frequency seperation is the smallest
            //between the current and stored value.
            if( peak_index != 0 ) {
                std::cout << "Duplicate Index Detected" << std::endl;

                //if stored value is greater than current value, replaced stored value with current
                if( found_power > peak_power ) {
                    std::cout << "Found Peak was Greater." << std::endl;
//                        std::cout<<"Power was: "<<found_power<<std::endl;
                    peak_index = found_index;
                    peak_power = found_power;
                    //if stored value is less than current leave stored value unchanged
                } else {
                    std::cout << "Found Peak was Less." << std::endl;
//                        std::cout<<"Power was: "<<found_power<<std::endl;
                }
            } else { //if no duplicate is found create a new entry
                peak_index = found_index;
                peak_power = found_power;
            }

        }
    }

    if( !peak_identified ) {
        throw mode_track_failure( "No local maxima found." );
    }

    return peak_index;
}

inline double QuadraticSpline( double a, double b, double c, double x ) {
    return a*std::pow( x, 2.0 ) + b*x + c;
}

double ModeTrack::GenerateSpline( int mode_number, double length ) {

    double a = std::get<0>(estimated_paths.at(mode_number));
    double b = std::get<1>(estimated_paths.at(mode_number));
    double c = std::get<2>(estimated_paths.at(mode_number));

    return QuadraticSpline(a,b,c,length);
}

void ModeTrack::PopulateBestFitCurves() {

    auto mode_one_coeffs = std::make_tuple(47.9998,-1041.54,8950.56);
    auto mode_two_coeffs = std::make_tuple(44.2758,-1055.35,9610.61);
    auto mode_three_coeffs = std::make_tuple(45.8298,-1139.8,10626.7);
    auto mode_four_coeffs = std::make_tuple(37.697,-1038.49,10780.2);

    estimated_paths.push_back(mode_one_coeffs);
    estimated_paths.push_back(mode_two_coeffs);
    estimated_paths.push_back(mode_three_coeffs);
    estimated_paths.push_back(mode_four_coeffs);
}

template<typename T>
inline void removeDuplicates(std::vector<T>& vec) {
    std::sort(vec.begin(), vec.end());
    vec.erase(std::unique(vec.begin(), vec.end()), vec.end());
}

bool ModeTrack::CheckBounds(double frequency) {
    if( upper_bound == 0.0f || lower_bound == 0.0f ) {
        //if upper or lower bound is not set always return true
        return true;
    } else if (frequency >= upper_bound) {
        return false;
    } else if (frequency <= lower_bound) {
        return false;
    } else {
        return true;
    }
}

template<typename T>
inline void c_print(T text,int color) {

    switch( color ) {
    case 0:
        //red, error text
        std::cout<<"\033[1;31m"<< text << "\033[0m\n";
        break;
    case 2:
        //green, positive confirmation
        std::cout<<"\033[32m"<< text << "\033[0m\n";
        break;
    case 4:
        //magenta, header
        std::cout<<"\033[35m"<< text << "\033[0m\n";
        break;
    case 8:
        //yellow, warning text
        std::cout<<"\033[33m"<< text << "\033[0m\n";
    default:
        break;
    }
}

//peak_list: list of indices (in frequency space) where peaks were found for a particular cavity length
//comparison_list: of data triples at the same cavity length
std::map<uint,double> ModeTrack::CompareAndFill( const std::vector<double>& peak_list,\
        const std::vector< data_triple< double > >& comparison_list ) {

    //format is <peak index,<delta_mu,frequency>>
    //where delta_mu is defined below
    std::map< uint,std::pair< uint,double> > found_peaks;

    //since the cavity length is identical for each element in the set of data triples
    //use the length reported by the first element.
    double g_length = comparison_list.at(0).cavity_length;

    for( const auto& peak_index : peak_list ) {

        std::vector<double> results;

        data_triple<double> data_entry = comparison_list.at(peak_index);

        double frequency = data_entry.frequency_MHz;
        double length = data_entry.cavity_length;

        for (int i = 0; i < 4 ; i++) {
            double estimated_frequency = GenerateSpline(i,length);

            std::cout << "Actual frequency "
                      << frequency
                      << " estimated frequency of peak "
                      << i
                      << " "
                      << estimated_frequency
                      << std::endl;

            double delta_mu = std::abs(frequency - estimated_frequency);
            results.push_back(delta_mu);
        }

        auto minima = std::min_element(std::begin(results), std::end(results));
        auto min_val = *minima;
        auto min_position = std::distance(std::begin(results), minima);

        std::cout<<"Smallest frequency seperation: "<<min_val;
        std::cout<<" at Peak: "<<min_position<<std::endl;

        if(min_val >= max_search_radius) {
            c_print("Identified peak was too far from any estimated value.",0);
            continue;
        }

        std::make_pair(min_val,frequency);

        //check to see if peak index is already an element in the map
        //if duplicate is found check which frequency seperation is the smallest
        //between the current and stored value.
        if( found_peaks.count(min_position) != 0 ) {
//            std::cout<<"Duplicate Index Detected"<<std::endl;

            //if stored value is greater than current value, replaced stored value with current
            if( found_peaks[min_position].first > min_val ) {
//                std::cout<<"Found Peak was Greater."<<std::endl;
                found_peaks[min_position] = std::make_pair(min_val,frequency);
                //if stored value is less than current leave stored value unchanged
            } else {
//                std::cout<<"Found Peak was Less."<<std::endl;
            }
        } else { //if no duplicate is found create a new entry
            found_peaks[min_position] = std::make_pair(min_val,frequency);
        }

    }

    //initalize map to default "error" value of all zeros
    //if this initial value is returned it will caused GetPeaks
    //to return a value of 0.0f which is interpreted as the
    //standard error value.
    if(found_peaks.empty()) {
        found_peaks[0]=std::make_pair(0,0.0);
    }

    //create container to hold identified peaks
    //format is [peak index],frequency
    std::map<uint,double> identified_peaks;

    //iterate from the first peak index to the last peak index, and all values in between
    for(uint i = found_peaks.begin()->first ; i <= found_peaks.rbegin()->first ; i ++) {

        //check if the peak index corresponds to a key in the map of found peaks
        //if peak index is present set frequency for peak using actual data
        //if peak index is not present assume that the peak was 'missed' and
        //fill in data using estimated peak position.
        if( found_peaks.count(i) != 0 ) {

            std::cout<<"Match for peak: "<<i<<", ";
            double peak_frequency = found_peaks[i].second;
//            std::cout<<"Using real value of: "<<peak_frequency<<std::endl;
            std::string message_str = "Using real value of: "+boost::lexical_cast<std::string>(peak_frequency)+"MHz";
            c_print(message_str,2);
            identified_peaks[i] = peak_frequency;

        } else {
            std::cout<<"No match for peak: "<<i<<", ";
            double peak_frequency = GenerateSpline(i,g_length);
//            std::cout<<"Filling with estimate of: "<<peak_frequency<<std::endl;
            std::string message_str = "Filling with estimate of: "+boost::lexical_cast<std::string>(peak_frequency)+"MHz";
            c_print(message_str,8);
            identified_peaks[i] = peak_frequency;
        }
    }

    return identified_peaks;

}

std::vector<double> ModeTrack::BilateralFilter( const std::vector<double>& data_list, double sigma_s, double sigma_r ) {

    int list_size = data_list.size();

    int radius = static_cast<int>( ceil( 5.0/2.0*sigma_s ) );

    //Copy data_list into deque to speed up front/back insertions that will be needed
    //for zero padding
    std::deque<double> data_deque ( data_list.begin(),data_list.end() );
    //Pad data_list with zeros at front and back
    for (int i=1; i<=radius; i++) {
        data_deque.push_front(0.0);
        data_deque.push_back(0.0);
    }

    std::vector<double> convolved_list;

    #pragma omp parallel for ordered schedule(dynamic)
    for( int p = radius; p < list_size + radius; p++ ) {

        double conv_element = 0.0;
        double norm_weight = 0.0;

        for(int q = p -radius; q <= p + radius ; q++) {

            double pos_weight = std::abs( p - q );
            double intens_weight = data_deque.at(p) - data_deque.at(q);
            double val_sigma_s = gaussian( pos_weight, sigma_s );
            double val_sigma_r = gaussian( intens_weight, sigma_r );
            norm_weight += val_sigma_s*val_sigma_r;
            conv_element += val_sigma_s*val_sigma_r*data_deque.at(q);

        }

        #pragma omp ordered
        convolved_list.push_back(conv_element/norm_weight);
    }

    return convolved_list;
}

std::vector< double > ModeTrack::data_triples_to_power( const std::vector<data_triple<double> >& data_list ) {

    std::vector< double > power_list;
    power_list.reserve( data_list.size() );

    for( uint i = 0; i < data_list.size() ; i++ ) {
        power_list.push_back( data_list[i].power_dBm );
    }

    return power_list;

}


/*
	This file contains the primitive type definition, macros and includes shared by other project files
*/
#ifndef polyvec_api_h_
#define polyvec_api_h_

// libc
#define _USE_MATH_DEFINES
#include <math.h>

// C++ STL
#include <stdint.h>
#include <string>
#include <memory>
#include <vector>
#include <unordered_map>
#include <unordered_set>
#include <limits>

#ifndef FLT_MAX
#define FLT_MAX std::numeric_limits<float>::max()
#endif

// Eigen
#include <Eigen/Core>

// Platform specific macros
#ifdef _MSC_VER

#define break_here throw std::exception()
#define polyvec_inline __forceinline

#define polyvec_win32 1
#define polyvec_posix 0

#else
#define break_here do{assert(0); throw std::runtime_error("error");}while(0)
#define polyvec_inline __attribute__((always_inline)) inline

#define polyvec_win32 0
#define polyvec_posix 1

#endif

#ifndef break_here
#define throw std::exception()
#endif

// Generic macros
#ifndef param_unused
#define param_unused(x) (void)(x)
#endif

#define result_unused(x) (void)(x)

#ifndef assert_break
#define assert_break(x) { if (!(x)) { fprintf(stderr, "\nFAILED ASSERT " #x  "\n      LOCATION %s:%d", __FILE__, __LINE__); break_here; } }
#define assert_break_msg(x, msg) { if (!(x)) { fprintf(stderr, "\nFAILED ASSERT %s"  "\n      LOCATION %s:%d", msg,__FILE__, __LINE__); break_here; } }
#endif

#define array_len(x) (sizeof(x)/sizeof((x)[0]))
#ifndef logic_xor // https://benpfaff.org/writings/clc/logical-xor.html
#define logic_xor(a, b) ((!(a)) != (!(b)))
#endif

#define polyvec_str(__X)   static_cast<std::ostringstream&>(std::ostringstream().flush() << __X).str()
#define polyvec_c_str(__X) polyvec_str(__X).c_str()

//
// Type aliases
namespace polyvec {

    using index = std::int64_t;
    using indexu = std::size_t;

    using str = std::string;
    using hash64 = uint64_t; // let's not confuse this with the other bunch of integers

    template <typename T>
    using ptr = std::unique_ptr<T>;

    template <typename K, typename V>
    using umap = std::unordered_map<K, V>;

    template <typename V>
    using uset = std::unordered_set<V>;

// Matrix aliases
    using byte4 = Eigen::Matrix<uint8_t, 4, 1>;
    using real = double;
    using int2 = Eigen::Vector2i;
    using int3 = Eigen::Vector3i;
    using int4 = Eigen::Vector4i;
    using real2 = Eigen::Vector2d;
    using real4 = Eigen::Vector4d;
    using mat2 = Eigen::Matrix2d;
    using mat3 = Eigen::Matrix3d;
    using mat4 = Eigen::Matrix4d;
    using real2xN = Eigen::Matrix2Xd;
    using real1xN = Eigen::VectorXd;

// Matrix Constants
    extern const index index_null;
    extern const hash64 hash64_null;
    extern const index index_max;
    extern const int   int_max;
    extern const uint32_t uint32_max;
    extern const int32_t int32_max;
    extern const int64_t int64_max;
    extern const double real_max;
    extern const real2 real2_0;
    extern const real2 real2_1;
    extern const real2 real2_lo; // lowest
    extern const real2 real2_hi; // max
#define            real2_xy(xy) (real2((xy), (xy))) // avoid evaluating twice?

    using real3 = Eigen::Vector3d;
    extern const real3 real3_0;
    extern const real3 real3_1;
    extern const real3 real3_lo; // lowest
    extern const real3 real3_hi; // max

//
// Constants
    namespace constants {
        static constexpr double PI          = M_PI;
        static constexpr double PI_inv      = M_1_PI;
        static constexpr double PI_half     = M_PI_2;
        static constexpr double PI_half_inv = 1. / PI_half;
        static constexpr double PI2         = PI * 2;
        static constexpr double PI3_4       = 3 * M_PI_4;
        static constexpr double PI_4        = M_PI_4;
        static constexpr double PIDiv180    = 0.017453292519943295769236907684886;

        static constexpr double SmallEps = 1e-4;
        static constexpr double BigEps   = 1e-07;
        static constexpr double Eps      = 1e-10;
    }

//
// Indexing
// The code works with stricly typed indices that do not allow implicit conversions to other types.
// Warnings should be set to level4/Wall and stop compilation.
//
// Add any overload when required, *keep* in mind that to mantain
// strict typing:
// 1. IF conversion operator to `index`        NO non-explicit constructor from `index`
// 2. IF non-explicit constructor from `index` NO  conversion operator to `index`
// (1) is better as allows having to overload all integer operations
//
// DO NOT COMPARE VALIDITY with -1 as it's a perfectly valid index
//
#define define_typed_index(t, i)\
    struct t{\
        const static t null;\
        i _v;\
        explicit t(i v = index_null) : _v(v) { }\
        operator std::int64_t()const { return _v; }\
        explicit operator bool()const { return _v != null; }\
        t& operator=(i v) { _v = v; return *this; }\
        t& operator=(int v) { _v = (i)v; return *this; }\
        bool operator <(i rhs) const { return (i)_v < rhs;}\
        t& operator++() { ++_v; return *this; }\
        t& operator--() { --_v; return *this; }\
    };\
    inline t operator-(t lhs, t rhs) { return (t)(lhs._v - rhs._v); }\
    inline t operator+(t lhs, t rhs) { return (t)(lhs._v + rhs._v); }\
    inline t operator-(t lhs, int rhs) { return (t)(lhs._v - rhs); }\
    inline t operator+(t lhs, int rhs) { return (t)(lhs._v + rhs); }\
    inline t operator%(t lhs, t rhs) { return (t)(lhs._v % rhs._v); }\
    static_assert(sizeof(i) == sizeof(t), "Replace compiler with less edgy cousin");

#define define_typed_index_hash(t)\
    struct _std_hash_index_##t {\
        hash64 operator() (t##_idx i) const {\
            return (hash64)i._v;\
        }\
    };\
    struct _std_hash_index_pair_##t {\
        hash64 operator() (const std::pair<t##_idx, t##_idx>& p) const {\
            return hash_index_pair(p.first._v, p.second._v);\
        }\
    };

    hash64 hash_index_pair ( index lhs, index rhs );
    hash64 hash_index_tuple ( index e0, index e1, index e2 );

// Since there are many different buffers around and they also may be circular or not
// each index has different types to avoid mistakes (naming helps too). It can be used
// as a normal index, but won't be implicitly converted to other types
    define_typed_index ( vertex_idx, index );
    define_typed_index ( corner_idx, index );
    define_typed_index ( node_idx, index );
    define_typed_index ( edge_idx, index );

    define_typed_index_hash ( vertex );
    define_typed_index_hash ( corner );
    define_typed_index_hash ( node );
    define_typed_index_hash ( edge );

    struct _std_hash_node {
        hash64 operator() ( const std::tuple<corner_idx, corner_idx, corner_idx>& t ) const {
            return hash_index_tuple ( std::get<0> ( t )._v, std::get<1> ( t )._v, std::get<2> ( t )._v );
        }
    };

}

// Prevent astyle from indenting namespace; while not touching already written files
#define NAMESPACE_BEGIN(NAME__) namespace NAME__ {
#define NAMESPACE_END(NAME__)                    }

#endif // polyvec_api_h_


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Copyright 2016 Colin Heinzmann                                            *
 *                                                                           *
 * 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>
using namespace std;
#include <unistd.h>

#include <QList>

#include <Eigen/Dense>

#include <opencv2/opencv.hpp>
using namespace cv;

#include <openbr/plugins/openbr_internal.h>
#include <openbr/core/common.h>
#include <openbr/core/eigenutils.h>
#include <openbr/core/opencvutils.h>

#include <cuda_runtime.h>
#include <cublas_v2.h>
#include <cusolverDn.h>
#include "cudadefines.hpp"

namespace br { namespace cuda { namespace pca {
  void castFloatToDouble(float* a, int inca, double* b, int incb, int numElems);
  void castDoubleToFloat(double* a, int inca, float* b, int incb, int numElems);
}}}

namespace br
{
/*!
 * \ingroup transforms
 * \brief Projects input into learned Principal Component Analysis subspace using CUDA. Modified from original PCA plugin.
 * \author Colin Heinzmann \cite DepthDeluxe
 *
 * \br_property float keep Options are: [keep < 0 - All eigenvalues are retained, keep == 0 - No PCA is performed and the eigenvectors form an identity matrix, 0 < keep < 1 - Keep is the fraction of the variance to retain, keep >= 1 - keep is the number of leading eigenvectors to retain] Default is 0.95.
 * \br_property int drop The number of leading eigen-dimensions to drop.
 * \br_property bool whiten Whether or not to perform PCA whitening (i.e., normalize variance of each dimension to unit norm)
 */
class CUDAPCATransform : public Transform
{
    Q_OBJECT

protected:
    Q_PROPERTY(float keep READ get_keep WRITE set_keep RESET reset_keep STORED false)
    Q_PROPERTY(int drop READ get_drop WRITE set_drop RESET reset_drop STORED false)
    Q_PROPERTY(bool whiten READ get_whiten WRITE set_whiten RESET reset_whiten STORED false)

    BR_PROPERTY(float, keep, 0.95)
    BR_PROPERTY(int, drop, 0)
    BR_PROPERTY(bool, whiten, false)

    Eigen::VectorXf mean;
    Eigen::VectorXf eVals;
    Eigen::MatrixXf eVecs;

    cublasHandle_t cublasHandle;
    float* cudaMeanPtr;     // holds the "keep" long vector
    float* cudaEvPtr;       // holds all the eigenvectors

public:
    CUDAPCATransform() : keep(0.95), drop(0), whiten(false) {
      // try to initialize CUBLAS
      cublasStatus_t status;
      status = cublasCreate(&cublasHandle);
      CUBLAS_ERROR_CHECK(status);
    }

    ~CUDAPCATransform() {
      // tear down CUBLAS
      cublasDestroy(cublasHandle);
    }

private:
    double residualReconstructionError(const Template &src) const
    {
        Template proj;
        project(src, proj);

        Eigen::Map<const Eigen::VectorXf> srcMap(src.m().ptr<float>(), src.m().rows*src.m().cols);
        Eigen::Map<Eigen::VectorXf> projMap(proj.m().ptr<float>(), keep);

        return (srcMap - mean).squaredNorm() - projMap.squaredNorm();
    }

    void train(const TemplateList &cudaTrainingSet)
    {
      cublasStatus_t cublasStatus;
      cudaError_t cudaError;

      // put all the data into a single matrix to perform PCA
      const int instances = cudaTrainingSet.size();
      const int dimsIn = *(int*)cudaTrainingSet.first().m().ptr<void*>()[1]
                               * *(int*)cudaTrainingSet.first().m().ptr<void*>()[2];

      // copy the data over
      double* cudaDataPtr;
      CUDA_SAFE_MALLOC(&cudaDataPtr, instances*dimsIn*sizeof(cudaDataPtr[0]), &cudaError);
      for (int i=0; i < instances; i++) {
        br::cuda::pca::castFloatToDouble(
          (float*)(cudaTrainingSet[i].m().ptr<void*>()[0]),
          1,
          cudaDataPtr+i*dimsIn,
          1,
          dimsIn
        );
      }

      trainCore(cudaDataPtr, dimsIn, instances);

      CUDA_SAFE_FREE(cudaDataPtr, &cudaError);
    }

    void project(const Template &src, Template &dst) const
    {
      cudaError_t cudaError;

      void* const* srcDataPtr = src.m().ptr<void*>();
      float* srcGpuMatPtr = (float*)srcDataPtr[0];
      int rows = *((int*)srcDataPtr[1]);
      int cols = *((int*)srcDataPtr[2]);
      int type = *((int*)srcDataPtr[3]);

      if (type != CV_32FC1) {
        cout << "ERR: Invalid image type" << endl;
        throw 0;
      }

      // save the destination rows
      int dstRows = (int)keep;

      Mat dstMat = Mat(src.m().rows, src.m().cols, src.m().type());
      void** dstDataPtr = dstMat.ptr<void*>();
      float** dstGpuMatPtrPtr = (float**)dstDataPtr;
      dstDataPtr[1] = srcDataPtr[1];  *((int*)dstDataPtr[1]) = 1;
      dstDataPtr[2] = srcDataPtr[2];  *((int*)dstDataPtr[2]) = dstRows;
      dstDataPtr[3] = srcDataPtr[3];

      // allocate the memory and set to zero
      //cout << "Allocating destination memory" << endl;
      cublasStatus_t status;
      cudaMalloc(dstGpuMatPtrPtr, dstRows*sizeof(float));
      cudaMemset(*dstGpuMatPtrPtr, 0, dstRows*sizeof(float));

      {
        float negativeOne = -1.0f;
        status = cublasSaxpy(
          cublasHandle,       // handle
          dstRows,            // vector length
          &negativeOne,       // alpha (1)
          cudaMeanPtr,        // mean
          1,                  // stride
          srcGpuMatPtr,       // y, the source
          1                   // stride
        );
        CUBLAS_ERROR_CHECK(status);
      }

      {
        float one = 1.0f;
        float zero = 0.0f;
        status = cublasSgemv(
          cublasHandle,       // handle
          CUBLAS_OP_T,        // normal vector multiplication
          eVecs.rows(),       // # rows
          eVecs.cols(),       // # cols
          &one,               // alpha (1)
          cudaEvPtr,          // pointer to the matrix
          eVecs.rows(),       // leading dimension of matrix
          srcGpuMatPtr,       // vector for multiplication
          1,                  // stride (1)
          &zero,              // beta (0)
          *dstGpuMatPtrPtr,   // vector to store the result
          1                   // stride (1)
        );
        CUBLAS_ERROR_CHECK(status);
      }

      //cout << "Saving result" << endl;
      dst = dstMat;
      CUDA_SAFE_FREE(srcGpuMatPtr, &cudaError);
    }

    void store(QDataStream &stream) const
    {
        stream << keep << drop << whiten <<  mean << eVecs;
    }

    void load(QDataStream &stream)
    {
        stream >> keep >> drop >> whiten >> mean >> eVecs;

        //cout << "Starting load process" << endl;

        cudaError_t cudaError;
        cublasStatus_t cublasStatus;
        CUDA_SAFE_MALLOC(&cudaMeanPtr, mean.rows()*mean.cols()*sizeof(float), &cudaError);
        CUDA_SAFE_MALLOC(&cudaEvPtr, eVecs.rows()*eVecs.cols()*sizeof(float), &cudaError);

        //cout << "Setting vector" << endl;
        // load the mean vector into GPU memory
        cublasStatus = cublasSetVector(
          mean.rows()*mean.cols(),
          sizeof(float),
          mean.data(),
          1,
          cudaMeanPtr,
          1
        );
        CUBLAS_ERROR_CHECK(cublasStatus);

        //cout << "Setting the matrix" << endl;
        // load the eigenvector matrix into GPU memory
        cublasStatus = cublasSetMatrix(
          eVecs.rows(),
          eVecs.cols(),
          sizeof(float),
          eVecs.data(),
          eVecs.rows(),
          cudaEvPtr,
          eVecs.rows()
        );
        CUBLAS_ERROR_CHECK(cublasStatus);
    }

protected:
    void trainCore(double* cudaDataPtr, int dimsIn, int instances) {
      cudaError_t cudaError;

      const bool dominantEigenEstimation = (dimsIn > instances);

      Eigen::MatrixXd allEVals, allEVecs;

      // allocate the eigenvectors
      if (dominantEigenEstimation) {
        allEVals = Eigen::MatrixXd(instances, 1);
        allEVecs = Eigen::MatrixXd(dimsIn, instances);
      } else {
        allEVals = Eigen::MatrixXd(dimsIn, 1);
        allEVecs = Eigen::MatrixXd(dimsIn, dimsIn);
      }

      if (keep != 0) {
        performCovarianceSVD(cudaDataPtr, dimsIn, instances, allEVals, allEVecs);
      } else {
        // null case
        mean = Eigen::VectorXf::Zero(dimsIn);
        allEVecs = Eigen::MatrixXd::Identity(dimsIn, dimsIn);
        allEVals = Eigen::VectorXd::Ones(dimsIn);
      }

      // *****************
      // We have now found the eigenvalues and eigenvectors
      // *****************

      if (keep <= 0) {
          keep = dimsIn - drop;
      } else if (keep < 1) {
          // Keep eigenvectors that retain a certain energy percentage.
          const double totalEnergy = allEVals.sum();
          if (totalEnergy == 0) {
              keep = 0;
          } else {
              double currentEnergy = 0;
              int i=0;
              while ((currentEnergy / totalEnergy < keep) && (i < allEVals.rows())) {
                  currentEnergy += allEVals(i);
                  i++;
              }
              keep = i - drop;
          }
      } else {
          if (keep + drop > allEVals.rows()) {
              qWarning("Insufficient samples, needed at least %d but only got %d.", (int)keep + drop, (int)allEVals.rows());
              keep = allEVals.rows() - drop;
          }
      }

      // Keep highest energy vectors
      eVals = Eigen::VectorXf((int)keep, 1);
      eVecs = Eigen::MatrixXf(allEVecs.rows(), (int)keep);
      for (int i=0; i<keep; i++) {
          int index = i+drop;
          eVals(i) = allEVals(index);
          eVecs.col(i) = allEVecs.col(index).cast<float>();
          if (whiten) eVecs.col(i) /= sqrt(eVals(i));
      }

      // Debug output
      if (Globals->verbose) qDebug() << "PCA Training:\n\tDimsIn =" << dimsIn << "\n\tKeep =" << keep;
    }

    // computes the covariance matrix and then pulls the eigenvalues+eigenvectors
    // out of it using SVD of a symmetric matrix
    void performCovarianceSVD(double* cudaDataPtr, int dimsIn, int instances, Eigen::MatrixXd& allEVals, Eigen::MatrixXd& allEVecs) {
      cudaError_t cudaError;

      const bool dominantEigenEstimation = (dimsIn > instances);

      // used for temporary storage
      Eigen::VectorXd meanDouble(dimsIn);

      // compute the mean
      for (int i=0; i < dimsIn; i++) {
        cublasDasum(
          cublasHandle,
          instances,
          cudaDataPtr+i,
          dimsIn,
          meanDouble.data()+i
        );
      }

      // put data back on GPU for further processing
      double* cudaMeanDoublePtr;
      CUDA_SAFE_MALLOC(&cudaMeanDoublePtr, dimsIn*sizeof(cudaMeanDoublePtr[0]), &cudaError);
      cublasSetVector(
        dimsIn,
        sizeof(cudaMeanDoublePtr[0]),
        meanDouble.data(),
        1,
        cudaMeanDoublePtr,
        1
      );

      // scale to calculate average
      {
        double scaleFactor = 1.0/(double)instances;
        cublasDscal(
          cublasHandle,
          dimsIn,
          &scaleFactor,
          cudaMeanDoublePtr,
          1
        );
      }

      // subtract mean from data
      for (int i=0; i < instances; i++) {
        double negativeOne = -1.0;
        cublasDaxpy(
          cublasHandle,
          dimsIn,
          &negativeOne,
          cudaMeanDoublePtr,
          1,
          cudaDataPtr+i*dimsIn,
          1
        );
      }

      // convert to float form and copy the data back
      CUDA_SAFE_MALLOC(&cudaMeanPtr, dimsIn*sizeof(cudaMeanPtr[0]), &cudaError);
      br::cuda::pca::castDoubleToFloat(cudaMeanDoublePtr, 1, cudaMeanPtr, 1, dimsIn);

      // copy the data back
      mean = Eigen::VectorXf(dimsIn);
      cublasGetVector(
        dimsIn,
        sizeof(cudaMeanPtr[0]),
        cudaMeanPtr,
        1,
        mean.data(),
        1
      );

      // free up the memory
      CUDA_SAFE_FREE(cudaMeanDoublePtr, &cudaError);
      CUDA_SAFE_FREE(cudaMeanPtr, &cudaError);

      // allocate space for the covariance matrix
      double* cudaCovariancePtr;
      int covRows = allEVals.rows();
      CUDA_SAFE_MALLOC(&cudaCovariancePtr, covRows*covRows*sizeof(cudaCovariancePtr[0]), &cudaError);

      // compute the covariance matrix
      if (dominantEigenEstimation) {
        // cov = data.transpose() * data / (instances-1.0);
        const double scaleFactor = 1.0/(instances-1.0);
        const double zero = 0.0;
        cublasDgemm(
          cublasHandle,
          CUBLAS_OP_T,
          CUBLAS_OP_N,
          instances,
          instances,
          dimsIn,
          &scaleFactor,
          cudaDataPtr,
          dimsIn,
          cudaDataPtr,
          dimsIn,
          &zero,
          cudaCovariancePtr,
          covRows
        );
      } else {
        // cov = data * data.transpose() / (instances-1.0);
        const double scaleFactor = 1.0/(instances-1.0);
        const double zero = 0.0;
        cublasDgemm(
          cublasHandle,
          CUBLAS_OP_N,
          CUBLAS_OP_T,
          dimsIn,
          dimsIn,
          instances,
          &scaleFactor,
          cudaDataPtr,
          dimsIn,
          cudaDataPtr,
          dimsIn,
          &zero,
          cudaCovariancePtr,
          covRows
        );
      }

      cusolverDnHandle_t cusolverHandle;
      cusolverStatus_t cusolverStatus;
      cusolverDnCreate(&cusolverHandle);

      // allocate appropriate working space
      int svdLWork;
      cusolverDnDgesvd_bufferSize(
        cusolverHandle,
        covRows,
        covRows,
        &svdLWork
      );
      double* cudaSvdWork;
      CUDA_SAFE_MALLOC(&cudaSvdWork, svdLWork*sizeof(cudaSvdWork[0]), &cudaError);

      double* cudaUPtr;
      CUDA_SAFE_MALLOC(&cudaUPtr, covRows*covRows*sizeof(cudaUPtr[0]), &cudaError);
      double* cudaSPtr;
      CUDA_SAFE_MALLOC(&cudaSPtr, covRows*sizeof(cudaSPtr[0]), &cudaError);
      double* cudaVTPtr;
      CUDA_SAFE_MALLOC(&cudaVTPtr, covRows*covRows*sizeof(cudaVTPtr[0]), &cudaError);

      int* cudaSvdDevInfoPtr;
      CUDA_SAFE_MALLOC(&cudaSvdDevInfoPtr, sizeof(*cudaSvdDevInfoPtr), &cudaError);
      int svdDevInfo;

      // perform SVD on an n x m matrix, in this case the matrix is the covariance
      // matrix and is symmetric, meaning the SVD will calculate the eigenvalues
      // and eigenvectors for us.
      cusolverStatus = cusolverDnDgesvd(
        cusolverHandle,
        'A',                // all columns of unitary matrix
        'A',                // all columns of array VT
        covRows,            // m
        covRows,            // n
        cudaCovariancePtr,  // decomposing the covariance matrix
        covRows,            // lda
        cudaSPtr,           // holds S
        cudaUPtr,           // holds U
        covRows,            // ldu
        cudaVTPtr,          // holds VT
        covRows,            // ldvt
        cudaSvdWork,        // work buffer ptr
        svdLWork,           // length of the work buffer
        NULL,               // rwork, not used for real data types
        cudaSvdDevInfoPtr   // devInfo pointer
      );
      CUSOLVER_ERROR_CHECK(cusolverStatus);

      // get the eigenvalues and free memory
      cublasGetVector(
        covRows,
        sizeof(cudaSPtr[0]),
        cudaSPtr,
        1,
        allEVals.data(),
        1
      );
      CUDA_SAFE_FREE(cudaSvdWork, &cudaError);
      CUDA_SAFE_FREE(cudaSPtr, &cudaError);
      CUDA_SAFE_FREE(cudaVTPtr, &cudaError);
      CUDA_SAFE_FREE(cudaSvdDevInfoPtr, &cudaError);

      // if this is a dominant eigen estimation, then perform matrix multiplication again
      // if (dominantEigenEstimation) allEVecs = data * allEVecs;
      if (dominantEigenEstimation) {
        double* cudaMultedAllEVecs;
        CUDA_SAFE_MALLOC(&cudaMultedAllEVecs, dimsIn*instances*sizeof(cudaMultedAllEVecs[0]), &cudaError);
        const double one = 1.0;
        const double zero = 0;

        cublasDgemm(
          cublasHandle,   // handle
          CUBLAS_OP_N,    // transa
          CUBLAS_OP_N,    // transb
          dimsIn,         // m
          instances,      // n
          instances,      // k
          &one,           // alpha
          cudaDataPtr,    // A
          dimsIn,         // lda
          cudaUPtr,       // B
          instances,      // ldb
          &zero,          // beta
          cudaMultedAllEVecs, // C
          dimsIn          // ldc
        );

        // normalize result then divide the column by the norm
        for (int i=0; i < instances; i++) {
          // compute the norm
          double norm;
          cublasDnrm2(
            cublasHandle,
            dimsIn,
            cudaMultedAllEVecs+i*dimsIn,
            1,
            &norm
          );

          // now divide by it
          norm = 1.0/norm;
          cublasDscal(
            cublasHandle,
            dimsIn,
            &norm,
            cudaMultedAllEVecs+i*dimsIn,
            1
          );
        }

        // get the eigenvectors from the multiplied value
        cublasGetMatrix(
          dimsIn,
          instances,
          sizeof(cudaMultedAllEVecs[0]),
          cudaMultedAllEVecs,
          dimsIn,
          allEVecs.data(),
          dimsIn
        );

        // free the memory used for multiplication
        CUDA_SAFE_FREE(cudaMultedAllEVecs, &cudaError);
      } else {
        // normalize result then divide the column by the norm
        for (int i=0; i < instances; i++) {
          // compute the norm
          double norm;
          cublasDnrm2(
            cublasHandle,
            covRows,
            cudaUPtr+i*covRows,
            1,
            &norm
          );

          // now divide by it
          norm = 1.0/norm;
          cublasDscal(
            cublasHandle,
            covRows,
            &norm,
            cudaUPtr+i*covRows,
            1
          );
        }

        // get the eigenvectors straight from the SVD
        cublasGetMatrix(
          covRows,
          covRows,
          sizeof(cudaUPtr[0]),
          cudaUPtr,
          covRows,
          allEVecs.data(),
          covRows
        );
      }

      // free all the memory
      CUDA_SAFE_FREE(cudaCovariancePtr, &cudaError);
      CUDA_SAFE_FREE(cudaUPtr, &cudaError);
      cusolverDnDestroy(cusolverHandle);
    }
};

BR_REGISTER(Transform, CUDAPCATransform)
} // namespace br

#include "cuda/cudapca.moc"


#include <webots/Emitter.hpp>
#include <webots/Receiver.hpp>

#include <string>
#include <vector>
#include <cstdlib>
#include <iostream>
#include <cmath>
#include<limits>

#include <Eigen/Dense>

#include "tracking.h"
#include "greyWolf.h"

#define _USE_MATH_DEFINES

double calculate_euclideanDistance(double x1, double z1, double x2, double z2){
  return pow(pow(x2 - x1,2)+pow(z2 - z1,2),0.5);
}

GreyWolf::GreyWolf(webots::Emitter *emitter_handle, webots::Receiver *receiver_handle, int timestep, robot_tracking::Position init_pos){
  emitter = emitter_handle;
  receiver = receiver_handle;
  timeStep = timestep;
  receiver->enable(timeStep);
  printDebuggingInfo = false;
  starting_position = init_pos;
  init_information.clear();
  event_message.clear();
  a <<1,1;
  r1<<1,1;
  r2<< 1,1;
  if(printDebuggingInfo){
    std::cout<<"_________ROBOT INDEX___________:"<<robot_index<<"\n";
    std::cout<<starting_position.x<<starting_position.z<<"\n";
  }
}

void GreyWolf::set_swarmSettings(int index, int num_wolves, int num_iterations){
  robot_index = index;
  numberOfWolves = num_wolves;
  num_iter = num_iterations;
}

void GreyWolf::set_boundaries(int x_upper,int x_lower,int z_upper,int z_lower){
  x_upper_boundary = x_upper;
  x_lower_boundary = x_lower;
  z_upper_boundary = z_upper;
  z_lower_boundary = z_lower;
}

void GreyWolf::set_individualSettings(double explore_factor, double multiplication_factor, int social_number){
  k_exploration = explore_factor;
  k_multiplication = multiplication_factor;
  social_learning_number = social_number;
}

void GreyWolf::emit_currentPosition(int current_iter){
  message.event_id = current_iter;
  message.index = robot_index;
  message.robot_position = my_position;
  message.event_message.assign("Position Update");
  emitter->send(&message, sizeof(Event));
  event_message.push_back(message);
}

void GreyWolf::init_StatusUpdate(robot_tracking::Position starting_position){
  message.event_id = 0;
  message.index = robot_index;
  message.robot_position = my_position;
  message.event_message.assign("Initialisation");
  emitter->send(&message, sizeof(Event));
  init_information.push_back(message);
}

void GreyWolf::update_position(robot_tracking::Position position){
  prev_position = my_position;
  my_position = position;
}

void GreyWolf::set_targetPosition(robot_tracking::Position target){
  target_position = target;
}

void GreyWolf::get_allEvents(int current_iter){
    event_message.clear();
    Event *received_event;
    while(receiver->getQueueLength() > 0){
      received_event = (Event *) receiver->getData();
      if(received_event->event_message.compare("Initialisation") == 0)
        init_information.push_back(*received_event);
      else if(received_event->event_message.compare("Position Update") == 0){
        // if(received_event->event_id == current_iter)
          event_message.push_back(*received_event);
      }
      receiver->nextPacket();
    }
    if(printDebuggingInfo){
      std::cout<<"Printing Events\n";
      print_AllEvents();
    }
}

void GreyWolf::print_AllEvents(){
  std::cout<<"_________ROBOT INDEX___________:"<<robot_index<<"\n";
  for(std::vector<Event>::const_iterator i = event_message.begin(); i != event_message.end(); ++i){
    std::cout<<i->event_id<<"\t"<<i->index<<"\t"<<i->event_message<<"\n";
    std::cout<<"Position"<<"\t"<<i->robot_position.x<<"\t"<<i->robot_position.z<<"\t"<<i->robot_position.heading<<"\n";
  }
}

void GreyWolf::clear_initInformation(){
  init_information.clear();
}

void GreyWolf::clear_eventMessage(){
  event_message.clear();
}

double GreyWolf::calculate_EucDist(robot_tracking::Position P1,robot_tracking::Position P2){
  return std::pow(std::pow(P1.x - P2.x,2) + std::pow(P1.z - P2.z,2),0.5);
}

double GreyWolf::generateRandomNumber(double lowerLimit, double upperLimit){
  double rand_num = ((double)rand())/((double)RAND_MAX +1);
  rand_num = lowerLimit + ((upperLimit - lowerLimit)*rand_num);
  return rand_num;
}

void GreyWolf::calculate_Constants(int current_iter){
  double buff = (double)k_exploration*((double)1 - ((double)current_iter/(double)num_iter));
  // std::cout<<"Buff"<<buff;
  a << buff,buff;
  // std::cout<<a<<"\n";
  r1 << generateRandomNumber(0,2),generateRandomNumber(0,2);
  r2 << generateRandomNumber(0,2),generateRandomNumber(0,2);
  if(printDebuggingInfo){
    std::cout<<"current iter"<<current_iter<<" and num iter"<<num_iter<<"\n";
    std::cout<<"_________ROBOT INDEX___________:"<<robot_index<<"\n";
    std::cout<<"R1: "<<r1[0]<<"\t"<<r1[1]<<"\n";
    std::cout<<"R2: "<<r2[0]<<"\t"<<r2[1]<<"\n";
    std::cout<<"A: "<<a[0]<<"\t"<<a[1]<<"\n";
  }
}
// bool GreyWolf::compareEvents(Event e1, Event e2){
  // return (e1.index<e2.index);
// }

// bool comparePairs(const std::pair<GreyWolf::Event,double> &a,
              // const std::pair<GreyWolf::Event,double> &b)
// {
    // return (a.second < b.second);
// }

double GreyWolf::calculate_fitnessOfWolf(int current_iter, robot_tracking::Position wolfStartingPosition, robot_tracking::Position wolfCurrentPosition){

    double k_intraSwarmDistance = 1 - (current_iter/num_iter);
    // double k_intraSwarmDistance = 0;
    double intraSwarmDistance = 0, 	wolf_communicationThreshold = 1;
    for(std::vector<Event>::const_iterator i = event_message.begin(); i!= event_message.end(); ++i){
      // if(i->event_id == current_iter && i->index != robot_index){
      if(i->index != robot_index){
        intraSwarmDistance += wolf_communicationThreshold/calculate_EucDist(wolfCurrentPosition,i->robot_position);
      }
    }
  //Calculate individual coverage
    double coverageIndividual = calculate_EucDist(wolfStartingPosition, wolfCurrentPosition);
    double k_coverageIndividual = 1 - (current_iter/num_iter);
  //Calculate distance from the goal
    double k_distanceFromGoal = 1 + (current_iter/num_iter);
    double distanceFromGoal = calculate_EucDist(target_position,wolfCurrentPosition);

    double fitness = (k_coverageIndividual*coverageIndividual) + (k_intraSwarmDistance*intraSwarmDistance) - (k_distanceFromGoal*distanceFromGoal);
    return fitness;
}

void GreyWolf::calculate_FitnessRanker(int current_iter){
  // fitnessRanker.clear();
  std::sort(init_information.begin(), init_information.end(),[](Event e1, Event e2){
    return (e1.index < e2.index);
  });// Fix this!
  for(std::vector<Event>::const_iterator i = event_message.begin(); i != event_message.end(); ++i){
    //Get the corresponding starting position for that event
    robot_tracking::Position startingPosition = init_information[i->index].robot_position;
    //Made change to the first parameter of the fitness function.
    //Earlier, the current iter was passed as first argument, now the event id is passed.
    double fitness = calculate_fitnessOfWolf(i->event_id, startingPosition, i->robot_position);
    fitnessRanker.push_back(std::make_pair(*i,fitness));
  }
  //Sort the fitnessRanker
  std::sort(fitnessRanker.begin(), fitnessRanker.end(), [](const std::pair<GreyWolf::Event,double> &a, const std::pair<GreyWolf::Event,double> &b){
    return (a.second < b.second);
  });
}

robot_tracking::Position GreyWolf::calculateNextPosition(int current_iter){
  bool isOmega = true;
  Eigen::Vector2d target(target_position.x, target_position.z);
  Eigen::Vector2d currentPosition(my_position.x, my_position.z);
  Eigen::Vector2d nextPosition(0,0);
  robot_tracking::Position next_pos;
  fitnessRanker.clear();
  calculate_FitnessRanker(current_iter);
  calculate_Constants(current_iter);
  Eigen::Vector2d A,C,D,buff;

  for (int i=0;i<social_learning_number;i++){
    if(fitnessRanker[i].first.index == robot_index)
      isOmega = false;
  }
  //Calculate A
  A << 2*a[0]*r1[0] , 2*a[1]*r1[1];
  A -= a;
  //Calculate C
  C = 2*r2;

  if(isOmega){
    //Calculate D
    D << C[0]*target[0] , C[1]*target[1];
    D -= currentPosition;
    nextPosition << A[0]*D[0] ,A[1]*D[1];
    nextPosition  = target - nextPosition;
  }
  else{
    for (int j=0;j<social_learning_number;j++){
      D << C[0]*fitnessRanker[j].first.robot_position.x, C[1]*fitnessRanker[j].first.robot_position.z;
      D -= currentPosition;
      buff <<fitnessRanker[j].first.robot_position.x - A[0]*D[0] , fitnessRanker[j].first.robot_position.z - A[1]*D[1];
      nextPosition+= buff;
    }
    nextPosition *= 1/social_learning_number;
  }

  nextPosition = currentPosition + k_multiplication*(nextPosition - currentPosition);
  next_pos.x = nextPosition[0];
  next_pos.z = nextPosition[1];
  next_pos.heading = M_PI/3;

  // std::cout<<"_________POSITION of ROBOT INDEX___________:"<<robot_index<<"\n";
  // std::cout<<"before boundary snap = ("<<next_pos.x<<","<<next_pos.z<<","<<next_pos.heading*(180/M_PI)<<")\n";

  if(!check_insideBoundary(next_pos)){
    // std::cout<<"Boundary snap called\n";
    next_pos = snapToBoundary(my_position, next_pos);
  }

  // std::cout<<"_________POSITION of ROBOT INDEX___________:"<<robot_index<<"\n";
  // std::cout<<"after boundary snap = ("<<next_pos.x<<","<<next_pos.z<<","<<next_pos.heading*(180/M_PI)<<")\n";

  return next_pos;
}

robot_tracking::Position GreyWolf::snapToBoundary(robot_tracking::Position currentPosition, robot_tracking::Position nextPosition){
  // Set boundary points
  double x1 = currentPosition.x , z1 = currentPosition.z;
  double x2 = nextPosition.x , z2 = nextPosition.z;
  double x3,z3,x4,z4;
  double ta,tb;
  double x_new, z_new;

  // std::cout<<"_________Before snapping Function of ROBOT INDEX___________:"<<robot_index<<"\n";
  // std::cout<<"X1,Z1= ("<<x1<<","<<z1<<")\n";
  // std::cout<<"X2,Z2= ("<<x2<<","<<z2<<")\n";

  if(x1>=x_upper_boundary || x1<=x_lower_boundary)
    x1 = round(x1);
  if(z1>=z_upper_boundary || z1<=z_lower_boundary)
    z1 = round(z1);

  //Setup boundaries
  x3 = x_upper_boundary;
  z3 = z_upper_boundary;
  x4 = x_upper_boundary;
  z4 = z_lower_boundary;
  //Calculate constants
  ta = (((z3 - z4)*(x1 - x3)) + ((x4 - x3)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));
  tb = (((z1 - z2)*(x1 - x3)) + ((x2 - x1)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));

  if(ta>=0 && ta<=1 && tb<=1 && tb>=0){
    x_new = x1 + ta*(x2 - x1);
    z_new = z1 + ta*(z2 - z1);
  }

  else{
    //Setup boundaries
    x3 = x_upper_boundary;
    z3 = z_upper_boundary;
    x4 = x_lower_boundary;
    z4 = z_upper_boundary;
    //Calculate constants
    ta = (((z3 - z4)*(x1 - x3)) + ((x4 - x3)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));
    tb = (((z1 - z2)*(x1 - x3)) + ((x2 - x1)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));

    if(ta>=0 && ta<=1 && tb<=1 && tb>=0){
      x_new = x1 + ta*(x2 - x1);
      z_new = z1 + ta*(z2 - z1);
    }
    else{
      //Setup boundaries
      x3 = x_lower_boundary;
      z3 = z_lower_boundary;
      x4 = x_lower_boundary;
      z4 = z_upper_boundary;
      //Calculate constants
      ta = (((z3 - z4)*(x1 - x3)) + ((x4 - x3)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));
      tb = (((z1 - z2)*(x1 - x3)) + ((x2 - x1)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));

      if(ta>=0 && ta<=1 && tb<=1 && tb>=0){
        x_new = x1 + ta*(x2 - x1);
        z_new = z1 + ta*(z2 - z1);
      }
      else{
        //Setup boundaries
        x3 = x_lower_boundary;
        z3 = z_lower_boundary;
        x4 = x_upper_boundary;
        z4 = z_lower_boundary;
        //Calculate constants
        ta = (((z3 - z4)*(x1 - x3)) + ((x4 - x3)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));
        tb = (((z1 - z2)*(x1 - x3)) + ((x2 - x1)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));

        if(ta>=0 && ta<=1 && tb<=1 && tb>=0){
          x_new = x1 + ta*(x2 - x1);
          z_new = z1 + ta*(z2 - z1);
        }
        else{
          x_new = nextPosition.x;
          z_new = nextPosition.z;
        }
      }
    }
  }

  // std::cout<<"_________Before scaling of ROBOT INDEX___________:"<<robot_index<<"\n";
  // std::cout<<"X_new,Z_new= ("<<x_new<<","<<z_new<<")\n";

  //Scale next Position so that it always remains inside the boundary
  //Done to prevent the robot from always flocking to the edges
  // std::cout<<"_________Difference in positions ROBOT INDEX___________:"<<robot_index<<"\n";
  // std::cout<<"X_diff,Z_diff= ("<<(x_new - currentPosition.x)<<","<<(z_new - currentPosition.z)<<")\n";

  robot_tracking::Position newPosition;
  newPosition.x = x_new;
  newPosition.z = z_new;
  newPosition.heading = nextPosition.heading;

  return newPosition;
}

robot_tracking::Position GreyWolf::generateRandomPosition(){
  robot_tracking::Position newPos;
  newPos.x = generateRandomNumber(x_lower_boundary,x_upper_boundary);
  newPos.z = generateRandomNumber(z_lower_boundary,z_upper_boundary);
  newPos.heading = 0;
  return newPos;
}

bool GreyWolf::check_insideBoundary(robot_tracking::Position nextPosition){
  bool x_insideBoundary = false, z_insideBoundary = false;
  if(nextPosition.x> x_lower_boundary && nextPosition.x< x_upper_boundary)
    x_insideBoundary = true;
  if(nextPosition.z> z_lower_boundary && nextPosition.z< z_upper_boundary)
    z_insideBoundary = true;

  return (x_insideBoundary && z_insideBoundary);
}

robot_tracking::Position GreyWolf::get_previousPosition(){
  return prev_position;
}


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 2011.
// Distributed under the Boost
// Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or
// copy at http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_BLOOM_FILTER_DYNAMIC_COUNTING_BLOOM_FILTER_HPP
#define BOOST_BLOOM_FILTER_DYNAMIC_COUNTING_BLOOM_FILTER_HPP 1

#include <cmath>
#include <vector>

#include <boost/config.hpp>

#include <boost/static_assert.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_unsigned.hpp>

#include <boost/bloom_filter/detail/twohash_counting_apply_hash.hpp>
#include <boost/bloom_filter/detail/extenders.hpp>
#include <boost/bloom_filter/hash/default.hpp>
#include <boost/bloom_filter/hash/murmurhash3.hpp>

namespace boost {
  namespace bloom_filters {
    template <typename T,
	      size_t BitsPerBin = 4,
	      size_t HashValues = 2,
	      size_t ExpectedInsertionCount = 0,
	      class HashFunction1 = boost_hash<T>,
	      class HashFunction2 = murmurhash3<T>,
	      class ExtensionFunction = detail::square,
	      typename Block = size_t,
	      typename Allocator = std::allocator<Block> >
    class twohash_dynamic_counting_bloom_filter {

      // Block needs to be an integral type
      BOOST_STATIC_ASSERT( boost::is_integral<Block>::value == true);

      // Block needs to be an unsigned type
      BOOST_STATIC_ASSERT( boost::is_unsigned<Block>::value == true);

      // BitsPerBin needs to be greater than 0
      BOOST_STATIC_ASSERT( BitsPerBin > 0);

      // it doesn't make sense to ever support using a BitsPerBin value larger
      // than the number of bits per Block. In that case, the user shouldn't
      // be using a Bloom filter to represent their data.
      BOOST_STATIC_ASSERT( (BitsPerBin < (sizeof(Block) * 8) ) );

      // because of the nature of this implementation, the Bloom filter
      // can have internal fragmentation if the calculation for
      // bins_per_slot has a remainder. The severity of the  internal
      // fragmentation is equal to the remainder * the number of slots.
      // This check prevents internal fragmentation.
      // This also necessarily limits to bin sizes to one of:
      // [1,2,4,8,16,32(64-bit system only)] bits
      BOOST_STATIC_ASSERT( ((sizeof(Block) * 8) % BitsPerBin) == 0);

    public:
      typedef T value_type;
      typedef T key_type;
      typedef HashFunction1 hash_function1_type;
      typedef HashFunction2 hash_function2_type;
      typedef ExtensionFunction extension_function_type;
      typedef Block block_type;
      typedef Allocator allocator_type;
      typedef twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
						    HashValues,
						    ExpectedInsertionCount,
						    HashFunction1,
						    HashFunction2,
						    ExtensionFunction,
						    Block, Allocator> this_type;

      typedef std::vector<Block, Allocator> bucket_type;
      typedef typename bucket_type::iterator bucket_iterator;
      typedef typename bucket_type::const_iterator bucket_const_iterator;

      static const size_t default_num_bins = 32;

    private:
      static const size_t slot_bits = sizeof(block_type) * 8;

      size_t bucket_size(const size_t requested_bins) const {
	const size_t bin_bits = requested_bins * BitsPerBin;
	return bin_bits / slot_bits + 1;
      }

      typedef detail::twohash_counting_apply_hash<HashValues,
						  this_type> apply_hash_type;

    public:
      //! constructors
      twohash_dynamic_counting_bloom_filter()
	: bits(bucket_size(default_num_bins)),
	  _num_bins(default_num_bins)
      {
      }

      explicit twohash_dynamic_counting_bloom_filter(const size_t requested_bins)
	: bits(bucket_size(requested_bins)),
	  _num_bins(requested_bins)
      {
      }

      template <typename InputIterator>
      twohash_dynamic_counting_bloom_filter(const InputIterator start,
					    const InputIterator end)
	: bits(bucket_size(std::distance(start, end) * 4)),
	  _num_bins(std::distance(start, end) * 4)
      {
	for (InputIterator i = start; i != end; ++i)
	  this->insert(*i);
      }

      //! meta functions
      size_t num_bins() const
      {
	return this->_num_bins;
      }

      static BOOST_CONSTEXPR size_t expected_insertion_count()
      {
	return ExpectedInsertionCount;
      }

      static BOOST_CONSTEXPR size_t bits_per_bin()
      {
	return BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t bins_per_slot()
      {
	return sizeof(block_type) * 8 / BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t mask()
      {
	return static_cast<Block>(0 - 1) >> (sizeof(Block) * 8 - BitsPerBin);
      }

      size_t bit_capacity() const
      {
        return this->num_bins() * BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t num_hash_functions()
      {
        return HashValues;
      }

      double false_positive_rate() const
      {
        const double n = static_cast<double>(this->count());
        static const double k = static_cast<double>(num_hash_functions());
        static const double m = static_cast<double>(this->num_bins());
        static const double e =
	  2.718281828459045235360287471352662497757247093699959574966;
        return std::pow(1 - std::pow(e, -k * n / m), k);
      }

      //? returns the number of bins that have at least 1 bit set
      size_t count() const
      {
	size_t ret = 0;

	for (bucket_const_iterator i = this->bits.begin(),
	       end = this->bits.end();
	     i != end; ++i) {
	  for (size_t bin = 0; bin < this->bins_per_slot(); ++bin) {
	    const size_t offset_bits = bin * BitsPerBin;
	    const size_t target_bits = (*i >> offset_bits) & this->mask();

	    if (target_bits > 0)
	      ++ret;
	  }
	}

        return ret;
      }

      bool empty() const
      {
	return this->count() == 0;
      }

      const bucket_type&
      data() const
      {
	return this->bits;
      }

      //! core ops
      void insert(const T& t)
      {
	apply_hash_type::insert(t,
				this->bits,
				this->num_bins());
      }

      template <typename InputIterator>
      void insert(const InputIterator start, const InputIterator end)
      {
	for (InputIterator i = start; i != end; ++i) {
	  this->insert(*i);
	}
      }

      void remove(const T& t)
      {
	apply_hash_type::remove(t,
				this->bits,
				this->num_bins());
      }

      template <typename InputIterator>
      void remove(const InputIterator start, const InputIterator end)
      {
	for (InputIterator i = start; i != end; ++i) {
	  this->remove(*i);
	}
      }

      bool probably_contains(const T& t) const
      {
	return apply_hash_type::contains(t,
					 this->bits,
					 this->num_bins());
      }

      //! auxiliary ops
      void clear()
      {
	for (bucket_iterator i = bits.begin(), end = bits.end();
	     i != end; ++i)
	  *i = 0;
      }

      void swap(twohash_dynamic_counting_bloom_filter& other)
      {
	twohash_dynamic_counting_bloom_filter tmp = other;
	other = *this;
	*this = tmp;
      }

      // equality comparison operators
      template <typename _T, size_t _BitsPerBin,
		size_t _HashValues, size_t _ExpectedInsertionCount,
		class _HashFn1, class _HashFn2, class _Extender,
		typename _Block, class _Allocator>
      friend bool
      operator==(const twohash_dynamic_counting_bloom_filter<_T, _BitsPerBin,
							     _HashValues,
							     _ExpectedInsertionCount,
							     _HashFn1,
							     _HashFn2,
							     _Extender,
							     _Block,
							     _Allocator>& lhs,
		 const twohash_dynamic_counting_bloom_filter<_T, _BitsPerBin,
							     _HashValues,
							     _ExpectedInsertionCount,
							     _HashFn1,
							     _HashFn2,
							     _Extender,
							     _Block,
							     _Allocator>& rhs);

      template <typename _T, size_t _BitsPerBin,
		size_t _HashValues, size_t _ExpectedInsertionCount,
		class _HashFn1, class _HashFn2, class _Extender,
		typename _Block, class _Allocator>
      friend bool
      operator!=(const twohash_dynamic_counting_bloom_filter<_T, _BitsPerBin,
							     _HashValues,
							     _ExpectedInsertionCount,
							     _HashFn1,
							     _HashFn2,
							     _Extender,
							     _Block,
							     _Allocator>& lhs,
		 const twohash_dynamic_counting_bloom_filter<_T, _BitsPerBin,
							     _HashValues,
							     _ExpectedInsertionCount,
							     _HashFn1,
							     _HashFn2,
							     _Extender,
							     _Block,
							     _Allocator>& rhs);

    private:
      bucket_type bits;
      size_t _num_bins;
    };

    template<class T, size_t BitsPerBin, size_t HashValues,
	     size_t ExpectedInsertionCount,
	     class HashFunction1, class HashFunction2,
	     class ExtensionFunction, typename Block,
	     class Allocator>
    void
    swap(twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
					       HashValues,
					       ExpectedInsertionCount,
					       HashFunction1,
					       HashFunction2,
					       ExtensionFunction,
					       Block,
					       Allocator>& lhs,
	 twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
					       HashValues,
					       ExpectedInsertionCount,
					       HashFunction1,
					       HashFunction2,
					       ExtensionFunction,
					       Block,
					       Allocator>& rhs)

    {
      lhs.swap(rhs);
    }

    template<class T, size_t BitsPerBin, size_t HashValues,
	     size_t ExpectedInsertionCount,
	     class HashFunction1, class HashFunction2,
	     class ExtensionFunction, typename Block,
	     class Allocator>
    bool
    operator==(const twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
							   HashValues,
							   ExpectedInsertionCount,
							   HashFunction1,
							   HashFunction2,
							   ExtensionFunction,
							   Block,
							   Allocator>& lhs,
	       const twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
							   HashValues,
							   ExpectedInsertionCount,
							   HashFunction1,
							   HashFunction2,
							   ExtensionFunction,
							   Block,
							   Allocator>& rhs)
    {
      if (lhs.bit_capacity() != rhs.bit_capacity())
	throw detail::incompatible_size_exception();

      return (lhs.bits == rhs.bits);
    }

    template<class T, size_t BitsPerBin, size_t HashValues,
	     size_t ExpectedInsertionCount,
	     class HashFunction1, class HashFunction2,
	     class ExtensionFunction, typename Block,
	     class Allocator>
    bool
    operator!=(const twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
							   HashValues,
							   ExpectedInsertionCount,
							   HashFunction1,
							   HashFunction2,
							   ExtensionFunction,
							   Block,
							   Allocator>& lhs,
	       const twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
							   HashValues,
							   ExpectedInsertionCount,
							   HashFunction1,
							   HashFunction2,
							   ExtensionFunction,
							   Block,
							   Allocator>& rhs)
    {
      if (lhs.bit_capacity() != rhs.bit_capacity())
	throw detail::incompatible_size_exception();

      return !(lhs == rhs);
    }

  } // namespace bloom_filter
} // namespace boost
#endif