// ========================================================================== //
// Copyright (c) 2014-2019 The University of Texas at Austin.                 //
// All rights reserved.                                                       //
//                                                                            //
// Licensed under the Apache License, Version 2.0 (the "License");            //
// you may not use this file except in compliance with the License.           //
// A copy of the License is included with this software in the file LICENSE.  //
// If your copy does not contain the License, you may obtain a copy of the    //
// License at:                                                                //
//                                                                            //
//     https://www.apache.org/licenses/LICENSE-2.0                            //
//                                                                            //
// Unless required by applicable law or agreed to in writing, software        //
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT  //
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.           //
// See the License for the specific language governing permissions and        //
// limitations under the License.                                             //
//                                                                            //
// ========================================================================== //

#include <iostream>

#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <cstdlib>
#include <math.h>
#include <dlfcn.h>

#include <dtypes.h>
#include <Application.h>
#include "Renderer.h"
#include "RenderingSet.h"

#include <ospray/ospray.h>

#include <time.h>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

using namespace boost;

int mpiRank, mpiSize;

#include "Debug.h"

using namespace gxy;
using namespace std;

int total_samples = 100;

#define WIDTH   1920
#define HEIGHT 1080

int width  = WIDTH;
int height = HEIGHT;

float radius = 0.02;
float target = 0.0;
float power = 10.0;
float sigma = 1.0;
int   max_consecutive_misses = 10;
int   iteration_limit = 20000;
int   hot_start = 1000;
float scaling = 1.0;
int   skip = 10;

#define TF_NONE     0
#define TF_LINEAR   1
#define TF_GAUSSIAN 2

int tf_type = TF_NONE;
float tf0, tf1;

variate_generator<mt19937, normal_distribution<> > *generator;

void normally_distributed_vector(double* v)
{
  for (size_t i = 0; i < 3; ++i)
    v[i]=(*generator)();
}

float gaussian(float x, float m, float s)
{
  return ( 1 / ( s * sqrt(2*M_PI) ) ) * exp( -0.5 * pow( (x-m)/s, 2.0 ) );
}

#define RNDM ((float)rand() / RAND_MAX)

class SampleMsg : public Work
{
    struct args
    {
        Key   vk;
        Key   pk;
        int   tf_type;
        float tf0;
        float tf1;
        float step;
        int   n_iterations;
        int   n_start;
        int   n_skip;
        int   n_miss;
    };

public:
  SampleMsg(VolumeP v, ParticlesP p, int tf_type, float tf0, float tf1, float step, int niterations, int nstart, int nskip, int nmiss)
            : SampleMsg(sizeof(args))
  {
        args *a = (args *)contents->get();
        a->vk = v->getkey();
        a->pk = p->getkey();
        a->tf_type = tf_type;
        a->tf0 = tf0;
        a->tf1 = tf1;
        a->step = step;
        a->n_iterations = niterations;
        a->n_start = nstart;
        a->n_skip = nskip;
        a->n_miss = nmiss;
    }

  ~SampleMsg() {}

  WORK_CLASS(SampleMsg, true)

private:
  float istep, jstep, kstep;

    vec3f get_starting_point(VolumeP v)
    {
        Box *box = v->get_local_box();
        return vec3f(box->xyz_min.x + RNDM*(box->xyz_max.x - box->xyz_min.x),
                     box->xyz_min.y + RNDM*(box->xyz_max.y - box->xyz_min.y),
                     box->xyz_min.z + RNDM*(box->xyz_max.z - box->xyz_min.z));
    }

  float Q(VolumeP v, float s, args *a)
  {
    float q;

    if (a->tf_type == TF_LINEAR)
    {
      if (a->tf0 < a->tf1)
      {
        if (s < a->tf0) q = 0.0;
        else if (s > a->tf1) q = 1.0;
        else q = (s - a->tf0) / (a->tf1 - a->tf0);
      }
      else
      {
        if (s < a->tf1) q = 1.0;
        else if (s > a->tf0) q = 0.0;
        else q = (s - a->tf0) / (a->tf1 - a->tf0);
      }
    }
    else
    {
      q = gaussian(s, a->tf0, a->tf1);
    }

    return q;
  }

  float sample(VolumeP v, Particle& p) { return sample(v, p.xyz.x, p.xyz.y, p.xyz.z); }
  float sample(VolumeP v, vec3f xyz) { return sample(v, xyz.x, xyz.y, xyz.z); }
  float sample(VolumeP v, float x, float y, float z)
  {
    float dx, dy, dz;
    v->get_deltas(dx, dy, dz);

    float ox, oy, oz;
    v->get_ghosted_local_origin(ox, oy, oz);

    x = (x - ox) / dx;
    y = (y - oy) / dy;
    z = (z - oz) / dz;

    int ix = (int)x;
    int iy = (int)y;
    int iz = (int)z;

    dx = x - ix;
    dy = y - iy;
    dz = z - iz;

    int v000 = (ix + 0) * istep + (iy + 0) * jstep + (iz + 0) * kstep;
    int v001 = (ix + 0) * istep + (iy + 0) * jstep + (iz + 1) * kstep;
    int v010 = (ix + 0) * istep + (iy + 1) * jstep + (iz + 0) * kstep;
    int v011 = (ix + 0) * istep + (iy + 1) * jstep + (iz + 1) * kstep;
    int v100 = (ix + 1) * istep + (iy + 0) * jstep + (iz + 0) * kstep;
    int v101 = (ix + 1) * istep + (iy + 0) * jstep + (iz + 1) * kstep;
    int v110 = (ix + 1) * istep + (iy + 1) * jstep + (iz + 0) * kstep;
    int v111 = (ix + 1) * istep + (iy + 1) * jstep + (iz + 1) * kstep;

    float b000 = (1.0 - dx) * (1.0 - dy) * (1.0 - dz);
    float b001 = (1.0 - dx) * (1.0 - dy) * (dz);
    float b010 = (1.0 - dx) * (dy) * (1.0 - dz);
    float b011 = (1.0 - dx) * (dy) * (dz);
    float b100 = (dx) * (1.0 - dy) * (1.0 - dz);
    float b101 = (dx) * (1.0 - dy) * (dz);
    float b110 = (dx) * (dy) * (1.0 - dz);
    float b111 = (dx) * (dy) * (dz);

    if (v->get_type() == Volume::FLOAT)
    {
      float *p = (float *)v->get_samples();
      return p[v000]*b000 + p[v001]*b001 + p[v010]*b010 + p[v011]*b011 +
             p[v100]*b100 + p[v101]*b101 + p[v110]*b110 + p[v111]*b111;
    }
    else
    {
      unsigned char *p = (unsigned char *)v->get_samples();
      return p[v000]*b000 + p[v001]*b001 + p[v010]*b010 + p[v011]*b011 +
             p[v100]*b100 + p[v101]*b101 + p[v110]*b110 + p[v111]*b111;
    }
  }

public:
  bool CollectiveAction(MPI_Comm c, bool s)
  {
    args *a = (args *)contents->get();

    VolumeP v = Volume::Cast(KeyedDataObject::GetByKey(a->vk));
    ParticlesP p = Particles::Cast(KeyedDataObject::GetByKey(a->pk));

    p->CopyPartitioning(v);

    float deltaX, deltaY, deltaZ;
    v->get_deltas(deltaX, deltaY, deltaZ);

    float ox, oy, oz;
    v->get_local_origin(ox, oy, oz);

    int lioff, ljoff, lkoff;
    v->get_ghosted_local_offsets(lioff, ljoff, lkoff);

    int nli, nlj, nlk;
    v->get_local_counts(nli, nlj, nlk);

    int gnli, gnlj, gnlk;
    v->get_ghosted_local_counts(gnli, gnlj, gnlk);

    int local_count = nli*nlj*nlk;

    float max_x = ox + (nli-1) * deltaX,
          max_y = oy + (nlj-1) * deltaY,
          max_z = oz + (nlk-1) * deltaZ;

    int ngx, ngy, ngz;
    v->get_local_counts(ngx, ngy, ngz);
    int global_count = ngx*ngy*ngz;

    istep = 1;
    jstep = gnli;
    kstep = gnli * gnlj;

    Particle tp;
    tp.xyz = get_starting_point(v);
    tp.u.value = sample(v, tp.xyz);

    float tq = Q(v, tp.u.value, a);

    Box *partition_box = v->get_local_box();

    int miss_count = 0;
    for (int iteration = 0; iteration < (a->n_start + a->n_iterations); )
    {
      double rv[3];
      normally_distributed_vector(rv);

      Particle cp(tp.xyz.x + rv[0], tp.xyz.y + rv[1], tp.xyz.z + rv[2], float(0.0));
      if (! partition_box->isIn(cp.xyz)) continue;

      iteration ++;

      cp.u.value = sample(v, cp);
      float cq = Q(v, cp.u.value, a);

      if ((cq > tq) || (RNDM < (cq/tq)))
      {
        tp = cp;
        tq = cq;
        if ((iteration > a->n_start) && ((iteration % a->n_skip) == 0))
          p->push_back(tp);
        miss_count = 0;
      }
      else if (++miss_count > a->n_miss)
      {
        tp.xyz = get_starting_point(v);
        tp.u.value = sample(v, tp.xyz);
        miss_count = 0;
      }
    }

    std::cerr << "created " << p->GetNumberOfVertices() << " samples\n";
    return false;
  }
};

