#pragma once

#include "algorithms/util/PeakDetection.hpp"
#include "algorithms/public/DataSetIdSequence.hpp"
#include "algorithms/util/DistanceFuncs.hpp"
#include "algorithms/public/KDTree.hpp"
#include "algorithms/public/KMeans.hpp"
#include "algorithms/public/MelBands.hpp"
#include "algorithms/util/AlgorithmUtils.hpp"
#include "algorithms/util/FluidEigenMappings.hpp"
#include "algorithms/util/MedianFilter.hpp"
#include "algorithms/util/SpectralEmbedding.hpp"
#include "data/TensorTypes.hpp"
#include "data/FluidDataSet.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <fstream>
#include <random>

namespace fluid {
namespace algorithm {

class GraphPlayUtils {

public:
  using  MatrixXd = Eigen::MatrixXd;
  using  VectorXd = Eigen::VectorXd;
  using DataSet = FluidDataSet<std::string, double, 1>;

  GraphPlayUtils(){
    using namespace std;
    random_device rd;
    mGen = mt19937(rd());
    mDis = uniform_real_distribution<> (0.0, 1.0);
    mFilter.init(5);
  }

  index randInt(index N){
    return static_cast<index>(mDis(mGen) * N);
  }

  double rand(){
    return mDis(mGen);
  }

  Eigen::ArrayXXd computeDM(RealMatrixView mag, index numBands,
    double sampleRate, index windowSize, index fftSize, index dist){
    using namespace Eigen;
    using namespace _impl;
    MelBands melBands = MelBands(numBands, fftSize);
    melBands.init(20, 5000, numBands, mag.cols(), sampleRate, windowSize);
    RealMatrix melSpec = RealMatrix(mag.rows(), numBands);
    for(index i = 0; i < mag.rows(); i++){
      melBands.processFrame(mag.row(i), melSpec.row(i), true, false, false);
    }
    MatrixXd tmp = asEigen<Matrix>(melSpec);
    return DistanceMatrix(tmp, dist);
  }

  void onsetDetection(Eigen::Ref<Eigen::ArrayXd> odf,
                      Eigen::Ref<Eigen::ArrayXXd> transitions, index offset = 2){
    for(index i = 0; i < odf.size(); i++){
      odf(i) = odf(i) - mFilter.processSample(odf(i));
    }
    auto onsets = mPD.process(odf, 0, 0.1, false, false);
    for(index i = 0; i < onsets.size(); i++){
      index pos = onsets[i].first;
      index start = std::max(index(0), pos - offset);
      index end = std::min(transitions.rows() - 1, pos + offset + 1);
      transitions.block(start, 0, end - start, transitions.cols()).setZero();
      transitions.block(0, start, transitions.rows(), end - start).setZero();
    }
  }

  FluidTensor<index, 1> kmeans(RealMatrixView data, index nClusters){
      algorithm::DataSetIdSequence seq("", 0, 0);
      index minClusterSize = 10;
      DataSet tmpDS = DataSet(data.cols());
      for(index i = 0; i < data.rows(); i++){
        tmpDS.add(seq.next(), data.row(i));
      }
      mKMeans.clear();
      mKMeans.train(tmpDS, nClusters, 100);
      FluidTensor<index, 1> clusters(data.rows());
      mKMeans.getAssignments(clusters);
      RealMatrix means(mKMeans.size(), mKMeans.dims());
      mKMeans.getMeans(means);
      for(index i = 0; i < nClusters; i++){
        index cSize = mKMeans.getClusterSize(i);
        //std::cout<<cSize<<std::endl;
        if(cSize > 0 && cSize < minClusterSize){
          RealMatrix distances(1, mKMeans.size());
          RealMatrix mean(1, mKMeans.dims());
          mean.row(0) =  means.col(i);
          mKMeans.getDistances(mean, distances);
          index closest = std::min_element(distances.begin(), distances.end()) - distances.begin();
          for(index j = 0; j < clusters.size(); j++)
            if(clusters(j) == i) clusters(j) = closest;
        }
      }
      //std::ofstream ofs ("clusters.mat", std::ofstream::out);ofs << clusters;ofs.close();
      return clusters;
  }

  void writeMatrix(Eigen::Ref<Eigen::MatrixXd> mat, std::string name){
    //std::ofstream ofs (name+".mat", std::ofstream::out);ofs << mat;ofs.close();
  }

  FluidTensor<index, 1>  spectralClustering(Eigen::Ref<Eigen::ArrayXXd> dm, index numClusters = 0){
    using namespace Eigen;
    index maxClusters = numClusters > 0? numClusters : std::min(index(50), dm.rows());
    index nPoints = dm.rows();
    MatrixXd tmpRp = (dm.array() < 0.25).cast<double>();
    MatrixXd weightedGraph = (1 - dm) * tmpRp.array();
    SpectralEmbedding spectralEmbedding;
    spectralEmbedding.train(weightedGraph.sparseView(), maxClusters);
    if(numClusters == 0 ){
      VectorXd eigenValues  =  spectralEmbedding.eigenValues();
      ArrayXd diff = (
        eigenValues.segment(1, eigenValues.size() - 2) -
        eigenValues.segment(0, eigenValues.size() - 2));
        VectorXd::Index maxIndex;
        double maxVal = diff.maxCoeff(&maxIndex);
        numClusters = std::min(2*(maxIndex + 1), maxClusters);
    }
    MatrixXd eigenVectors = spectralEmbedding.eigenVectors().block(0,0, nPoints, numClusters);
    eigenVectors.rowwise().normalize();
    return kmeans( _impl::asFluid(eigenVectors), numClusters);
  }

private:
  MedianFilter mFilter;
  PeakDetection mPD;
  KMeans mKMeans;
  std::mt19937 mGen;
  std::uniform_real_distribution<> mDis;
};
} // namespace algorithm
} // namespace fluid


// ========================================================================= //
// Filename      :  test_sampling.cpp
// Creation Date :  2016-6-21
// Created by    :  anfranek
// ========================================================================= //

#include <gtest/gtest.h>

#include <cmath>

#include <Eigen/Core>

#include "sampling.h"

TEST(equidistant_sampling, two_point_line)
{
    util::AlignedVecOfVec2d line = {{0, 0}, {4, 0}};

    util::AlignedVecOfVec2d expected = {{0, 0}, {1, 0}, {2, 0}, {3, 0}, {4, 0}};
    auto sampled = util::sample_equidistant(line, 5);

    EXPECT_EQ(expected, sampled);
}

TEST(equidistant_sampling, three_point_line)
{
    util::AlignedVecOfVec2d line = {{0, 0}, {1, 1}, {2, 0}};

    util::AlignedVecOfVec2d expected = {{0, 0}, {2./3., 2./3.}, {4./3., 2./3.}, {2, 0}};
    auto sampled = util::sample_equidistant(line, 4);

    for(int i = 0; i < static_cast<int>(expected.size()); i++)
    {
        EXPECT_DOUBLE_EQ(expected[i][0], sampled[i][0]);
        EXPECT_DOUBLE_EQ(expected[i][1], sampled[i][1]);
    }
}


/**
* @file distance.hpp
* @author Toyas Dhake (Driver), Shivam Akhauri (Navigator)
* @date 11 October 2019
* @copyright 2019 Toyas Dhake, Shivam Akhauri
* @brief This is a class for depth perception module based on image from single
* camera.
*/

#ifndef INCLUDE_DISTANCE_HPP_
#define INCLUDE_DISTANCE_HPP_

#include <dlib/image_processing/frontal_face_detector.h>
#include <vector>
#include <opencv2/opencv.hpp>
#include <face.hpp>

class CalculateDistance {
 private:
    // width of the face in the refernce image
    double knownWidth = 7.0;
    // distance of the face in the refernce image
    double knownDistance = 36.0;
 public:
    // focal length of the camera
    double focalLength;
    // the calculated distance of the detcetd face in the frame
    double realTimeDistance = 0;
    double calculateFocalLength();
    // constructor
    CalculateDistance();
    // function to pass each detected face in the
    // frame to calculateFocalLength function
    std::vector<Face> getDistance(cv::Mat image,
                                        dlib::frontal_face_detector detector);
    // function to calculate distance of the frame which contains formula
    double calDist(double width, double focalLength);
};

#endif  // INCLUDE_DISTANCE_HPP_


//
// $Id$
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// 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.
//

#ifndef GAUSSFILTERTYPES_H
#define GAUSSFILTERTYPES_H

#include <boost/math/distributions/normal.hpp>
#include "pwiz/utility/findmf/base/filter/utilities/gauss.hpp"

namespace ralab{
  namespace base{
    namespace filter{

      /*! \brief generate the gauss filter function for filtering of peaks with fwhm (full width at half max)

                        \post accumulate(gauss) == 1.
                        \return accumulate(gauss) == 1.
                        */
      template <typename TReal>
      TReal getGaussianFilter
      (
          std::vector<TReal> & gauss, //!<[out] Gaussian for filtering
          TReal fwhm = 20 //!<[in] full width at half max in points
          )
      {
        std::vector<TReal> x;
        ralab::base::base::seq<TReal>( -ceil(TReal(2*fwhm)), ceil(TReal(2*fwhm)) , x);
        TReal sigma = fwhm/2.35;
        //generate response
        return utilities::getGaussWorker(sigma, gauss, x);
      }