WORK_CLASS_TYPE(SampleMsg)

void
syntax(char *a)
{
  cerr << "image writer for Metropolis-Hastings sampling" << endl;
  cerr << "syntax: " << a << " [options] data" << endl;
  cerr << "optons:" << endl;
  cerr << "  -D            run debugger" << endl;
  cerr << "  -M m          max number of consecutive misses allowed (" << max_consecutive_misses << ")" << "\n";
  cerr << "  -i n          iteration limit (20000)\n";
  cerr << "  -h n          hot start count (1000)\n";
  cerr << "  -S step       scaling of candidate step (1)\n";
  cerr << "  -K skip       record every skip'th successful sample (10)\n";
  cerr << "  -R p0 p1      linear transfer function, from p0 to p1 extended (default, linear data range)\n";
  cerr << "  -G p0 p1      gaussian transfer function p0 is median, p2 is stdev (default, linear data range)\n";
  cerr << "  -s x y        window size (" << WIDTH << "x" << HEIGHT << ")" << endl;
  cerr << "  -t target     value of highest interest (" << target << ")" << endl;
  cerr << "  -p power      exponent of interest (" << power << ")" << endl;
  cerr << "  -r radius     radius of samples (" << radius << ")" << endl;
  cerr << "  -k sigma      standard deviation of normally distributed random step vector (" << sigma << ")" << endl;
  exit(1);
}

#if 0
namespace gxy
{
extern void DDSpheres_Hello();
};
#endif

int
main(int argc, char * argv[])
{
  string data = "";
  char *dbgarg;
  bool dbg = false;
  bool override_windowsize = false;

#if 0
  dbg = true;
  std::cerr << getpid() << "\n";
  while (dbg)
    sleep(1);
#endif

  ospInit(&argc, (const char **)argv);

#if 0
  DDSpheres_Hello();

  {
    void *foo = dlopen(NULL, RTLD_NOW | RTLD_GLOBAL);
    void *s = dlsym(foo, "ospray_init_module_ispc");
    if (s) std::cerr << "ospray_init_module_ispc OK\n"; else std::cerr << "ospray_init_module_ispc NOT FOUND\n";

    s = dlsym(foo, "ospray_create_geometry__dspheres");
    if (s) std::cerr << "ospray_create_geometry__dspheres OK\n"; else std::cerr << "ospray_create_geometry__dspheres NOT FOUND\n";
  }
#endif

  Application theApplication(&argc, &argv);
  theApplication.Start();

  for (int i = 1; i < argc; i++)
  {
    if (argv[i][0] == '-')
    {
      switch (argv[i][1])
      {
        case 'D': dbg = true, dbgarg = argv[i] + 2; break;
        case 'M': max_consecutive_misses = atoi(argv[++i]); break;
        case 'i': iteration_limit = atoi(argv[++i]); break;
        case 'h': hot_start = atoi(argv[++i]); break;
        case 'S': scaling = atof(argv[++i]); break;
        case 'K': skip = atoi(argv[++i]); break;
        case 'R': tf0 = atof(argv[++i]), tf1 = atof(argv[++i]), tf_type = TF_LINEAR; break;
        case 'G': tf0 = atof(argv[++i]), tf1 = atof(argv[++i]), tf_type = TF_GAUSSIAN; break;
        case 'r': radius = atof(argv[++i]); break;
        case 't': target = atof(argv[++i]); break;
        case 'p': power  = atof(argv[++i]); break;
        case 'k': sigma = atof(argv[++i]); break;
        case 's': width  = atoi(argv[++i]);
                  height = atoi(argv[++i]);
                  override_windowsize = true;
                  break;
        default:
          syntax(argv[0]);
      }
    }
    else if (data == "") data = argv[i];
    else syntax(argv[0]);
  }

  generator = new variate_generator<mt19937, normal_distribution<> >(mt19937(time(0)), normal_distribution<>(0.0, sigma));

  Renderer::Initialize();
  theApplication.Run();

  RendererP theRenderer = Renderer::NewP();

  mpiRank = theApplication.GetRank();
  mpiSize = theApplication.GetSize();

  srand(mpiRank);

  Debug *d = dbg ? new Debug(argv[0], false, dbgarg) : NULL;

  SampleMsg::Register();

  if (mpiRank == 0)
  {
    theRenderer->Commit();

    VolumeP volume = Volume::NewP();
    volume->Import(data);

    if (tf_type == TF_NONE)
      volume->get_global_minmax(tf0, tf1), tf_type = TF_LINEAR;

    ParticlesP samples = Particles::NewP();

    SampleMsg *smsg = new SampleMsg(volume, samples, tf_type, tf0, tf1, scaling, iteration_limit, hot_start, skip, max_consecutive_misses);
    smsg->Broadcast(true, true);

    samples->Commit();

    DatasetsP theDatasets = Datasets::NewP();
    theDatasets->Insert("samples", samples);
    theDatasets->Insert("volume", volume);
    theDatasets->Commit();

    vector<CameraP> theCameras;

#if 1
    for (int i = 0; i < 20; i++)
    {
      CameraP cam = Camera::NewP();

      cam->set_viewup(0.0, 1.0, 0.0);
      cam->set_angle_of_view(30.0);

      float angle = 2*3.1415926*(i / 20.0);

      float vpx = 8.0 * cos(angle);
      float vpy = 8.0 * sin(angle);

      cam->set_viewpoint(vpx, vpy, 0.0);
      cam->set_viewdirection(-vpx, -vpy, 0.0);

      cam->Commit();
      theCameras.push_back(cam);
    }
#else
    CameraP cam = Camera::NewP();
    cam->set_viewup(0.0, 1.0, 0.0);
    cam->set_angle_of_view(45.0);
    cam->set_viewpoint(4.0, 0.0, 0.0);
    cam->set_viewdirection(-2.0, 0.0, 0.0);
    cam->Commit();
    theCameras.push_back(cam);
#endif

    ParticlesVisP pvis = ParticlesVis::NewP();
    pvis->SetName("samples");
    pvis->Commit(theDatasets);

    VolumeVisP vvis = VolumeVis::NewP();
    vvis->SetName("volume");

    vec4f cmap[] = {
        {0.00,1.0,0.5,0.5},
        {0.25,0.5,1.0,0.5},
        {0.50,0.5,0.5,1.0},
        {0.75,1.0,1.0,0.5},
        {1.00,1.0,0.5,1.0}
    };

    vec2f omap[] = {
                { 0.00, 0.00},
                { 1.00, 0.10},
                { 8.00, 0.10}
               };

    vvis->SetColorMap(5, cmap);
    vvis->SetOpacityMap(3, omap);
    vvis->SetVolumeRendering(true);
    vvis->Commit(theDatasets);

    VisualizationP v = Visualization::NewP();
    float light[] = {1.0, 2.0, 3.0}; int t = 1;
    v->get_the_lights()->SetLights(1, light, &t);
    v->get_the_lights()->SetK(0.4, 0.6);
    v->get_the_lights()->SetShadowFlag(false);
    v->get_the_lights()->SetAO(0, 0.0);
    v->AddVis(pvis);
    v->AddVis(vvis);
    v->Commit(theDatasets);

    RenderingSetP theRenderingSet = RenderingSet::NewP();

    int indx = 0;
    for (auto c : theCameras)
    {
      RenderingP theRendering = Rendering::NewP();
      theRendering->SetTheOwner((indx++) % mpiSize);
      if (override_windowsize)
      {
          c->set_width(width);
          c->set_height(height);
      }
      theRendering->SetTheCamera(c);
      theRendering->SetTheDatasets(theDatasets);
      theRendering->SetTheVisualization(v);
      theRendering->Commit();
      theRenderingSet->AddRendering(theRendering);
    }

    theRenderingSet->Commit();

    theRenderer->Start(theRenderingSet);

#ifdef GXY_WRITE_IMAGES
    theRenderingSet->WaitForDone();
#endif

    theRenderingSet->SaveImages(string("samples"));

    theApplication.QuitApplication();
  }

  theApplication.Wait();
}


#include <Configuration.h> // Package Path
#include <avatar_locomanipulation/enable_pinocchio_with_hpp_fcl.h> // Enable HPP FCL
// Multibody
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"
// Parsers
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
// Algorithms
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/model.hpp"
// Spatial
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
// Standard
#include <math.h>
#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/shared_ptr.hpp>

pinocchio::fcl::Quaternion3f makeQuat(double w, double x, double y, double z){
  pinocchio::fcl::Quaternion3f q;
  q.w() = w;
  q.x() = x;
  q.y() = y;
  q.z() = z;
  return q;
}

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

  pinocchio::Model valkyriemodel, boxmodel, valkyrieboxmodel;
  pinocchio::GeometryModel geomValkyrie, geomBox, geomValkyrieBox;

  //----------------- Robot Model
  std::string filename = (argc<=1) ? THIS_PACKAGE_PATH"models/valkyrie_simplified.urdf" : argv[1];
  //const std::string srdf_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/romeo_description/srdf/romeo.srdf";
  std::string srdf_filename = (argc<=1) ? THIS_PACKAGE_PATH"models/valkyrie_disable_collisions.srdf" : argv[1];
  std::vector < std::string > packageDirs;
  std::string meshDir  = THIS_PACKAGE_PATH"../val_model/";
  packageDirs.push_back(meshDir);

  // build URDF model
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),valkyriemodel);

  // build Geometry model
  pinocchio::urdf::buildGeom(valkyriemodel, filename, pinocchio::COLLISION, geomValkyrie, packageDirs );

  Eigen::VectorXd q = Eigen::VectorXd::Zero(valkyriemodel.nq); // Config

  // floating base joints: x, y, z
  q[0] = 0.0;  q[1] = 0.0;  q[2] = 0.0;

  // floating base quaternion: qx, qy, qz, qw
  double theta = 0;//M_PI/4.0;
  Eigen::AngleAxis<double> aa(theta, Eigen::Vector3d(0.0, 0.0, 1.0)); // yaw pi/4 to the left
  Eigen::Quaternion<double> quat_init; quat_init =  aa;

  q[3] = quat_init.x();// 0.0;
  q[4] = quat_init.y(); //0.0;
  q[5] = quat_init.z(); //sin(theta/2.0);
  q[6] = quat_init.w(); //cos(theta/2.0);
  //------------- End Robot Model

  //------------- Box Model
  static double pi = M_PI;
  pinocchio::fcl::Transform3f tf1 (makeQuat (0, 0, 1, 0), pinocchio::fcl::Vec3f (1.0, 0, 1.125));
  pinocchio::SE3 placement = pinocchio::toPinocchioSE3(tf1);

  pinocchio::Model::JointIndex idx;
  idx = boxmodel.addJoint(boxmodel.getJointId("universe"),pinocchio::JointModelPlanar(),pinocchio::SE3::Identity(),"planar1_joint");
  boxmodel.addJointFrame(idx);
  boxmodel.appendBodyToJoint(idx,pinocchio::Inertia::Random(),pinocchio::SE3::Identity());
  boxmodel.addBodyFrame("planar1_body", idx, pinocchio::SE3::Identity());

  boost::shared_ptr<pinocchio::fcl::Box> sample(new pinocchio::fcl::Box(0.05, 1.0, 2.25));
  pinocchio::Model::FrameIndex box_id = boxmodel.getBodyId("planar1_body");
  pinocchio::Model::JointIndex box_parent = boxmodel.frames[box_id].parent;

  pinocchio::GeometryModel::GeomIndex box_geom_id = geomBox.addGeometryObject(pinocchio::GeometryObject("box",
                                                                           boxmodel.getBodyId("planar1_body"),box_parent,
                                                                           sample,placement, "", Eigen::Vector3d::Ones())
                                                            );
  geomBox.geometryObjects[box_geom_id].parentJoint = boxmodel.frames[box_id].parent;

  Eigen::VectorXd p(boxmodel.nq);
  p << 0, 0, 1, 0;
  //------------- End Box Model

// Box:
  geomBox.addAllCollisionPairs();
  pinocchio::Data boxdata(boxmodel);
  pinocchio::GeometryData geomDataBox(geomBox);
  pinocchio::updateGeometryPlacements(boxmodel, boxdata, geomBox, geomDataBox, p);
// Valkyrie:
  geomValkyrie.addAllCollisionPairs();
  std::cout << "geomValkyrie.collisionPairs.size(): " << geomValkyrie.collisionPairs.size() << std::endl;
  //pinocchio::srdf::removeCollisionPairs(valkyriemodel, geomValkyrie, srdf_filename, false);
  std::cout << "geomValkyrie.collisionPairs.size(): " << geomValkyrie.collisionPairs.size() << std::endl;
  pinocchio::Data valkyriedata(valkyriemodel);
  pinocchio::GeometryData geomDataValkyrie(geomValkyrie);
  pinocchio::updateGeometryPlacements(valkyriemodel, valkyriedata, geomValkyrie, geomDataValkyrie, q);

//---------------- Begin Append
  std::cout << "geomBox.ngeoms: " << geomBox.ngeoms << std::endl;
  std::cout << "geomValkyrie.ngeoms: " << geomValkyrie.ngeoms << std::endl;

  std::cout << "q.transpose: " << q.transpose() << std::endl;
  std::cout << "p.transpose: " << p.transpose() << std::endl;

  // Append model: appends boxmodel to valkyrie model, and geomBox to geomValkyrie
  	// appends at some frame index fid with the relative config SE3::Identity()
  		// and it outputs to the final model and geomModel
  	// setting fid = valkyriemodel.frames.size() means that it is added to the last frame
  pinocchio::appendModel(valkyriemodel, boxmodel, geomValkyrie, geomBox, valkyriemodel.frames.size()-1, pinocchio::SE3::Identity(), valkyrieboxmodel, geomValkyrieBox);
  std::cout << "geomValkyrieBox.ngeoms: " << geomValkyrieBox.ngeoms << std::endl;
  std::cout << "boxmodel.nq: " << boxmodel.nq << std::endl;
  std::cout << "valkyriemodel.nq: " << valkyriemodel.nq << std::endl;
  std::cout << "valkyrieboxmodel.nq: " << valkyrieboxmodel.nq << std::endl;