      /*! \brief generate the gauss filter function for filtering of peaks with fwhm (full width at half max)

                        \post accumulate(gauss) == 1.
                        \return accumulate(gauss) == 1.
                        */
      template <typename TReal>
      TReal getGaussianFilterQuantile
      (
          std::vector<TReal> & gauss, //!<[out] Gaussian for filtering
          TReal fwhm = 20, //!<[in] full width at half max in points
          TReal quantile = 0.01 //!< would mean that the generated distribution covers at least 99.8 of mass
          )
      {
        if( quantile >= 0.5)
          {
            throw std::logic_error("quantile >= 0.5");
          }
        std::vector<TReal> x;

        TReal sigma = fwhm/2.35;
        boost::math::normal_distribution<TReal> nd_(0,sigma);
        TReal quant = floor(boost::math::quantile(nd_,quantile));
        ralab::base::base::seq( quant , -quant , x);
        return utilities::getGaussWorker(sigma, gauss, x);

      }

      /*! \brief generate first derivative Gauss

                        \post accumulate(gauss1d) == 0.
                        \post accumulate(fabs(gauss1d)) == 1.
                        */

      template <typename TReal>
      TReal getGaussian1DerFilter(
          std::vector<TReal> & gauss1d, //!<[out] Gaussian for filtering
          TReal fwhm = 20 //!<[in] full width at half max in points
          )
      {
        std::vector<TReal> x;
        ralab::base::base::seq( - ceil(TReal(2*fwhm)), ceil(TReal(2*fwhm)) , x);
        TReal sigma = fwhm/2.35;
        //generate response
        return utilities::getGaussian1DerWorker(sigma, gauss1d, x);
      }

      template <typename TReal>
      TReal getGaussian1DerFilterQuantile(
          std::vector<TReal> & gauss1d, //!<[out] Gaussian for filtering
          TReal fwhm = 20, //!<[in] full width at half max in points
          TReal quantile = 0.1
          )
      {
        if( quantile >= 0.5)
          {
            throw std::logic_error("quantile >= 0.5");
          }
        std::vector<TReal> x;
        TReal sigma = fwhm/2.35;
        boost::math::normal_distribution<TReal> nd_(0,sigma);
        TReal quant = floor(boost::math::quantile(nd_,quantile));
        ralab::base::base::seq( quant , -quant , x);
        //generate response
        return utilities::getGaussian1DerWorker(sigma, gauss1d, x);
      }
    }//filter
  }//base
}//ralab

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2006 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file lmdif.hpp
    \brief wrapper for MINPACK minimization routine
*/

#ifndef quantlib_optimization_lmdif_hpp
#define quantlib_optimization_lmdif_hpp

#include <ql/qldefines.hpp>
#include <boost/function.hpp>

namespace QuantLib {

    namespace MINPACK {
        typedef boost::function<void (int,
                                      int,
                                      double*,
                                      double*,
                                      int*)> LmdifCostFunction;

        void lmdif(int m,int n,double* x,double* fvec,double ftol,
                   double xtol,double gtol,int maxfev,double epsfcn,
                   double* diag, int mode, double factor,
                   int nprint, int* info,int* nfev,double* fjac,
                   int ldfjac,int* ipvt,double* qtf,
                   double* wa1,double* wa2,double* wa3,double* wa4,
                   const LmdifCostFunction& fcn);

        void qrsolv(int n,double* r,int ldr,int* ipvt,
                    double* diag,double* qtb, double* x,
                    double* sdiag,double* wa);
        void qrfac(int m,int n,double* a,int, int pivot,int* ipvt,
                   int,double* rdiag,double* acnorm,double* wa);
    }
}
#endif


/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

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

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <cstddef>
#include <cmath>
#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/not.hpp>
#include <boost/utility/enable_if.hpp>
#include <sprig/type_traits/is_call_copy_param.hpp>

namespace sprig {
	//
	// integer_digits
	//
	template<typename T>
	SPRIG_INLINE typename boost::enable_if<
		boost::mpl::and_<
			typename boost::is_arithmetic<T>::type,
			typename is_call_copy_param<T>::type
		>,
		std::size_t
	>::type
	integer_digits(T const t) {
		return static_cast<std::size_t>(std::ceil(std::log10(t)));
	}
	template<typename T>
	SPRIG_INLINE typename boost::enable_if<
		boost::mpl::and_<
			typename boost::is_arithmetic<T>::type,
			typename boost::mpl::not_<typename is_call_copy_param<T>::type>::type
		>,
		std::size_t
	>::type
	integer_digits(T const& t) {
		return static_cast<std::size_t>(std::ceil(std::log10(t)));
	}
	//
	// numeric_abs
	//
	template<typename T>
	SPRIG_INLINE typename boost::enable_if<sprig::is_call_copy_param<T>, T>::type
	numeric_abs(T const t) {
		return t < 0 ? -t : t;
	}
	template<typename T>
	SPRIG_INLINE typename boost::disable_if<sprig::is_call_copy_param<T>, T>::type
	numeric_abs(T const& t) {
		return t < 0 ? -t : t;
	}
} // namespace sprig