//---------------- End Append

  // Create config vector for new appended model
  Eigen::VectorXd r(valkyrieboxmodel.nq);
  r << q, p;

  geomValkyrieBox.addAllCollisionPairs();
  pinocchio::Data valkyrieboxdata(valkyrieboxmodel);
  pinocchio::GeometryData geomDataValkyrieBox(geomValkyrieBox);
  pinocchio::updateGeometryPlacements(valkyrieboxmodel, valkyrieboxdata, geomValkyrieBox, geomDataValkyrieBox, r);

  // Define our counters, collision/distance results, contacts
  int j, i, k;
  pinocchio::fcl::CollisionResult result;
  pinocchio::fcl::DistanceResult dresult;
  std::vector<pinocchio::fcl::Contact> contacts;

  pinocchio::computeCollisions(valkyrieboxmodel, valkyrieboxdata, geomValkyrieBox, geomDataValkyrieBox, r);

  // Print relevant collision/distance data
  std::cout << "COLLISION RESULTS 1" << std::endl;
  for(j=0; j<geomDataValkyrieBox.collisionResults.size(); j++)
  {
  	result = geomDataValkyrieBox.collisionResults[j];
  	pinocchio::CollisionPair idx = geomValkyrieBox.collisionPairs[j];
  	result.getContacts(contacts);
      	if(contacts.size() != 0)
      	{
      		for(k=0; k<contacts.size(); k++)
      		{
      			std::cout << "Contact Found Between: " << geomValkyrieBox.getGeometryName(idx.first) << " and " << geomValkyrieBox.getGeometryName(idx.second) << std::endl;
        		std::cout << "position: " << contacts[k].pos << std::endl;
        		std::cout << "-------------------" << std::endl;
      		}
      	}
      	else
      	{
      		pinocchio::computeDistance(geomValkyrieBox, geomDataValkyrieBox, geomValkyrieBox.findCollisionPair(idx));
      		dresult = geomDataValkyrieBox.distanceResults[j];
      		std::cout << "Minimum Distance Between: " << geomValkyrieBox.getGeometryName(idx.first) << " and " << geomValkyrieBox.getGeometryName(idx.second) << " = " << dresult.min_distance << std::endl;
      	}
  }

  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "---------------------------------------------------------" << std::endl;
  std::cout << "---------------------------------------------------------" << std::endl;

  //------------------ Update the robot configuration
  Eigen::VectorXd q1 = Eigen::VectorXd::Zero(valkyriemodel.nq); // Config

  // floating base joints: x, y, z
  q1[0] = 0.0;  q1[1] = 0.0;  q1[2] = 0.0;

  // floating base quaternion: qx, qy, qz, qw
  double theta1 = M_PI/4.0;
  Eigen::AngleAxis<double> bb(theta1, Eigen::Vector3d(0.0, 0.0, 1.0)); // yaw pi/4 to the left
  Eigen::Quaternion<double> quat_init_1; quat_init_1 =  bb;

  q1[3] = quat_init_1.x();// 0.0;
  q1[4] = quat_init_1.y(); //0.0;
  q1[5] = quat_init_1.z(); //sin(theta/2.0);
  q1[6] = quat_init_1.w(); //cos(theta/2.0);
  //------------------ End Update the robot configuration

  //------------------ Update the box configuration
  Eigen::VectorXd p1(boxmodel.nq);
  p1 << 5, 0, 1, 0;
  //------------------ End Update the box configuration

  // Create config vector for new appended model
  Eigen::VectorXd r1(valkyrieboxmodel.nq);
  r1 << q1, p1;

  pinocchio::computeCollisions(valkyrieboxmodel, valkyrieboxdata, geomValkyrieBox, geomDataValkyrieBox, r1);

  // Print relevant collision/distance data
  std::cout << "COLLISION RESULTS 2" << std::endl;
  for(j=0; j<geomDataValkyrieBox.collisionResults.size(); j++)
  {
  	result = geomDataValkyrieBox.collisionResults[j];
  	pinocchio::CollisionPair idx = geomValkyrieBox.collisionPairs[j];
  	result.getContacts(contacts);
      	if(contacts.size() != 0)
      	{
      		for(k=0; k<contacts.size(); k++)
      		{
      			std::cout << "Contact Found Between: " << geomValkyrieBox.getGeometryName(idx.first) << " and " << geomValkyrieBox.getGeometryName(idx.second) << std::endl;
        		std::cout << "position: " << contacts[k].pos << std::endl;
        		std::cout << "-------------------" << std::endl;
      		}
      	}
      	else
      	{
      		pinocchio::computeDistance(geomValkyrieBox, geomDataValkyrieBox, geomValkyrieBox.findCollisionPair(idx));
      		dresult = geomDataValkyrieBox.distanceResults[j];
      		std::cout << "Minimum Distance Between: " << geomValkyrieBox.getGeometryName(idx.first) << " and " << geomValkyrieBox.getGeometryName(idx.second) << " = " << dresult.min_distance << std::endl;
      	}
  }

}


/**
 * \ file TimeVaryingSecondOrderSVFFilter.cpp
 */

#include <ATK/Core/InPointerFilter.h>

#include <ATK/EQ/TimeVaryingSecondOrderSVFFilter.h>

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

#include <array>

#include <boost/math/constants/constants.hpp>

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

constexpr gsl::index PROCESSSIZE = 1024*64;
constexpr gsl::index input_sampling_rate = 1024*64;

BOOST_AUTO_TEST_CASE( TimeVaryingSecondOrderSVFBandPassCoefficients_Q_test )
{
  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFBandPassCoefficients<double> > filter;
  filter.set_Q(0.5);
  BOOST_CHECK_EQUAL(filter.get_Q(), 0.5);
}

BOOST_AUTO_TEST_CASE( TimeVaryingSecondOrderSVFBandPassCoefficients_Q_range_test )
{
  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFBandPassCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_Q(0.), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( TimeVaryingSecondOrderSVFBellCoefficients_gain_test )
{
  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFBellCoefficients<double> > filter;
  filter.set_gain(0.5);
  BOOST_CHECK_EQUAL(filter.get_gain(), 0.5);
}

BOOST_AUTO_TEST_CASE( TimeVaryingSecondOrderSVFBellCoefficients_gain_range_test )
{
  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFBellCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_gain(0.), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( TimeVaryingSecondOrderSVFLowShelfCoefficients_Q_test )
{
  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFLowShelfCoefficients<double> > filter;
  filter.set_gain(0.5);
  BOOST_CHECK_EQUAL(filter.get_gain(), 0.5);
}

BOOST_AUTO_TEST_CASE( TimeVaryingSecondOrderSVFHighShelfCoefficients_Q_test )
{
  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFHighShelfCoefficients<double> > filter;
  filter.set_gain(0.5);
  BOOST_CHECK_EQUAL(filter.get_gain(), 0.5);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFNotchCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFNotchCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFNotchCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFNotchCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFPeakCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFPeakCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFPeakCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFPeakCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFBellCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(10);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFBellCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(10);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFBellCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(10);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFBellCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(10);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFLowShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFLowShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFLowShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 100 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFHighShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 1000 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFHighShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 1000 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

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

  ATK::TimeVaryingSecondOrderSVFFilter<ATK::TimeVaryingSecondOrderSVFHighShelfCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);

  std::array<float, 2*PROCESSSIZE> data;
  for(gsl::index i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = std::tan(boost::math::constants::pi<double>() * 1000 / input_sampling_rate);;
  }

  ATK::InPointerFilter<float> generator2(data.data(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

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

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

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}


//===---------------------------------------------------------------------------------*- C++-*-===//
//                         _____                        _
//                        / ____|                      (_)
//                       | (___   ___  __ _ _   _  ___  _  __ _
//                        \___ \ / _ \/ _` | | | |/ _ \| |/ _` |
//                        ____) |  __/ (_| | |_| | (_) | | (_| |
//                       |_____/ \___|\__, |\__,_|\___/|_|\__,_| - Game Engine
//                                       | |
//                                       |_|
//
// This file is distributed under the MIT License (MIT).
// See LICENSE.txt for details.
//
//===------------------------------------------------------------------------------------------===//

#include "sequoia-engine/Game/SceneNode.h"
#include "sequoia-engine/Core/Format.h"
#include "sequoia-engine/Core/Logging.h"
#include "sequoia-engine/Core/StringRef.h"
#include "sequoia-engine/Core/StringUtil.h"
#include "sequoia-engine/Math/CoordinateSystem.h"
#include <boost/lexical_cast.hpp>
#include <numeric>

namespace sequoia {

namespace game {

SceneNode::SceneNode(const std::string& name, SceneNode::SceneNodeKind kind)
    : std::enable_shared_from_this<SceneNode>(), kind_(kind), position_(math::vec3()),
      orientation_(math::quat()), scale_(1.0f), modelMatrix_(), modelMatrixIsDirty_(true),
      parent_(), name_(name) {
  for(int i = 0; i < capabilities_.size(); ++i)
    capabilities_[i] = nullptr;
}

SceneNode::SceneNode(const SceneNode& other)
    : std::enable_shared_from_this<SceneNode>(), kind_(other.kind_), position_(other.position_),
      orientation_(other.orientation_), scale_(other.scale_), modelMatrix_(other.modelMatrix_),
      modelMatrixIsDirty_(other.modelMatrixIsDirty_), parent_(other.parent_) {

  // Adjust the name, we append a `_copy_X` where `X` is the version of the copy
  StringRef nameRef(other.name_);
  std::size_t copyPos = nameRef.find("_copy_");

  if(copyPos != StringRef::npos) {
    StringRef versionStr = nameRef.substr(copyPos + sizeof("_copy_") - 1);
    int version = boost::lexical_cast<int>(versionStr.data(), versionStr.size()) + 1;
    name_ = nameRef.substr(0, copyPos).str() + "_copy_" + std::to_string(version);
  } else {
    name_ = other.name_ + "_copy_1";
  }

  // Clone children
  for(const auto& child : other.children_)
    children_.emplace_back(child->clone());

  // Clone capabilites
  for(int i = 0; i < capabilities_.size(); ++i)
    capabilities_[i] = other.capabilities_[i] ? other.capabilities_[i]->clone(this) : nullptr;
}

SceneNode::~SceneNode() {}

void SceneNode::resetOrientation() { setOrientation(math::quat()); }

math::mat3 SceneNode::getLocalAxes() const {
  math::vec3 axisX = getOrientation() * math::CoordinateSystem::X();
  math::vec3 axisY = getOrientation() * math::CoordinateSystem::Y();
  math::vec3 axisZ = getOrientation() * math::CoordinateSystem::Z();
  return math::mat3(axisX, axisY, axisZ);
}

void SceneNode::applyImpl(const std::function<void(SceneNode*)>& functor) {
  functor(this);
  for(const auto& child : getChildren())
    child->applyImpl(functor);
}

void SceneNode::applyNoexceptImpl(
    const std::function<void(SceneNode*) noexcept>& functor) noexcept {
  functor(this);
  for(const auto& child : getChildren())
    child->applyNoexceptImpl(functor);
}

void SceneNode::update(const UpdateEvent& event) {
  for(int i = 0; i < capabilities_.size(); ++i)
    if(capabilities_[i])
      capabilities_[i]->update(event);
}

void SceneNode::translate(const math::vec3& d, TransformSpace relativeTo) {
  setPosition(getPosition() + d);
}

void SceneNode::rotate(const math::quat& q, TransformSpace relativeTo) {
  math::quat newOrientation = q * getOrientation();

  // Normalize quaternion to avoid drift
  newOrientation = math::normalize(newOrientation);
  setOrientation(newOrientation);
}

void SceneNode::rotate(const math::vec3& axis, const math::Radian& angle,
                       SceneNode::TransformSpace relativeTo) {
  rotate(math::angleAxis(angle.inRadians(), axis), relativeTo);
}

void SceneNode::roll(const math::Radian& angle, TransformSpace relativeTo) {
  rotate(math::CoordinateSystem::Z(), angle, relativeTo);
}

void SceneNode::pitch(const math::Radian& angle, TransformSpace relativeTo) {
  rotate(math::CoordinateSystem::X(), angle, relativeTo);
}

void SceneNode::yaw(const math::Radian& angle, TransformSpace relativeTo) {
  rotate(math::CoordinateSystem::Y(), angle, relativeTo);
}

std::shared_ptr<SceneNode> SceneNode::clone() { return SceneNode::allocate<SceneNode>(*this); }

std::string SceneNode::toString() const {
  auto stringPair = toStringImpl();
  return core::format("{}[\n  {}]", stringPair.first, core::indent(stringPair.second));
}

std::pair<std::string, std::string> SceneNode::toStringImpl() const {
  return std::make_pair(
      "SceneNode",
      core::format(
          "name = {},\n"
          "position = {},\n"
          "orientation = {},\n"
          "scale = {},\n"
          "parent = {},\n"
          "capabilities = {},\n"
          "children = {},\n",
          name_, position_, orientation_, scale_, hasParent() ? getParent()->getName() : "null",

          // Capabilities
          std::accumulate(
              capabilities_.begin(), capabilities_.end(), true,
              [](bool isNull, const auto& capability) { return isNull && capability == nullptr; })
              ? "null"
              : core::toStringRange(capabilities_,
                                    [](const auto& capability) {
                                      return core::indent(capability ? capability->toString()
                                                                     : "null");
                                    }),

          // Children
          hasChildren()
              ? core::toStringRange(children_,
                                    [](const auto& node) { return core::indent(node->toString()); })
              : "null"));
}

void SceneNode::computeModelMatrix() {
  // ModelMatrix = TranslationMatrix * RotatationMatrix * ScaleMatrix
  modelMatrix_ = math::translate(math::mat4(1.0f), position_);
  modelMatrix_ = modelMatrix_ * math::mat4_cast(orientation_);
  modelMatrix_ = math::scale(modelMatrix_, glm::vec3(scale_));

  modelMatrixIsDirty_ = false;
}

} // namespace game

} // namespace sequoia


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE MergeMultiSgdVarUpdatesTransformation0

#include <../../random_util.hpp>
#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/session.hpp>
#include <popart/sgd.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/testdevice.hpp>

#include <algorithm>
#include <map>
#include <memory>
#include <tuple>
#include <vector>

using namespace popart;

namespace {
TensorId
bnconv(Builder *b, TensorId act, ConstVoidData cwdata, ConstVoidData bwdata) {
  auto aiOnnx      = b->aiOnnxOpset9();
  auto convWeights = b->addInitializedInputTensor(cwdata);
  auto scale       = b->addInitializedInputTensor(bwdata);
  auto bias        = b->addInitializedInputTensor(bwdata);
  auto mean        = b->addInitializedInputTensor(bwdata);
  auto var         = b->addInitializedInputTensor(bwdata);
  auto act0 =
      aiOnnx.conv({act, convWeights}, {1, 1}, 1, {}, {1, 1, 1, 1}, {1, 1});
  auto act1 = aiOnnx.batchnormalization({act0, scale, bias, mean, var}, 5);
  // return the output of batch-norm
  return act1[0];
}
} // namespace

BOOST_AUTO_TEST_CASE(Transformation_MergeMultiSGD) {

  auto test = [](MergeVarUpdateType mvu) {
    // we will generate random input data
    int seed = 1013;
    DefaultRandomEngine eng(seed);
    UniformRealDistribution<float> fdis(-4.f, +4.f);

    // construct onnx model
    auto builder = Builder::create();
    auto aiOnnx  = builder->aiOnnxOpset9();

    int nInChans  = 3;
    int batchsize = 1;
    TensorInfo in0info{"FLOAT",
                       std::vector<int64_t>{batchsize, nInChans, 6, 6}};
    auto in0 = builder->addInputTensor(in0info);
    std::vector<float> in0data(in0info.nelms());
    for (auto &val : in0data) {
      val = fdis(eng);
    }

    // The model will be
    // reduce(bn(conv(bn(conv(bn(conv(bn(conv(....(bn(conv(input))...)))), nConv
    // convolutions chained together, with the number of channels increasing by
    // 1 at each subsequent conv.
    constexpr int nConv = 8;
    std::array<std::vector<float>, nConv> convWeights;
    std::array<std::vector<float>, nConv> bnWeights;
    std::array<ConstVoidData, nConv> cvds;
    std::array<ConstVoidData, nConv> bnds;
    std::array<TensorId, nConv + 1> actIds;
    actIds[0] = in0;

    int64_t copyElms = 0;
    int64_t sgdElms  = 0;
    for (int i = 0; i < nConv; ++i) {
      int nOutChans = nInChans + 1;
      TensorInfo conv_weight_info{
          "FLOAT", std::vector<int64_t>{nOutChans, nInChans, 1, 1}};
      TensorInfo bn_weight_info("FLOAT", std::vector<int64_t>{nOutChans});

      copyElms += 2 * nOutChans;
      sgdElms += 2 * nOutChans;
      sgdElms += nOutChans * nInChans;

      convWeights[i] = std::vector<float>(nOutChans * nInChans, 0);
      for (auto &x : convWeights[i]) {
        x = fdis(eng);
      }
      bnWeights[i] = std::vector<float>(nOutChans, 0);
      for (auto &x : bnWeights[i]) {
        x = fdis(eng);
      }

      cvds[i] = {convWeights[i].data(), conv_weight_info};
      bnds[i] = {bnWeights[i].data(), bn_weight_info};

      actIds[i + 1] = bnconv(builder.get(), actIds[i], cvds[i], bnds[i]);
      nInChans      = nOutChans;
    }

    auto reduced =
        aiOnnx.reducesum({actIds[nConv]}, std::vector<int64_t>{{1, 2, 3}});
    float lossLambda = 0.26;
    auto l1 = builder->aiGraphcoreOpset1().l1loss({reduced}, lossLambda);
    std::string proto = builder->getModelProto();
    auto modelProto   = io::getModelFromString(proto);

    // create the IR
    auto art      = AnchorReturnType("All");
    auto dataFlow = DataFlow(1, {{reduced, art}});

    auto device = popart::createTestDevice(TEST_TARGET);

    auto opts = SessionOptions();

    // accurate scheduling is quite slow for this one. Imposing this limit on
    // how many swaps to perform in the scheduling algorithm reduces test time
    // from 280 seconds to 50 seconds.
    opts.swapLimitScheduler = 20;
    opts.firstDotOp         = 0;
    opts.finalDotOp         = 200;
    opts.dotChecks.insert(DotCheck::Final);
    opts.logDir                     = ".";
    opts.mergeVarUpdate             = mvu;
    opts.mergeVarUpdateMemThreshold = mvu == MergeVarUpdateType::AutoLoose
                                          ? 10000
                                          : 24; // 24 bytes = 7 floats
    opts.looseThresholdAtPeak = 10000;

    float learningRate = 0.1;
    auto optimizer     = SGD({{"defaultLearningRate", {learningRate, false}}});

    Ir ir;
    ir.prepare({modelProto,
                InputShapeInfo(),
                dataFlow,
                l1,
                &optimizer,
                *device,
                opts,
                Patterns()});

    // Check the ir
    //
    // For case MergeVarUpdateType::All, all the ConstSgdVarUpdates have the
    // same learning rate, weight decay, so should all be merged into a single
    // group

    // Lambda to check the number of ops are as expected.
    auto checkOps = [&](const popart::AiGraphcoreOpIdV1 &opType, int expected) {
      auto count = ir.opsOfType(opType).size();
      std::cout << opType.type << " Elements : " << count
                << " Expected: " << expected << std::endl;
      BOOST_CHECK(count == expected);
    };
    if (mvu == MergeVarUpdateType::All) {
      // 10 reshapes per layer are:
      // 3) conv filters, bn bias, bn scale
      // 3) grads of each of the above
      // 2) running mean & variance
      // 2) updates for each of the above

      checkOps(Onnx::CustomOperators::ReshapeInplace, nConv * 10);

      // 4 ConcatInplace
      checkOps(Onnx::CustomOperators::ConcatInplace, 4);
      checkOps(Onnx::CustomOperators::SGD0VarUpdate, 1);
      checkOps(Onnx::CustomOperators::CopyVarUpdate, 1);

    } else if (mvu == MergeVarUpdateType::None) {
      checkOps(Onnx::CustomOperators::SGD0VarUpdate, 3 * nConv);
      checkOps(Onnx::CustomOperators::CopyVarUpdate, 2 * nConv);
      checkOps(Onnx::CustomOperators::ReshapeInplace, 0);
      checkOps(Onnx::CustomOperators::ConcatInplace, 0);
    }

    else if (mvu == MergeVarUpdateType::AutoTight) {
      auto thr = opts.mergeVarUpdateMemThreshold;
      std::cout << "memory threshold : " << thr << std::endl;
      auto expectednsgd = (sgdElms * 4) / thr + ((sgdElms * 4) % thr != 0);
      checkOps(Onnx::CustomOperators::SGD0VarUpdate, expectednsgd);

      auto expectedncopy = (copyElms * 4) / thr + ((copyElms * 4) % thr != 0);
      checkOps(Onnx::CustomOperators::CopyVarUpdate, expectedncopy);
    }

    else if (mvu == MergeVarUpdateType::AutoLoose) {
      // because both thresholds are greater than the total memory, there should
      // be just 1 SGDVarUpdate and 1 CopyVarUpdate
      checkOps(Onnx::CustomOperators::SGD0VarUpdate, 1);
      checkOps(Onnx::CustomOperators::CopyVarUpdate, 1);
    }
  };

  test(MergeVarUpdateType::All);
  test(MergeVarUpdateType::None);
  test(MergeVarUpdateType::AutoTight);
  test(MergeVarUpdateType::AutoLoose);
}


//
// Copyright (c) 2015-2016,2018 CNRS
//

#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"

#include <boost/python/overloads.hpp>

namespace pinocchio
{
  namespace python
  {

    BOOST_PYTHON_FUNCTION_OVERLOADS(jacobianCenterOfMassUpdate_overload,jacobianCenterOfMass,3,4)

    BOOST_PYTHON_FUNCTION_OVERLOADS(jacobianCenterOfMassNoUpdate_overload,jacobianCenterOfMass,2,3)

    static SE3::Vector3
    com_0_proxy(const Model& model,
                Data & data,
                const Eigen::VectorXd & q)
    {
      return centerOfMass(model,data,q,true);
    }

    static SE3::Vector3
    com_1_proxy(const Model& model,
                Data & data,
                const Eigen::VectorXd & q,
                const Eigen::VectorXd & v)
    {
      return centerOfMass(model,data,q,v,true);
    }

    static SE3::Vector3
    com_2_proxy(const Model & model,
                Data & data,
                const Eigen::VectorXd & q,
                const Eigen::VectorXd & v,
                const Eigen::VectorXd & a)
    {
      return centerOfMass(model,data,q,v,a,true);
    }

    void exposeCOM()
    {
      using namespace Eigen;

      bp::def("centerOfMass",com_0_proxy,
              bp::args("Model","Data",
                       "Joint configuration q (size Model::nq)"),
              "Compute the center of mass, putting the result in Data and return it.");

      bp::def("centerOfMass",com_1_proxy,
              bp::args("Model","Data",
                       "Joint configuration q (size Model::nq)",
                       "Joint velocity v (size Model::nv)"),
              "Computes the center of mass position and velocuty by storing the result in Data"
              "and returns the center of mass position of the full model expressed in the world frame.");

      bp::def("centerOfMass",com_2_proxy,
              bp::args("Model","Data",
                       "Joint configuration q (size Model::nq)",
                       "Joint velocity v (size Model::nv)",
                       "Joint acceleration a (size Model::nv)"),
              "Computes the center of mass position, velocity and acceleration by storing the result in Data"
              "and returns the center of mass position of the full model expressed in the world frame.");

      bp::def("jacobianCenterOfMass",
              (const Data::Matrix3x & (*)(const Model &, Data &, const Eigen::MatrixBase<Eigen::VectorXd> &, bool))&jacobianCenterOfMass<double,0,JointCollectionDefaultTpl,VectorXd>,
              jacobianCenterOfMassUpdate_overload(bp::args("Model","Data",
                       "Joint configuration q (size Model::nq)",
                       "computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"),
              "Computes the jacobian of the center of mass, puts the result in Data and return it.")[
              bp::return_value_policy<bp::return_by_value>()]);

            bp::def("jacobianCenterOfMass",
              (const Data::Matrix3x & (*)(const Model &, Data &, bool))&jacobianCenterOfMass<double,0,JointCollectionDefaultTpl>,
              jacobianCenterOfMassNoUpdate_overload(bp::args("Model","Data",
                       "computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"),
              "Computes the jacobian of the center of mass, puts the result in Data and return it.")[
              bp::return_value_policy<bp::return_by_value>()]);
    }

  } // namespace python
} // namespace pinocchio


#include <iostream>
//#include<mlpack/core.hpp>
// #include <armadillo>
// #include <string>

#include "headers/splines.h"

using namespace std;

int main() {
  // string data_source = "data.csv";
  // arma::mat data;

  // if (data.load(data_source)) {
  //   cout << data_source << " successfully loaded!" << endl;
  //   cout << data_source << ":" << endl << data << endl;
  // } else {
  //   cout << "data.csv failed to load!" << endl << endl;
  // }

  BSplines f(1, 1);

  return 0;
}


#pragma once

#include <boost/multi_array.hpp>
#include <boost/multi_array/types.hpp>

#include <memory>
#include <vector>
#include <tuple>

typedef std::size_t size_t;

template <typename T>
using volume = boost::multi_array<T,3>;

template <typename T>
using volume_ref = boost::multi_array_ref<T,3>;

template <typename T>
using volume_const_ref = boost::const_multi_array_ref<T,3>;

template <typename T>
using volume_ptr = std::shared_ptr<volume<T>>;

template <typename T>
using volume_ref_ptr = std::shared_ptr<volume_ref<T>>;

template <typename T>
using volume_const_ref_ptr = std::shared_ptr<volume_const_ref<T>>;

template <typename T>
using affinity_graph = boost::multi_array<T,4>;

template <typename T>
using affinity_graph_ref = boost::multi_array_ref<T,4>;

template <typename T>
using affinity_graph_const_ref = boost::const_multi_array_ref<T,4>;

template <typename T>
using affinity_graph_ptr = std::shared_ptr<affinity_graph<T>>;

template <typename T>
using affinity_graph_ref_ptr = std::shared_ptr<affinity_graph_ref<T>>;

template <typename T>
using affinity_graph_const_ref_ptr = std::shared_ptr<affinity_graph_const_ref<T>>;


//
// MIT License
//
// Copyright (c) Deif Lou
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//

#include <opencv2/imgproc.hpp>
#include <tina/sys/sysPro.h>
#include <tina/sys/sysDef.h>
#include <tina/image/imgDef.h>
#include <tina/image/imgPro.h>
#include <tina/medical/medDef.h>
#include <tina/medical/medPro.h>
#undef E
#include <Eigen/Dense>

#include "filter.h"
#include "filterwidget.h"
#include <imgproc/lut.h>
#include <imgproc/types.h>
#include <imgproc/colorconversion.h>
#include <misc/util.h>

#define MAX_IMAGE_SIZE 256

Imrect * modified_xy_normf(Imrect *im, double constant, double sigma, double thresh)
{
    Q_UNUSED(thresh)
    Imrect *im1,*im2,*imdx,*imdy;
    Imrect *imxs,*imys,*corr;

    im1 = im_square(im);

    smooth_slopes(constant,sigma,im1,&imdx,&imdy,&corr,&imxs,&imys);
    im_free(im1);
    enf_integ(&imdx,&imdy,corr,imxs,imys);
    im_free(corr);
    im2 = im_integrate(imdx,imdy);

    im_free(imxs); im_free(imys);
    im_free(imdx);
    im_free(imdy);
    if (im2 == NULL)
    {
    error("imcalc_xy_norm:  image not in calculator ",warning);
        return(NULL);
    }

    return im2;
}

Filter::Filter() :
    mRefinement(10),
    mSmoothness(10),
    mOutputMode(CorrectedImageMode1)
{
}

Filter::~Filter()
{
}

ImageFilter *Filter::clone()
{
    Filter * f = new Filter();
    f->mRefinement = mRefinement;
    f->mSmoothness = mSmoothness;
    f->mOutputMode = mOutputMode;
    return f;
}

extern "C" QHash<QString, QString> getIBPPluginInfo();
QHash<QString, QString> Filter::info()
{
    return getIBPPluginInfo();
}

QImage Filter::process(const QImage &inputImage)
{
    if (inputImage.isNull() || inputImage.format() != QImage::Format_ARGB32)
        return inputImage;

    register int x, y, w = inputImage.width(), h = inputImage.height(), mean = 0, sw, sh;
    register HSL * bitsHSL = (HSL *)malloc(w * h * sizeof(HSL)), * bitsHSLsl;
    cv::Mat mlchannel(h, w, CV_8UC1);
    register unsigned char * mbits8;

    // Convert to HSL
    convertBGRToHSL(inputImage.bits(), (unsigned char *)bitsHSL, w * h);

    // Separate L channel
    for (y = 0; y < h; y++)
    {
        bitsHSLsl = bitsHSL + y * w;
        mbits8 = mlchannel.ptr(y);
        for (x = 0; x < w; x++)
        {
            *mbits8 = bitsHSLsl->l;
            bitsHSLsl++;
            mbits8++;
        }
    }

    // Resample
    cv::Mat mInitial;
    if (w != MAX_IMAGE_SIZE || h != MAX_IMAGE_SIZE)
    {
        if (w > h)
        {
            sw = MAX_IMAGE_SIZE;
            sh = h * MAX_IMAGE_SIZE / w;
        }
        else
        {
            sh = MAX_IMAGE_SIZE;
            sw = w * MAX_IMAGE_SIZE / h;
        }
        sw = sw < 1 ? 1 : sw;
        sh = sh < 1 ? 1 : sh;

        cv::Mat mresized(sh, sw, CV_8UC1);
        cv::resize(mlchannel, mresized, cv::Size(sw, sh), 0, 0, cv::INTER_CUBIC);
        mInitial = mresized;
    }
    else
    {
        sw = w;
        sh = h;
        mInitial = mlchannel;
    }

    // ---------------------------
    // Tina Vision Bias Correction
    // ---------------------------

    Imrect * tvInitialImage = im_alloc(sh, sw, 0, uchar_v);
    for (y = 0; y < sh; y++)
        memcpy(((unsigned char **)tvInitialImage->data)[y], mInitial.ptr(y), sw);

    Imrect * tvBiasImage = modified_xy_normf(tvInitialImage, mRefinement, mSmoothness, 0);
    im_free(tvInitialImage);
    Imrect * tvExpImage = im_exp(tvBiasImage);
    im_free(tvBiasImage);

    float * tvImagePtr;
    Imrect * tvFinalImage = tvExpImage;
    int totalPixels = sw * sh;

    // Set up the matrix A and the vector b for least squares fitting with the initial image
    register int row = 0;
    Eigen::MatrixXf ls_A = Eigen::MatrixXf(totalPixels, 2);
    Eigen::VectorXf ls_b = Eigen::VectorXf(totalPixels);

    for (y = 0; y < sh; y++)
    {
        mbits8 = mInitial.ptr(y);
        tvImagePtr = ((float **)tvFinalImage->data)[y];
        for (x = 0; x < sw; x++)
        {
            ls_A(row, 0) = 1;
            ls_A(row, 1) = *tvImagePtr;
            ls_b(row) = *mbits8;

            tvImagePtr++;
            mbits8++;
            row++;
        }
    }
    // Solve...
    Eigen::VectorXf ls_x = (ls_A).jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(ls_b);

    for (y = 0; y < sh; y++)
    {
        tvImagePtr = ((float **)tvFinalImage->data)[y];
        for (x = 0; x < sw; x++)
        {
            *tvImagePtr = ls_x(0) + (*tvImagePtr) * ls_x(1);
            tvImagePtr++;
        }
    }

    // Remap due to out of range values and scale
    double minIIH = 10000, maxIIH = -10000, minIIHc, maxIIHc, range1, range2;

    for (y = 0; y < sh; y++)
    {
        tvImagePtr = ((float **)tvFinalImage->data)[y];
        for (x = 0; x < sw; x++)
        {
            if ((*tvImagePtr) > maxIIH)
                maxIIH = (*tvImagePtr);
            if ((*tvImagePtr) < minIIH)
                minIIH = (*tvImagePtr);
            tvImagePtr++;
        }
    }
    minIIHc = IBP_clamp(0., minIIH, 255.);
    maxIIHc = IBP_clamp(0., maxIIH, 255.);
    range1 = maxIIH - minIIH;
    range2 = maxIIHc - minIIHc;
    for (y = 0; y < sh; y++)
    {
        mbits8 = mInitial.ptr(y);
        tvImagePtr = ((float **)tvFinalImage->data)[y];
        for (x = 0; x < sw; x++)
        {
            *mbits8 = round((minIIHc + ((*tvImagePtr) - minIIH) * range2 / range1));
            mean += *mbits8;
            mbits8++;
            tvImagePtr++;
        }
    }
    mean /= totalPixels;

    im_free(tvFinalImage);

    // ----------------------------------
    // End of Tina Vision Bias Correction
    // ----------------------------------

    // Resample
    if (w != sw || h != sh)
        cv::resize(mInitial, mlchannel, cv::Size(w, h), 0, 0, cv::INTER_CUBIC);
    else
        mlchannel = mInitial;

    // Make output image
    if (mOutputMode == CorrectedImageMode1)
    {
        // Divide lightness channel
        for (y = 0; y < h; y++)
        {
            bitsHSLsl = bitsHSL + y * w;
            mbits8 = mlchannel.ptr(y);
            for (x = 0; x < w; x++)
            {
                bitsHSLsl->l = IBP_clamp(0, lut02[bitsHSLsl->l][IBP_clamp(1, *mbits8, 255)] * mean / 255, 255);
                bitsHSLsl++;
                mbits8++;
            }
        }
    }
    else if (mOutputMode == CorrectedImageMode2)
    {
        // Divide lightness channel
        for (y = 0; y < h; y++)
        {
            bitsHSLsl = bitsHSL + y * w;
            mbits8 = mlchannel.ptr(y);
            for (x = 0; x < w; x++)
            {
                bitsHSLsl->l = IBP_clamp(0, lut02[bitsHSLsl->l][IBP_clamp(1, *mbits8, 255)], 255);
                bitsHSLsl++;
                mbits8++;
            }
        }
    }
    else
    {
        for (y = 0; y < h; y++)
        {
            bitsHSLsl = bitsHSL + y * w;
            mbits8 = mlchannel.ptr(y);
            for (x = 0; x < w; x++)
            {
                bitsHSLsl->h = bitsHSLsl->s = 0;
                bitsHSLsl->l = *mbits8;
                bitsHSLsl++;
                mbits8++;
            }
        }
    }

    // Convert to RGB
    QImage finalImage = inputImage.copy();
    convertHSLToBGR((const unsigned char *)bitsHSL, finalImage.bits(), w * h);
    free(bitsHSL);

    return finalImage;
}

bool Filter::loadParameters(QSettings &s)
{
    int refinement, smoothness;
    QString outputModeStr;
    OutputMode outputMode;
    bool ok;

    refinement = s.value("refinement", 0).toUInt(&ok);
    if (!ok || refinement > 100 || refinement < 1)
        return false;

    smoothness = s.value("smoothness", 0).toUInt(&ok);
    if (!ok || smoothness > 100 || smoothness < 1)
        return false;

    outputModeStr = s.value("outputmode", "correctedimagemode1").toString();
    if (outputModeStr == "correctedimagemode1")
        outputMode = CorrectedImageMode2;
    else if (outputModeStr == "correctedimagemode2")
        outputMode = CorrectedImageMode1;
    else if (outputModeStr == "iihcorrectionmodel")
        outputMode = IIHCorrectionModel;
    else
        return false;

    setOutputMode(outputMode);
    setRefinement(refinement);
    setSmoothness(smoothness);

    return true;
}

bool Filter::saveParameters(QSettings &s)
{
    s.setValue("refinement", mRefinement);
    s.setValue("smoothness", mSmoothness);
    s.setValue("outputmode", mOutputMode == CorrectedImageMode1 ? "correctedimagemode1" :
                             mOutputMode == CorrectedImageMode2 ? "correctedimagemode2" : "iihcorrectionmodel");
    return true;
}

QWidget *Filter::widget(QWidget *parent)
{
    FilterWidget * fw = new FilterWidget(parent);
    fw->setRefinement(mRefinement);
    fw->setSmoothness(mSmoothness);
    fw->setOutputMode(mOutputMode);
    connect(this, SIGNAL(refinementChanged(int)), fw, SLOT(setRefinement(int)));
    connect(this, SIGNAL(smoothnessChanged(int)), fw, SLOT(setSmoothness(int)));
    connect(this, SIGNAL(outputModeChanged(Filter::OutputMode)), fw, SLOT(setOutputMode(Filter::OutputMode)));
    connect(fw, SIGNAL(refinementChanged(int)), this, SLOT(setRefinement(int)));
    connect(fw, SIGNAL(smoothnessChanged(int)), this, SLOT(setSmoothness(int)));
    connect(fw, SIGNAL(outputModeChanged(Filter::OutputMode)), this, SLOT(setOutputMode(Filter::OutputMode)));
    return fw;
}

void Filter::setRefinement(int v)
{
    if (v == mRefinement)
        return;
    mRefinement = v;
    emit refinementChanged(v);
    emit parametersChanged();
}

void Filter::setSmoothness(int v)
{
    if (v == mSmoothness)
        return;
    mSmoothness = v;
    emit smoothnessChanged(v);
    emit parametersChanged();
}

void Filter::setOutputMode(Filter::OutputMode om)
{
    if (om == mOutputMode)
        return;
    mOutputMode = om;
    emit outputModeChanged(om);
    emit parametersChanged();
}


#include <boost/geometry/formulas/vertex_longitude.hpp>


#include "mtao/opengl/Window.h"
#include <iostream>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#include <Eigen/Geometry>
#pragma GCC diagnostic pop
#include "imgui.h"
#include <memory>
#include <algorithm>
#include "mtao/geometry/mesh/boundary_facets.h"
#include "mtao/geometry/mesh/read_obj.hpp"
#include "mtao/geometry/bounding_box.hpp"
#include "mtao/opengl/drawables.h"
#include <mtao/types.h>
#include <Corrade/Containers/Array.h>
#include <Corrade/Containers/ArrayView.h>

#include <Magnum/Shaders/VertexColor.h>
#include <Magnum/GL/Renderer.h>
#include <Corrade/Utility/Arguments.h>
#include <mtao/opengl/objects/grid.h>
#include <mtao/geometry/grid/grid.h>

#include <mtao/eigen/stack.h>
#include <memory>
#include <algorithm>

#include <mtao/eigen_utils.h>
#include<Eigen/IterativeLinearSolvers>
using namespace Magnum::Math::Literals;

using namespace mtao::opengl;

class MeshViewer: public mtao::opengl::Window2 {
    public:
        enum class Mode: int { Smoothing, LSReinitialization };
        Mode mode = Mode::LSReinitialization;

        float permeability = 100.0;
        float timestep = 1000.0;
        bool animate = false;
        using Vec = mtao::VectorX<GLfloat>;
        using Vec2 = mtao::Vec2f;
        Vec data;
        Vec data_original;
        Vec dx;
        Vec dy;
        Vec signs;
        Eigen::SparseMatrix<float> L;

        std::array<int,2> N{{20,20}};
        int& NI=N[0];
        int& NJ=N[1];

        Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>> solver;

        MeshViewer(const Arguments& args): Window2(args) {

            edge_drawable = new mtao::opengl::Drawable<Magnum::Shaders::Flat2D>{grid,_flat_shader, drawables()};
            edge_drawable->activate_triangles({});
            edge_drawable->activate_edges();
            //face_drawable = new mtao::opengl::Drawable<Magnum::Shaders::VertexColor2D>{grid,_vcolor_shader, drawables()};
            grid.setParent(&root());
            cursor_mesh.setParent(&scene());
            cursor_drawable = new mtao::opengl::Drawable<Magnum::Shaders::Flat2D>{cursor_mesh,_flat_shader, drawables()};
            cursor_mesh.setVertexBuffer(mtao::Vec2f::Zero().eval());
            cursor_drawable->data().color = 0xffffff_rgbf;
            visible_grid.setParent(&scene());
            visible_drawable = new mtao::opengl::Drawable<Magnum::Shaders::Flat2D>{visible_grid,_flat_shader, drawables()};
            visible_drawable->data().color = 0xff0000_rgbf;
            visible_drawable->deactivate();
            visible_drawable->activate_edges();
            update();
        }
        void gui() override {}
        void update() {
            Eigen::AlignedBox<float,2> bbox;
            bbox.min().setConstant(-1);
            bbox.max().setConstant(1);
            auto g = mtao::geometry::grid::Grid2f::from_bbox
                (bbox, std::array<int,2>{{10,10}});

            grid.set(g);
            cursor_drawable->deactivate();
            cursor_drawable->activate_points();
            {
                auto g = mtao::geometry::grid::Grid2f(std::array<int,2>{{2,2}});

                //auto V = g.vertices();
                //mtao::RowVectorX<float> C = V.colwise()
                visible_grid.set(g);
            }

        }
        void draw() override {
            Magnum::GL::Renderer::disable(Magnum::GL::Renderer::Feature::FaceCulling);
            Magnum::GL::Renderer::setPointSize(10.);
            Window2::draw();
        }
        void mouseMoveEvent(MouseMoveEvent& event) override{
            Window2::mouseMoveEvent(event);
            {
                auto cursor = localPosition(event.position());
                cursor_mesh.setVertexBuffer( cursor);;

            }
        }

    private:
        Magnum::Shaders::Flat2D _flat_shader;
        Magnum::Shaders::VertexColor2D _vcolor_shader;
        mtao::opengl::objects::Grid<2> grid;
        mtao::opengl::objects::Mesh<2> cursor_mesh;
        mtao::opengl::objects::Grid<2> visible_grid;
        mtao::opengl::Drawable<Magnum::Shaders::Flat2D>* edge_drawable = nullptr;
        mtao::opengl::Drawable<Magnum::Shaders::VertexColor2D>* face_drawable = nullptr;
        mtao::opengl::Drawable<Magnum::Shaders::Flat2D>* cursor_drawable = nullptr;
        mtao::opengl::Drawable<Magnum::Shaders::Flat2D>* visible_drawable = nullptr;

};

MAGNUM_APPLICATION_MAIN(MeshViewer)