#endif	// #ifndef SPRIG_MATH_HPP


#include "geometrycentral/numerical/linear_solvers.h"
#include "geometrycentral/surface/manifold_surface_mesh.h"
#include "geometrycentral/surface/meshio.h"
#include "geometrycentral/surface/simple_polygon_mesh.h"
#include "geometrycentral/surface/vertex_position_geometry.h"

#include <emscripten/bind.h>
#include <emscripten/val.h>

#include <Eigen/Dense>
#include <Eigen/SparseCore>

#include "geometrycentral/surface/manifold_surface_mesh.h"
#include "geometrycentral/surface/simple_polygon_mesh.h"
#include "geometrycentral/surface/trace_geodesic.h"
#include "geometrycentral/surface/vertex_position_geometry.h"

#include "Walker.h"

using namespace emscripten;
using namespace geometrycentral;
using namespace geometrycentral::surface;

struct GeoMesh {
  std::unique_ptr<ManifoldSurfaceMesh> mesh;
  std::unique_ptr<VertexPositionGeometry> geo;
};

struct StepResult {
  Vector3 T, N, B, pos;
  Vector2 dir;
  SurfacePoint surfacePos;
  std::vector<Vector3> trajectory;
};

// Stolen from Ricky Reusser https://observablehq.com/d/d0df0c04ce5c94FCC
template <typename T>
void copyToVector(const val &typedArray, std::vector<T> &vec) {
  unsigned int length = typedArray["length"].as<unsigned int>();
  val memory = val::module_property("buffer");
  vec.reserve(length);
  val memoryView = typedArray["constructor"].new_(
      memory, reinterpret_cast<uintptr_t>(vec.data()), length);
  memoryView.call<void>("set", typedArray);
}

SurfacePoint getStartingPoint(GeoMesh &geo) {
  Face start = geo.mesh->face(0);
  return SurfacePoint(start, Vector3{1. / 3., 1. / 3., 1. / 3.});
}

StepResult takeStep(Vector2 direction, SurfacePoint pos, GeoMesh &geo,
                    double stepSize) {
  StepResult result;

  result.trajectory = step(direction, pos, *geo.geo, stepSize);

  result.pos = pos.interpolate(geo.geo->inputVertexPositions);
  result.T = getExtrinsicDirection(direction, pos, *geo.geo).normalize();
  geo.geo->requireFaceNormals();
  result.N = geo.geo->faceNormals[pos.face];
  result.B = cross(result.T, result.N);
  result.dir = direction;
  result.surfacePos = pos;

  return result;
}

// Mostly stolen from Ricky Reusser https://observablehq.com/d/d0df0c04ce5c94fc
EMSCRIPTEN_BINDINGS(my_module) {
  value_array<Vector3>("Vector3")
      .element(&Vector3::x)
      .element(&Vector3::y)
      .element(&Vector3::z);
  value_array<Vector2>("Vector2").element(&Vector2::x).element(&Vector2::y);

  register_vector<Vector3>("VectorVector3");
  register_vector<size_t>("VectorSizeT");
  register_vector<std::vector<size_t>>("VectorVectorSizeT");

  class_<SurfacePoint>("SurfacePoint");

  class_<GeoMesh>("GCMesh")
      .function("polygons", optional_override([](GeoMesh &self) {
                  return self.mesh->getFaceVertexList();
                }))
      .function("vertexCoordinates", optional_override([](const GeoMesh &self) {
                  std::vector<Vector3> vCoords;
                  for (Vertex v : self.mesh->vertices())
                    vCoords.push_back(self.geo->inputVertexPositions[v]);
                  return vCoords;
                }));

  function(
      "readMesh", optional_override([](std::string str, std::string type = "") {
        std::stringstream in;
        in << str;

        GeoMesh gMesh;
        std::tie(gMesh.mesh, gMesh.geo) = readManifoldSurfaceMesh(in, type);
        return gMesh;
      }));

  value_object<StepResult>("StepResult")
      .field("T", &StepResult::T)
      .field("N", &StepResult::N)
      .field("B", &StepResult::B)
      .field("pos", &StepResult::pos)
      .field("dir", &StepResult::dir)
      .field("surfacePos", &StepResult::surfacePos)
      .field("trajectory", &StepResult::trajectory);

  function("getStartingPoint", &getStartingPoint);
  function("takeStep", &takeStep);
}