// Copyright 2019 ETH Zürich, Thomas Schöps
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include <complex>
#include <iostream>

#include <Eigen/Geometry>
#include <gtest/gtest.h>
#include <libvis/eigen.h>
#include <libvis/image_display.h>
#include <libvis/libvis.h>
#include <libvis/logging.h>

#include "badslam/direct_ba.h"
#include "badslam/kernels.h"
#include "badslam/util.cuh"
#include "badslam/render_window.h"
#include "badslam/cuda_depth_processing.cuh"

using namespace vis;

// LambertW function (and helper functions to compute it) from
// https://github.com/IstvanMezo/LambertW-function . It is required for this
// test to invert the depth distortion function.

// z * exp(z)
complex<double> zexpz(complex<double> z) {
  return z * exp(z);
}

// The derivative of z * exp(z) = exp(z) + z * exp(z)
complex<double> zexpz_d(complex<double> z) {
  return exp(z) + z * exp(z);
}

// The second derivative of z * exp(z) = 2. * exp(z) + z * exp(z)
complex<double> zexpz_dd(complex<double> z) {
  return 2. * exp(z) + z * exp(z);
}

// Determine the initial point for the root finding
complex<double> LambertWInitPoint(complex<double> z, int k) {
  const double pi{ 3.14159265358979323846 };
  const double e{ 2.71828182845904523536 };
  complex<double> I{0, 1};
  complex<double> two_pi_k_I{ 0., 2. * pi * k };
  complex<double> ip{ log(z) + two_pi_k_I - log(log(z) + two_pi_k_I) };// initial point coming from the general asymptotic approximation
  complex<double> p{ sqrt( 2. * (e * z + 1.) ) };// used when we are close to the branch cut around zero and when k=0,-1

  if (abs(z - (-exp(-1.))) <= 1.) { //we are close to the branch cut, the initial point must be chosen carefully
    if (k == 0) ip = -1. + p - 1./3. * pow(p, 2) + 11./72. * pow(p, 3);
    if (k == 1 && z.imag() < 0.) ip = -1. - p - 1./3. * pow(p, 2) - 11. / 72. * pow(p, 3);
    if (k == -1 && z.imag() > 0.) ip = -1. - p - 1./3. * pow(p, 2) - 11./72. * pow(p, 3);
  }

  if (k == 0 && abs(z - .5) <= .5) ip = (0.35173371 * (0.1237166 + 7.061302897 * z)) / (2. + 0.827184 * (1. + 2. * z));// (1,1) Pade approximant for W(0,a)

  if (k == -1 && abs(z - .5) <= .5) ip = -(((2.2591588985 +
    4.22096*I) * ((-14.073271 - 33.767687754*I) * z - (12.7127 -
    19.071643*I) * (1. + 2.*z))) / (2. - (17.23103 - 10.629721*I) * (1. + 2.*z)));// (1,1) Pade approximant for W(-1,a)

  return ip;
}

complex<double> LambertW(complex<double> z, int k = 0) {
  // For some particular z and k W(z,k) has simple value:
  if (z == 0.) return (k == 0) ? 0. : -INFINITY;
  if (z == -exp(-1.) && (k == 0 || k == -1)) return -1.;
  if (z == exp(1.) && k == 0) return 1.;

  // Halley method begins
  complex<double> w{ LambertWInitPoint(z, k) }, wprev{ LambertWInitPoint(z, k) }; // intermediate values in the Halley method
  const unsigned int maxiter = 30; // max number of iterations. This eliminates improbable infinite loops
  unsigned int iter = 0; // iteration counter
  double prec = 1.E-30; // difference threshold between the last two iteration results (or the iter number of iterations is taken)

  do {
    wprev = w;
    w -= 2.*((zexpz(w) - z) * zexpz_d(w)) /
      (2.*pow(zexpz_d(w),2) - (zexpz(w) - z)*zexpz_dd(w));
    iter++;
  } while ((abs(w - wprev) > prec) && iter < maxiter);
  return w;
}

// Helper function to render a set of planes into a depth and normal image.
void RenderPlanes(
    const SE3f& global_tr_frame,
    int plane_count,
    Hyperplane<float, 3>* planes,
    float true_a,
    float true_cfactor,
    float raw_to_float_depth,
    const PinholeCamera4f& camera,
    Image<float>* plane_depth_image,
    Image<u16>* depth_image) {
  const int width = plane_depth_image->width();
  const int height = plane_depth_image->height();

  const Vec3f& global_t_frame = global_tr_frame.translation();
  const Mat3f global_r_frame = global_tr_frame.rotationMatrix();

  plane_depth_image->SetTo(0.f);
  depth_image->SetTo(static_cast<u16>(numeric_limits<u16>::max()));

  for (int plane_index = 0; plane_index < plane_count; ++ plane_index) {
    for (int y = 0; y < height; ++ y) {
      for (int x = 0; x < width; ++ x) {
        Vec3f frame_direction = camera.UnprojectFromPixelCenterConv(Vec2f(x, y));
        ASSERT_EQ(frame_direction.z(), 1.0f);
        ParametrizedLine<float, 3> ray(global_t_frame, global_r_frame * frame_direction);
        float z = ray.intersectionParameter(planes[plane_index]);
        float previous_measurement = (*plane_depth_image)(x, y);
        if (z > 0 && (previous_measurement == 0 || z < previous_measurement)) {
          (*plane_depth_image)(x, y) = z;
        }
      }
    }
  }

  // Convert raw to distorted depth
  for (int y = 1; y < height - 1; ++ y) {
    for (int x = 1; x < width - 1; ++ x) {
      float true_depth = (*plane_depth_image)(x, y);
      if (true_depth == 0) {
        (*depth_image)(x, y) = numeric_limits<u16>::max();
      } else if (true_a == 0 && true_cfactor == 0) {
        (*depth_image)(x, y) = std::min<u32>(numeric_limits<u16>::max(), true_depth / raw_to_float_depth + 0.5f);
      } else {
        float distorted_depth = 1.0 / ((true_a + true_depth * LambertW(complex<double>(-true_a * true_cfactor * exp(- true_a / true_depth))).real()) / (true_a * true_depth));
        (*depth_image)(x, y) = std::min<u32>(numeric_limits<u16>::max(), distorted_depth / raw_to_float_depth + 0.5f);
      }
    }
  }

  // Uncomment to debug:
//   static ImageDisplay debug_display;
//   debug_display.Update(*depth_image, "debug: depth map", static_cast<u16>(0.f), static_cast<u16>(3.0f / raw_to_float_depth));
//   std::getchar();
}

// Intrinsics optimization test. Creates a scene consisting of
// a set of planes, then renders some keyframes from this geometry and distorts
// their depth images. Applies bundle adjustment and tests whether this
// distortion can be compensated.
//
// It should be straightforward to use this test code as a template to apply
// direct bundle adjustment on a set of images. Simply load the depth and color
// images from files instead of generating them, and supply initial estimates
// for the keyframe poses.
static void TestDepthDeformationOptimizationWithGeometricResidual(bool use_pcg) {
  srand(0);

  // Initialize the camera (here we use the same camera intrinsics for the depth
  // and color camera).
  constexpr int width = 640;
  constexpr int height = 480;
  const float camera_parameters[4] = {0.5f * height, 0.5f * height, 0.5f * width - 0.5f, 0.5f * height - 0.5f};
  PinholeCamera4f camera(width, height, camera_parameters);

  // Initialize a CUDA stream.
  cudaStream_t stream;
  cudaStreamCreate(&stream);

  // Optional: Create a render window for debugging.
  constexpr bool show_visualization = false;

  shared_ptr<BadSlamRenderWindow> render_window;  // render window callbacks
  shared_ptr<RenderWindow> render_window_ui;  // render window UI object
  OpenGLContext opengl_context;  // context for the main thread
  OpenGLContext opengl_context_2;  // context for the BA thread
  OpenGLContext no_opengl_context;  // stores the original (non-existing) context

  if (show_visualization) {
    float splat_half_extent_in_pixels = 3.0f;
    int render_window_default_width = 1280;
    int render_window_default_height = 720;

    // Warning: do not use make_shared() here to allocate render_window, since
    // it might ignore the class' aligned allocator, leading to potential
    // crashes in Eigen operations later on render_window's members.
    render_window = shared_ptr<BadSlamRenderWindow>(
        new BadSlamRenderWindow(splat_half_extent_in_pixels));
    render_window_ui = RenderWindow::CreateWindow(
        "BAD SLAM Test", render_window_default_width, render_window_default_height,
        RenderWindow::API::kOpenGL, render_window);
    render_window->InitializeForCUDAInterop(
        /*max_surfel_count*/ 1000 * 1000,
        &opengl_context,
        &opengl_context_2,
        camera);
    SwitchOpenGLContext(opengl_context, &no_opengl_context);
  }

  // Initialize DirectBA
  constexpr float raw_to_float_depth = 1.f / 1000;
  DirectBA direct_ba(
      /*max_surfel_count*/ 1000 * 1000,
      raw_to_float_depth,
      /*baseline_fx*/ 40,
      /*sparse_surfel_cell_size*/ 2.f,
      /*surfel_merge_dist_factor*/ 0.8f,
      /*min_observation_count_while_bootstrapping_1*/ 2,
      /*min_observation_count_while_bootstrapping_2*/ 2,
      /*min_observation_count*/ 2,
      /*color_camera_initial_estimate*/ camera,
      /*depth_camera_initial_estimate*/ camera,
      /*pyramid_level_for_color*/ 0,
      /*use_depth_residuals*/ true,
      /*use_descriptor_residuals*/ false,  // NOTE: disabling photometric residuals!
      render_window,
      /*global_T_anchor_frame*/ SE3f());

  if (show_visualization) {
    render_window->SetDirectBA(&direct_ba);
  }

  // Create a known depth map with a number of planes and known distortion
  constexpr float true_a = 0.03f;
  constexpr float true_cfactor = 0.005f;

  // Set initial values for the optimization
  direct_ba.a() = 0;
  direct_ba.cfactor_buffer()->Clear(0, stream);

  // Set the first keyframe's pose
  Matrix<float, 6, 1> global_tr_frame_log;
  global_tr_frame_log << 0.01f, 0.02f, 0.03f, 0.004f, 0.005f, 0.006f;
  SE3f global_tr_frame_0 = SE3f::exp(global_tr_frame_log);

  // Generate test geometry consisting of some random planes
  constexpr int kPlaneCount = 20;
  Hyperplane<float, 3> planes[kPlaneCount];

  for (int plane_index = 0; plane_index < kPlaneCount; ++ plane_index) {
    Vec3f plane_normal = Vec3f::Random();
    plane_normal.z() = -1.f;
    plane_normal.normalize();
    planes[plane_index] = Hyperplane<float, 3>(plane_normal, 2.5f);
  }

  // Allocate buffers for rendering the planes into
  Image<float> plane_depth_image(width, height);
  Image<u16> depth_image(width, height);

  // Use a homogeneous color image (descriptor residuals are disabled)
  Image<Vec3u8> color_image(width, height);
  for (int y = 0; y < height; ++ y) {
    for (int x = 0; x < width; ++ x) {
      color_image(x, y) = Vec3u8::Zero();
    }
  }

  // Create keyframes observing the surface from different distances
  constexpr int kNumKeyframes = 12;
  RGBDVideo<Vec3u8, u16> rgbd_video;

  SE3f last_frame_tr_global = SE3f();
  for (int i = 0; i < kNumKeyframes; ++ i) {
    Matrix<float, 6, 1> frame_0_T_frame_log;
    frame_0_T_frame_log <<
        3.0f * ((rand() % 200) / 200.f - 0.5f),
        3.0f * ((rand() % 200) / 200.f - 0.5f),
        3.0f * ((rand() % 200) / 200.f - 0.5f),
        3.5f * (((rand() % 200) - 100) / 500.f),
        3.5f * (((rand() % 200) - 100) / 500.f),
        3.5f * (((rand() % 200) - 100) / 500.f);
    SE3f frame_0_T_frame = SE3f::exp(frame_0_T_frame_log);
    SE3f global_tr_frame = global_tr_frame_0 * frame_0_T_frame;

    RenderPlanes(global_tr_frame, kPlaneCount, planes, true_a, true_cfactor,
                 raw_to_float_depth, camera, &plane_depth_image, &depth_image);

    shared_ptr<Keyframe> keyframe(new Keyframe(
        stream,
        /*frame_index*/ i,
        direct_ba.depth_params(),
        direct_ba.depth_camera(),
        depth_image,
        color_image,  // NOTE: Colors are not reprojected, but this does not matter since they are not used
        global_tr_frame));
    direct_ba.AddKeyframe(keyframe);
    last_frame_tr_global = keyframe->frame_T_global();
  }

  // The 'c' factor is tested on a random pixel
  constexpr int kCFactorTestX = 50;
  constexpr int kCFactorTestY = 50;
  Image<float> cfactor_image(direct_ba.cfactor_buffer()->width(),
                             direct_ba.cfactor_buffer()->height());
  ASSERT_LT(kCFactorTestX, cfactor_image.width());
  ASSERT_LT(kCFactorTestY, cfactor_image.height());

  // Optimize intrinsics
  for (int i = 0; i < (use_pcg ? 20 : 400); ++ i) {
    direct_ba.BundleAdjustment(
        stream,
        /*optimize_depth_intrinsics*/ i != 0,
        /*optimize_color_intrinsics*/ false,
        /*do_surfel_updates*/ true,
        /*optimize_poses*/ false,
        /*optimize_geometry*/ true,
        /*min_iterations*/ 1,
        /*max_iterations*/ 10,
        use_pcg,
        /*active_keyframe_window_start*/ 0,
        /*active_keyframe_window_end*/ direct_ba.keyframes().size() - 1,
        /*increase_ba_iteration_count*/ i != 0);

    if (show_visualization) {
      std::getchar();
    }

    // Uncomment this to print the current state at each iteration:
//     LOG(INFO) << "estimated a: " << direct_ba.a() << ", true a: " << true_a;
//     direct_ba.cfactor_buffer()->DownloadAsync(0, &cfactor_image);
//     LOG(INFO) << "estimated cfactor: " << cfactor_image(kCFactorTestX, kCFactorTestY) << ", true cfactor: " << true_cfactor;
  }

  EXPECT_NEAR(true_a, direct_ba.a(), 1e-2f);  // NOTE: loose threshold since this seems to converge extremely slowly (and probably does not matter much)
  direct_ba.cfactor_buffer()->DownloadAsync(0, &cfactor_image);
  EXPECT_NEAR(true_cfactor, cfactor_image(kCFactorTestX, kCFactorTestY), 1e-3f);

  if (show_visualization) {
    SwitchOpenGLContext(no_opengl_context);
    opengl_context.Deinitialize();
    opengl_context_2.Deinitialize();
  }

  cudaStreamDestroy(stream);
}

TEST(Optimization, AlternatingDepthDeformationOptimizationWithGeometricResidual) {
  TestDepthDeformationOptimizationWithGeometricResidual(/*use_pcg*/ false);
}

TEST(Optimization, PCGDepthDeformationOptimizationWithGeometricResidual) {
  TestDepthDeformationOptimizationWithGeometricResidual(/*use_pcg*/ true);
}

// This test specifically tests for pinhole camera intrinsics (fx, fy, cx, cy)
// optimization instead of depth deformation optimization.
static void TestIntrinsicsOptimizationWithGeometricResidual(bool use_pcg) {
  srand(0);

  // Initialize the camera (here we use the same camera intrinsics for the depth
  // and color camera).
  constexpr int width = 640;
  constexpr int height = 480;
  const float camera_parameters[4] = {0.5f * height, 0.45f * height, 0.5f * width - 0.5f, 0.5f * height - 0.5f};
  PinholeCamera4f camera(width, height, camera_parameters);

  // Initialize a CUDA stream.
  cudaStream_t stream;
  cudaStreamCreate(&stream);

  // Optional: Create a render window for debugging.
  constexpr bool show_visualization = false;

  shared_ptr<BadSlamRenderWindow> render_window;  // render window callbacks
  shared_ptr<RenderWindow> render_window_ui;  // render window UI object
  OpenGLContext opengl_context;  // context for the main thread
  OpenGLContext opengl_context_2;  // context for the BA thread
  OpenGLContext no_opengl_context;  // stores the original (non-existing) context

  if (show_visualization) {
    float splat_half_extent_in_pixels = 3.0f;
    int render_window_default_width = 1280;
    int render_window_default_height = 720;

    // Warning: do not use make_shared() here to allocate render_window, since
    // it might ignore the class' aligned allocator, leading to potential
    // crashes in Eigen operations later on render_window's members.
    render_window = shared_ptr<BadSlamRenderWindow>(
        new BadSlamRenderWindow(splat_half_extent_in_pixels));
    render_window_ui = RenderWindow::CreateWindow(
        "BAD SLAM Test", render_window_default_width, render_window_default_height,
        RenderWindow::API::kOpenGL, render_window);
    render_window->InitializeForCUDAInterop(
        /*max_surfel_count*/ 1000 * 1000,
        &opengl_context,
        &opengl_context_2,
        camera);
    SwitchOpenGLContext(opengl_context, &no_opengl_context);
  }

  // Distort depth intrinsics
  const float distorted_camera_parameters[4] = {0.5f * height + 0.5f, 0.45f * height - 0.6f, 0.5f * width - 0.5f + 1.23f, 0.5f * height - 0.5f - 2.17f};
  PinholeCamera4f distorted_depth_camera(width, height, distorted_camera_parameters);

  // Initialize DirectBA
  constexpr float raw_to_float_depth = 1.f / 1000;
  DirectBA direct_ba(
      /*max_surfel_count*/ 1000 * 1000,
      raw_to_float_depth,
      /*baseline_fx*/ 40,
      /*sparse_surfel_cell_size*/ 2.f,
      /*surfel_merge_dist_factor*/ 0.8f,
      /*min_observation_count_while_bootstrapping_1*/ 2,
      /*min_observation_count_while_bootstrapping_2*/ 2,
      /*min_observation_count*/ 2,
      /*color_camera_initial_estimate*/ camera,
      /*depth_camera_initial_estimate*/ camera,
      /*pyramid_level_for_color*/ 0,
      /*use_depth_residuals*/ true,
      /*use_descriptor_residuals*/ false,  // NOTE: disabling photometric residuals!
      render_window,
      /*global_T_anchor_frame*/ SE3f());

  if (show_visualization) {
    render_window->SetDirectBA(&direct_ba);
  }

  // Set the first keyframe's pose
  Matrix<float, 6, 1> global_tr_frame_log;
  global_tr_frame_log << 0.01f, 0.02f, 0.03f, 0.004f, 0.005f, 0.006f;
  SE3f global_tr_frame_0 = SE3f::exp(global_tr_frame_log);

  // Generate test geometry consisting of some random planes
  constexpr int kPlaneCount = 20;
  Hyperplane<float, 3> planes[kPlaneCount];

  for (int plane_index = 0; plane_index < kPlaneCount; ++ plane_index) {
    Vec3f plane_normal = Vec3f::Random();
    plane_normal.z() = -1.f;
    plane_normal.normalize();
    planes[plane_index] = Hyperplane<float, 3>(plane_normal, 2.5f);
  }

  // Allocate buffers for rendering the planes into
  Image<float> plane_depth_image(width, height);
  Image<u16> depth_image(width, height);

  // Use a homogeneous color image (descriptor residuals are disabled)
  Image<Vec3u8> color_image(width, height);
  for (int y = 0; y < height; ++ y) {
    for (int x = 0; x < width; ++ x) {
      color_image(x, y) = Vec3u8::Zero();
    }
  }

  // Create keyframes observing the surface from different distances
  constexpr int kNumKeyframes = 3 * 12;
  RGBDVideo<Vec3u8, u16> rgbd_video;

  SE3f last_frame_tr_global = SE3f();
  for (int i = 0; i < kNumKeyframes; ++ i) {
    Matrix<float, 6, 1> frame_0_T_frame_log;
    frame_0_T_frame_log <<
        3.0f * ((rand() % 200) / 200.f - 0.5f),
        3.0f * ((rand() % 200) / 200.f - 0.5f),
        3.0f * ((rand() % 200) / 200.f - 0.5f),
        3.5f * (((rand() % 200) - 100) / 500.f),
        3.5f * (((rand() % 200) - 100) / 500.f),
        3.5f * (((rand() % 200) - 100) / 500.f);
    SE3f frame_0_T_frame = SE3f::exp(frame_0_T_frame_log);
    SE3f global_tr_frame = global_tr_frame_0 * frame_0_T_frame;

    RenderPlanes(global_tr_frame, kPlaneCount, planes, /*true_a*/ 0, /*true_cfactor*/ 0,
                 raw_to_float_depth, camera, &plane_depth_image, &depth_image);

    shared_ptr<Keyframe> keyframe(new Keyframe(
        stream,
        /*frame_index*/ i,
        direct_ba.depth_params(),
        direct_ba.depth_camera(),
        depth_image,
        color_image,
        global_tr_frame));
    direct_ba.AddKeyframe(keyframe);
    last_frame_tr_global = keyframe->frame_T_global();
  }

  for (auto& keyframe : direct_ba.keyframes()) {
    direct_ba.CreateSurfelsForKeyframe(stream, true, keyframe);
  }
  direct_ba.SetDepthCamera(distorted_depth_camera);

  // Optimize intrinsics
  // NOTE: We are testing many more iterations than necessary here since this
  //       triggered some issues where the a parameter could become strongly
  //       negative and then numerical issues destroyed the reconstruction.
  for (int i = 0; i < 100; ++ i) {
    direct_ba.BundleAdjustment(
        stream,
        /*optimize_depth_intrinsics*/ true,
        /*optimize_color_intrinsics*/ false,
        /*do_surfel_updates*/ false,
        /*optimize_poses*/ false,
        /*optimize_geometry*/ false,
        /*min_iterations*/ 1,
        /*max_iterations*/ 10,
        use_pcg,
        /*active_keyframe_window_start*/ 0,
        /*active_keyframe_window_end*/ direct_ba.keyframes().size() - 1,
        /*increase_ba_iteration_count*/ i != 0);

    if (show_visualization) {
      std::getchar();
    }

    // Uncomment this to print the current state at each iteration:
    PinholeCamera4f estimated_camera = direct_ba.depth_camera();
    LOG(INFO) << "camera_difference: " << (estimated_camera.parameters()[0] - camera.parameters()[0])
                                      << ", " << (estimated_camera.parameters()[1] - camera.parameters()[1])
                                      << ", " << (estimated_camera.parameters()[2] - camera.parameters()[2])
                                      << ", " << (estimated_camera.parameters()[3] - camera.parameters()[3]);
  }

  PinholeCamera4f estimated_camera = direct_ba.depth_camera();
  EXPECT_NEAR(camera.parameters()[0], estimated_camera.parameters()[0], 0.001);
  EXPECT_NEAR(camera.parameters()[1], estimated_camera.parameters()[1], 0.001);
  EXPECT_NEAR(camera.parameters()[2], estimated_camera.parameters()[2], 0.001);
  EXPECT_NEAR(camera.parameters()[3], estimated_camera.parameters()[3], 0.001);

  if (show_visualization) {
    SwitchOpenGLContext(no_opengl_context);
    opengl_context.Deinitialize();
    opengl_context_2.Deinitialize();
  }

  cudaStreamDestroy(stream);
}

TEST(Optimization, AlternatingIntrinsicsOptimizationWithGeometricResidual) {
  TestIntrinsicsOptimizationWithGeometricResidual(/*use_pcg*/ false);
}

TEST(Optimization, PCGIntrinsicsOptimizationWithGeometricResidual) {
  TestIntrinsicsOptimizationWithGeometricResidual(/*use_pcg*/ true);
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  TestSmartProjectionPoseFactor.cpp
 *  @brief Unit tests for ProjectionFactor Class
 *  @author Chris Beall
 *  @author Luca Carlone
 *  @author Zsolt Kira
 *  @date   Sept 2013
 */

#include "../SmartProjectionPoseFactor.h"

#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <boost/assign/std/vector.hpp>
#include <CppUnitLite/TestHarness.h>
#include <iostream>

using namespace std;
using namespace boost::assign;
using namespace gtsam;

static bool isDebugTest = false;

// make a realistic calibration matrix
static double fov = 60; // degrees
static size_t w=640,h=480;

static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480));
static boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));

static double rankTol = 1.0;
static double linThreshold = -1.0;
static bool manageDegeneracy = true;
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(2));

// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;

// tests data
static Symbol x1('X',  1);
static Symbol x2('X',  2);
static Symbol x3('X',  3);

static Key poseKey1(x1);
static Point2 measurement1(323.0, 240.0);
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));

typedef SmartProjectionPoseFactor<Pose3,Point3,Cal3_S2> SmartFactor;
typedef SmartProjectionPoseFactor<Pose3,Point3,Cal3Bundler> SmartFactorBundler;

void projectToMultipleCameras(
    SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark,
    vector<Point2>& measurements_cam){

  Point2 cam1_uv1 = cam1.project(landmark);
  Point2 cam2_uv1 = cam2.project(landmark);
  Point2 cam3_uv1 = cam3.project(landmark);
  measurements_cam.push_back(cam1_uv1);
  measurements_cam.push_back(cam2_uv1);
  measurements_cam.push_back(cam3_uv1);
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor) {
  SmartFactor::shared_ptr factor1(new SmartFactor());
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor2) {
  SmartFactor factor1(rankTol, linThreshold);
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor3) {
  SmartFactor::shared_ptr factor1(new SmartFactor());
  factor1->add(measurement1, poseKey1, model, K);
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor4) {
  SmartFactor factor1(rankTol, linThreshold);
  factor1.add(measurement1, poseKey1, model, K);
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, ConstructorWithTransform) {
  bool manageDegeneracy = true;
  bool enableEPI = false;
  SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1);
  factor1.add(measurement1, poseKey1, model, K);
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Equals ) {
  SmartFactor::shared_ptr factor1(new SmartFactor());
  factor1->add(measurement1, poseKey1, model, K);

  SmartFactor::shared_ptr factor2(new SmartFactor());
  factor2->add(measurement1, poseKey1, model, K);

  CHECK(assert_equal(*factor1, *factor2));
}

/* *************************************************************************/
TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){
  // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl;

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera level_camera(level_pose, *K2);

  // create second camera 1 meter to the right of first camera
  Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
  SimpleCamera level_camera_right(level_pose_right, *K2);

  // landmark ~5 meters infront of camera
  Point3 landmark(5, 0.5, 1.2);

  // 1. Project two landmarks into two cameras and triangulate
  Point2 level_uv = level_camera.project(landmark);
  Point2 level_uv_right = level_camera_right.project(landmark);

  Values values;
  values.insert(x1, level_pose);
  values.insert(x2, level_pose_right);

  SmartFactor factor1;
  factor1.add(level_uv, x1, model, K);
  factor1.add(level_uv_right, x2, model, K);

  double actualError = factor1.error(values);
  double expectedError = 0.0;
  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);

  SmartFactor::Cameras cameras = factor1.cameras(values);
  double actualError2 = factor1.totalReprojectionError(cameras);
  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);

  // test vector of errors
  //Vector actual = factor1.unwhitenedError(values);
  //EXPECT(assert_equal(zero(4),actual,1e-8));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, noisy ){
  // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl;

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera level_camera(level_pose, *K2);

  // create second camera 1 meter to the right of first camera
  Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
  SimpleCamera level_camera_right(level_pose_right, *K2);

  // landmark ~5 meters infront of camera
  Point3 landmark(5, 0.5, 1.2);

  // 1. Project two landmarks into two cameras and triangulate
  Point2 pixelError(0.2,0.2);
  Point2 level_uv = level_camera.project(landmark) + pixelError;
  Point2 level_uv_right = level_camera_right.project(landmark);

  Values values;
  values.insert(x1, level_pose);
  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
  values.insert(x2, level_pose_right.compose(noise_pose));

  SmartFactor::shared_ptr factor1(new SmartFactor());
  factor1->add(level_uv, x1, model, K);
  factor1->add(level_uv_right, x2, model, K);

  double actualError1= factor1->error(values);

  SmartFactor::shared_ptr factor2(new SmartFactor());
  vector<Point2> measurements;
  measurements.push_back(level_uv);
  measurements.push_back(level_uv_right);

  std::vector< SharedNoiseModel > noises;
  noises.push_back(model);
  noises.push_back(model);

  std::vector< boost::shared_ptr<Cal3_S2> > Ks;  ///< shared pointer to calibration object (one for each camera)
  Ks.push_back(K);
  Ks.push_back(K);

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);

  factor2->add(measurements, views, noises, Ks);

  double actualError2= factor2->error(values);

  DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){
  // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl;

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K2);

  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
  SimpleCamera cam2(pose2, *K2);

  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
  SimpleCamera cam3(pose3, *K2);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(3, 0, 3.0);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
  smartFactor1->add(measurements_cam1, views, model, K2);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
  smartFactor2->add(measurements_cam2, views, model, K2);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
  smartFactor3->add(measurements_cam3, views, model, K2);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));

  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  // initialize third pose with some noise, we expect it to move back to original pose3
  values.insert(x3, pose3*noise_pose);
  if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");

  LevenbergMarquardtParams params;
  if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
  if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;

  Values result;
  gttic_(SmartProjectionPoseFactor);
  LevenbergMarquardtOptimizer optimizer(graph, values, params);
  result = optimizer.optimize();
  gttoc_(SmartProjectionPoseFactor);
  tictoc_finishedIteration_();

//  GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
//  VectorValues delta = GFG->optimize();

  // result.print("results of 3 camera, 3 landmark optimization \n");
  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
  if(isDebugTest) tictoc_print_();
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){
  // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl;

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K);

  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
  SimpleCamera cam2(pose2, *K);

  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
  SimpleCamera cam3(pose3, *K);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(3, 0, 3.0);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
  smartFactor1->add(measurements_cam1, views, model, K);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
  smartFactor2->add(measurements_cam2, views, model, K);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
  smartFactor3->add(measurements_cam3, views, model, K);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));

  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  // initialize third pose with some noise, we expect it to move back to original pose3
  values.insert(x3, pose3*noise_pose);
  if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");

  LevenbergMarquardtParams params;
  if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
  if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;

  Values result;
  gttic_(SmartProjectionPoseFactor);
  LevenbergMarquardtOptimizer optimizer(graph, values, params);
  result = optimizer.optimize();
  gttoc_(SmartProjectionPoseFactor);
  tictoc_finishedIteration_();

  // result.print("results of 3 camera, 3 landmark optimization \n");
  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
  if(isDebugTest) tictoc_print_();
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, jacobianSVD ){

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K);
  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
  SimpleCamera cam2(pose2, *K);
  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
  SimpleCamera cam3(pose3, *K);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(3, 0, 3.0);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
  smartFactor1->add(measurements_cam1, views, model, K);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
  smartFactor2->add(measurements_cam2, views, model, K);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
  smartFactor3->add(measurements_cam3, views, model, K);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));

  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  values.insert(x3, pose3*noise_pose);

  LevenbergMarquardtParams params;
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, params);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, landmarkDistance ){

  double excludeLandmarksFutherThanDist = 2;

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K);
  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
  SimpleCamera cam2(pose2, *K);
  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
  SimpleCamera cam3(pose3, *K);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(3, 0, 3.0);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
  smartFactor1->add(measurements_cam1, views, model, K);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
  smartFactor2->add(measurements_cam2, views, model, K);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
  smartFactor3->add(measurements_cam3, views, model, K);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));

  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  values.insert(x3, pose3*noise_pose);

  // All factors are disabled and pose should remain where it is
  LevenbergMarquardtParams params;
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, params);
  result = optimizer.optimize();
  EXPECT(assert_equal(values.at<Pose3>(x3),result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){

  double excludeLandmarksFutherThanDist = 1e10;
  double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K);
  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
  SimpleCamera cam2(pose2, *K);
  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
  SimpleCamera cam3(pose3, *K);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(3, 0, 3.0);
  Point3 landmark4(5, -0.5, 1);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
  measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none,
      JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
  smartFactor1->add(measurements_cam1, views, model, K);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
      excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
  smartFactor2->add(measurements_cam2, views, model, K);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
      excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
  smartFactor3->add(measurements_cam3, views, model, K);

  SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
      excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
  smartFactor4->add(measurements_cam4, views, model, K);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(smartFactor4);
  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));

  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  values.insert(x3, pose3);

  // All factors are disabled and pose should remain where it is
  LevenbergMarquardtParams params;
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, params);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, jacobianQ ){

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K);
  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
  SimpleCamera cam2(pose2, *K);
  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
  SimpleCamera cam3(pose3, *K);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(3, 0, 3.0);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
  smartFactor1->add(measurements_cam1, views, model, K);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
  smartFactor2->add(measurements_cam2, views, model, K);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
  smartFactor3->add(measurements_cam3, views, model, K);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));

  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  values.insert(x3, pose3*noise_pose);

  LevenbergMarquardtParams params;
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, params);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){
  //  cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K2);

  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
  SimpleCamera cam2(pose2, *K2);

  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
  SimpleCamera cam3(pose3, *K2);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(3, 0, 3.0);

  typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
  NonlinearFactorGraph graph;

  // 1. Project three landmarks into three cameras and triangulate
  graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2));
  graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2));
  graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2));

  graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2));
  graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2));
  graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2));

  graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2));
  graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2));
  graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2));

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));

  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  values.insert(x3, pose3* noise_pose);
  values.insert(L(1), landmark1);
  values.insert(L(2), landmark2);
  values.insert(L(3), landmark3);
  if(isDebugTest)  values.at<Pose3>(x3).print("Pose3 before optimization: ");

  LevenbergMarquardtParams params;
  if(isDebugTest)  params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
  if(isDebugTest)  params.verbosity = NonlinearOptimizerParams::ERROR;
  LevenbergMarquardtOptimizer optimizer(graph, values, params);
  Values result = optimizer.optimize();

  if(isDebugTest)  result.at<Pose3>(x3).print("Pose3 after optimization: ");
  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, CheckHessian){

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K);

  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
  SimpleCamera cam2(pose2, *K);

  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
  SimpleCamera cam3(pose3, *K);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(3, 0, 3.0);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  double rankTol = 10;

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol));
  smartFactor1->add(measurements_cam1, views, model, K);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol));
  smartFactor2->add(measurements_cam2, views, model, K);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol));
  smartFactor3->add(measurements_cam3, views, model, K);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);

  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  // initialize third pose with some noise, we expect it to move back to original pose3
  values.insert(x3, pose3*noise_pose);
  if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");

  boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(values);
  boost::shared_ptr<GaussianFactor> hessianFactor2 = smartFactor2->linearize(values);
  boost::shared_ptr<GaussianFactor> hessianFactor3 = smartFactor3->linearize(values);

  Matrix CumulativeInformation = hessianFactor1->information() +  hessianFactor2->information() + hessianFactor3->information();

  boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(values);
  Matrix GraphInformation = GaussianGraph->hessian().first;

  // Check Hessian
  EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8));

  Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() +
      hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation();

  // Check Information vector
  // cout << AugInformationMatrix.size() << endl;
  Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector

  // Check Hessian
  EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
  // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl;

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K2);

  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
  SimpleCamera cam2(pose2, *K2);

  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
  SimpleCamera cam3(pose3, *K2);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);

  double rankTol = 50;
  SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
  smartFactor1->add(measurements_cam1, views, model, K2);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
  smartFactor2->add(measurements_cam2, views, model, K2);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
  Point3 positionPrior = gtsam::Point3(0,0,1);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
  graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
  graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));

  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2*noise_pose);
  // initialize third pose with some noise, we expect it to move back to original pose3
  values.insert(x3, pose3*noise_pose*noise_pose);
  if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");

  LevenbergMarquardtParams params;
  if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
  if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;

  Values result;
  gttic_(SmartProjectionPoseFactor);
  LevenbergMarquardtOptimizer optimizer(graph, values, params);
  result = optimizer.optimize();
  gttoc_(SmartProjectionPoseFactor);
  tictoc_finishedIteration_();

  // result.print("results of 3 camera, 3 landmark optimization \n");
  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
  std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl;
  // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
  if(isDebugTest) tictoc_print_();
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
  // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K);

  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
  SimpleCamera cam2(pose2, *K);

  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
  SimpleCamera cam3(pose3, *K);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(3, 0, 3.0);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  // 1. Project three landmarks into three cameras and triangulate
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  double rankTol = 10;

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
  smartFactor1->add(measurements_cam1, views, model, K);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
  smartFactor2->add(measurements_cam2, views, model, K);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
  smartFactor3->add(measurements_cam3, views, model, K);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
  Point3 positionPrior = gtsam::Point3(0,0,1);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
  graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
  graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));

  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  // initialize third pose with some noise, we expect it to move back to original pose3
  values.insert(x3, pose3*noise_pose);
  if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");

  LevenbergMarquardtParams params;
  if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
  if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;

  Values result;
  gttic_(SmartProjectionPoseFactor);
  LevenbergMarquardtOptimizer optimizer(graph, values, params);
  result = optimizer.optimize();
  gttoc_(SmartProjectionPoseFactor);
  tictoc_finishedIteration_();

  // result.print("results of 3 camera, 3 landmark optimization \n");
  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
  std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl;
  // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
  if(isDebugTest) tictoc_print_();
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, Hessian ){
  // cout << " ************************ SmartProjectionPoseFactor: Hessian **********************" << endl;

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K2);

  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
  SimpleCamera cam2(pose2, *K2);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);

  // 1. Project three landmarks into three cameras and triangulate
  Point2 cam1_uv1 = cam1.project(landmark1);
  Point2 cam2_uv1 = cam2.project(landmark1);
  vector<Point2> measurements_cam1;
  measurements_cam1.push_back(cam1_uv1);
  measurements_cam1.push_back(cam2_uv1);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
  smartFactor1->add(measurements_cam1,views, model, K2);

  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);

  boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor1->linearize(values);
  if(isDebugTest) hessianFactor->print("Hessian factor \n");

  // compute triangulation from linearization point
  // compute reprojection errors (sum squared)
  // compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
  // check that it is correctly scaled when using noiseProjection = [1/4  0; 0 1/4]
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, HessianWithRotation ){
  // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl;

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K);

  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
  SimpleCamera cam2(pose2, *K);

  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
  SimpleCamera cam3(pose3, *K);

  Point3 landmark1(5, 0.5, 1.2);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);

  SmartFactor::shared_ptr smartFactorInstance(new SmartFactor());
  smartFactorInstance->add(measurements_cam1, views, model, K);

  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  values.insert(x3, pose3);

  boost::shared_ptr<GaussianFactor> hessianFactor = smartFactorInstance->linearize(values);
  // hessianFactor->print("Hessian factor \n");

  Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0));

  Values rotValues;
  rotValues.insert(x1, poseDrift.compose(pose1));
  rotValues.insert(x2, poseDrift.compose(pose2));
  rotValues.insert(x3, poseDrift.compose(pose3));

  boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactorInstance->linearize(rotValues);
  // hessianFactorRot->print("Hessian factor \n");

  // Hessian is invariant to rotations in the nondegenerate case
  EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) );

  Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5));

  Values tranValues;
  tranValues.insert(x1, poseDrift2.compose(pose1));
  tranValues.insert(x2, poseDrift2.compose(pose2));
  tranValues.insert(x3, poseDrift2.compose(pose3));

  boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactorInstance->linearize(tranValues);

  // Hessian is invariant to rotations and translations in the nondegenerate case
  EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) );
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){
  // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl;

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  SimpleCamera cam1(pose1, *K2);

  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0));
  SimpleCamera cam2(pose2, *K2);

  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0));
  SimpleCamera cam3(pose3, *K2);

  Point3 landmark1(5, 0.5, 1.2);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);

  SmartFactor::shared_ptr smartFactor(new SmartFactor());
  smartFactor->add(measurements_cam1, views, model, K2);

  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  values.insert(x3, pose3);

  boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(values);
  if(isDebugTest)  hessianFactor->print("Hessian factor \n");

  Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0));

  Values rotValues;
  rotValues.insert(x1, poseDrift.compose(pose1));
  rotValues.insert(x2, poseDrift.compose(pose2));
  rotValues.insert(x3, poseDrift.compose(pose3));

  boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(rotValues);
  if(isDebugTest)  hessianFactorRot->print("Hessian factor \n");

  // Hessian is invariant to rotations in the nondegenerate case
  EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) );

  Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5));

  Values tranValues;
  tranValues.insert(x1, poseDrift2.compose(pose1));
  tranValues.insert(x2, poseDrift2.compose(pose2));
  tranValues.insert(x3, poseDrift2.compose(pose3));

  boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactor->linearize(tranValues);

  // Hessian is invariant to rotations and translations in the nondegenerate case
  EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) );
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
  SmartProjectionPoseFactor<Pose3,Point3,Cal3Bundler> factor1(rankTol, linThreshold);
  boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
  factor1.add(measurement1, poseKey1, model, Kbundler);
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, Cal3Bundler ){
  // cout << " ************************ SmartProjectionPoseFactor: Cal3Bundler **********************" << endl;

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  PinholeCamera<Cal3Bundler> cam1(pose1, *Kbundler);

  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
  PinholeCamera<Cal3Bundler> cam2(pose2, *Kbundler);

  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
  PinholeCamera<Cal3Bundler> cam3(pose3, *Kbundler);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(3, 0, 3.0);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  // 1. Project three landmarks into three cameras and triangulate
  Point2 cam1_uv1 = cam1.project(landmark1);
  Point2 cam2_uv1 = cam2.project(landmark1);
  Point2 cam3_uv1 = cam3.project(landmark1);
  measurements_cam1.push_back(cam1_uv1);
  measurements_cam1.push_back(cam2_uv1);
  measurements_cam1.push_back(cam3_uv1);

  Point2 cam1_uv2 = cam1.project(landmark2);
  Point2 cam2_uv2 = cam2.project(landmark2);
  Point2 cam3_uv2 = cam3.project(landmark2);
  measurements_cam2.push_back(cam1_uv2);
  measurements_cam2.push_back(cam2_uv2);
  measurements_cam2.push_back(cam3_uv2);

  Point2 cam1_uv3 = cam1.project(landmark3);
  Point2 cam2_uv3 = cam2.project(landmark3);
  Point2 cam3_uv3 = cam3.project(landmark3);
  measurements_cam3.push_back(cam1_uv3);
  measurements_cam3.push_back(cam2_uv3);
  measurements_cam3.push_back(cam3_uv3);

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler());
  smartFactor1->add(measurements_cam1, views, model, Kbundler);

  SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler());
  smartFactor2->add(measurements_cam2, views, model, Kbundler);

  SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler());
  smartFactor3->add(measurements_cam3, views, model, Kbundler);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));

  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  // initialize third pose with some noise, we expect it to move back to original pose3
  values.insert(x3, pose3*noise_pose);
  if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");

  LevenbergMarquardtParams params;
  if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
  if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;

  Values result;
  gttic_(SmartProjectionPoseFactor);
  LevenbergMarquardtOptimizer optimizer(graph, values, params);
  result = optimizer.optimize();
  gttoc_(SmartProjectionPoseFactor);
  tictoc_finishedIteration_();

  // result.print("results of 3 camera, 3 landmark optimization \n");
  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
  EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-6));
  if(isDebugTest) tictoc_print_();
 }

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly  ){

  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
  PinholeCamera<Cal3Bundler> cam1(pose1, *Kbundler);

  // create second camera 1 meter to the right of first camera
  Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
  PinholeCamera<Cal3Bundler> cam2(pose2, *Kbundler);

  // create third camera 1 meter above the first camera
  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
  PinholeCamera<Cal3Bundler> cam3(pose3, *Kbundler);

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(3, 0, 3.0);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  // 1. Project three landmarks into three cameras and triangulate
  Point2 cam1_uv1 = cam1.project(landmark1);
  Point2 cam2_uv1 = cam2.project(landmark1);
  Point2 cam3_uv1 = cam3.project(landmark1);
  measurements_cam1.push_back(cam1_uv1);
  measurements_cam1.push_back(cam2_uv1);
  measurements_cam1.push_back(cam3_uv1);

  Point2 cam1_uv2 = cam1.project(landmark2);
  Point2 cam2_uv2 = cam2.project(landmark2);
  Point2 cam3_uv2 = cam3.project(landmark2);
  measurements_cam2.push_back(cam1_uv2);
  measurements_cam2.push_back(cam2_uv2);
  measurements_cam2.push_back(cam3_uv2);

  Point2 cam1_uv3 = cam1.project(landmark3);
  Point2 cam2_uv3 = cam2.project(landmark3);
  Point2 cam3_uv3 = cam3.project(landmark3);
  measurements_cam3.push_back(cam1_uv3);
  measurements_cam3.push_back(cam2_uv3);
  measurements_cam3.push_back(cam3_uv3);

  double rankTol = 10;

  SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy));
  smartFactor1->add(measurements_cam1, views, model, Kbundler);

  SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy));
  smartFactor2->add(measurements_cam2, views, model, Kbundler);

  SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy));
  smartFactor3->add(measurements_cam3, views, model, Kbundler);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
  Point3 positionPrior = gtsam::Point3(0,0,1);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
  graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
  graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));

  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
  Values values;
  values.insert(x1, pose1);
  values.insert(x2, pose2);
  // initialize third pose with some noise, we expect it to move back to original pose3
  values.insert(x3, pose3*noise_pose);
  if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");

  LevenbergMarquardtParams params;
  if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
  if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;

  Values result;
  gttic_(SmartProjectionPoseFactor);
  LevenbergMarquardtOptimizer optimizer(graph, values, params);
  result = optimizer.optimize();
  gttoc_(SmartProjectionPoseFactor);
  tictoc_finishedIteration_();

  // result.print("results of 3 camera, 3 landmark optimization \n");
  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
  std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl;
  // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
  if(isDebugTest) tictoc_print_();
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8
// test_complex.cpp

// (C) Copyright 2005 Matthias Troyer .
// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// should pass compilation and execution

#include <fstream>

#include <cstddef> // NULL
#include <cstdlib> // rand
#include <cstdio> // remove
#include <boost/config.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/math/special_functions/next.hpp>

#if defined(BOOST_NO_STDC_NAMESPACE)
#include <boost/limits.hpp>
namespace std{
    using ::rand;
    using ::remove;
    #if BOOST_WORKAROUND(BOOST_MSVC, >= 1400) && !defined(UNDER_CE)
        using ::numeric_limits;
    #endif
}
#endif

#include "test_tools.hpp"

#include <boost/preprocessor/stringize.hpp>
#include BOOST_PP_STRINGIZE(BOOST_ARCHIVE_TEST)

#include <boost/serialization/complex.hpp>

#include <iostream>

int test_main( int /* argc */, char* /* argv */[] )
{
    const char * testfile = boost::archive::tmpnam(NULL);
    BOOST_REQUIRE(NULL != testfile);

    // test array of objects
    std::complex<float> a(
        static_cast<float>(std::rand()) / static_cast<float>(std::rand()),
        static_cast<float>(std::rand()) / static_cast<float>(std::rand())
    );
    std::complex<double> b(
        static_cast<double>(std::rand()) / static_cast<double>(std::rand()),
        static_cast<double>(std::rand()) / static_cast<double>(std::rand())
    );
    {
        test_ostream os(testfile, TEST_STREAM_FLAGS);
        test_oarchive oa(os);
        oa << boost::serialization::make_nvp("afloatcomplex", a);
        oa << boost::serialization::make_nvp("adoublecomplex", b);
    }
    std::complex<float> a1;
    std::complex<double> b1;
    {
        test_istream is(testfile, TEST_STREAM_FLAGS);
        test_iarchive ia(is);
        ia >> boost::serialization::make_nvp("afloatcomplex", a1);
        ia >> boost::serialization::make_nvp("adoublecomplex", b1);
    }

    std::cerr << "a.real()-a1a.real() distance = " << std::abs( boost::math::float_distance(a.real(), a1.real())) << std::endl;
    BOOST_CHECK(std::abs(boost::math::float_distance(a.real(), a1.real())) < 2);
    std::cerr << "a.imag() - a1a.imag() distance = " << std::abs( boost::math::float_distance(a.imag(), a1.imag())) << std::endl;
    BOOST_CHECK(std::abs(boost::math::float_distance(a.imag(), a1.imag())) < 2);
    std::cerr << "b.real() - b1.real() distance = " << std::abs( boost::math::float_distance(b.real(), b1.real())) << std::endl;
    BOOST_CHECK(std::abs(boost::math::float_distance(b.real(), b1.real())) < 2);
    std::cerr << "b.imag() - b1.imag() distance = " << std::abs( boost::math::float_distance(b.imag(), b1.imag())) << std::endl;
    BOOST_CHECK(std::abs(boost::math::float_distance(b.imag(), b1.imag())) < 2);

    std::remove(testfile);
    return EXIT_SUCCESS;
}

// EOF


#pragma once

#include <Eigen/Dense>
#include <vector>

namespace pd::ecs::cmp::root
{
	// ---------------------------------------------------------------------------------------------------------
	class Desktop
	{
	public:
		Eigen::AlignedBox2i desktopSize;
		std::vector<Eigen::AlignedBox2i> screens;

		int getScreenForPosition(const Eigen::Vector2i& pos) const
		{
			int i = 0;
			for (const Eigen::AlignedBox2i& screen : screens)
			{
				if (screen.contains(pos))
					return i;

				++i;
			}

			return -1;
		}
	};
}


//
// Copyright Antoine Leblanc 2010 - 2015
// Distributed under the MIT license.
//
// http://nauths.fr
// http://github.com/nicuveo
// mailto://antoine.jp.leblanc@gmail.com
//

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Includes

#include <cstdlib>
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "nauths/mml/mml.hh"

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Macros

#define TEST_TILING(type, good_size)                            \
  {                                                             \
    Shape ref(Shape::circle(1000));                             \
                                                                \
    for (int a = 0; a < 360; a += 72)                           \
      for (int x = 1000; x <= 8000; x *= 2)                     \
        for (int y = 1000; y <= 8000; y *= 2)                   \
        {                                                       \
          double ra = (a * (2 * mml::pi()) / 360.);             \
          Tessellation tess(type, ref, Vector(x, y), ra);       \
          const std::vector<Shape>& pt = tess.pattern();        \
          const Vector& dh = tess.dh();                         \
          const Vector& dv = tess.dv();                         \
                                                                \
          std::set<Point, Less> points;                         \
                                                                \
          for (const Shape& s : pt)                             \
            for (int dx = -1; dx <= 1; ++dx)                    \
              for (int dy = -1; dy <= 1; ++dy)                  \
              {                                                 \
                Shape rs = s.moved_of(dx * dh + dy * dv);       \
                for (const Point& p : rs.polygon().points())    \
                  points.insert(p);                             \
              }                                                 \
                                                                \
          BOOST_CHECK_MESSAGE(points.size() == good_size,       \
                              "a: " << ra   << ", "             \
                              << "x: " << x    << ", "          \
                              << "y: " << y    << ": "          \
                              << points.size() << " != "        \
                              << good_size);                    \
        }                                                       \
  }                                                             \

#define TEST_LINKS(type)                                        \
  {                                                             \
    Shape ref(Shape::circle(1000));                             \
    Shape test(Shape::circle(500));                             \
    Tessellation tess(type, ref, 200);                          \
    Tessellation::iterator it;                                  \
    std::map<int, std::vector<int> > l;                         \
    unsigned n = tess.pattern().size();                         \
                                                                \
    for (it = tess.begin(false, true); it != tess.end(); ++it)  \
    {                                                           \
      for (const Tessellation::Link& nid : it.links())          \
      {                                                         \
        l[it.index()].push_back(nid.first);                     \
        l[nid.first].push_back(it.index());                     \
      }                                                         \
    }                                                           \
                                                                \
    for (it = tess.begin(false); it != tess.end(); ++it)        \
      if (mml::overlaps(test, *it))                             \
        BOOST_CHECK_MESSAGE(l[it.index()].size() ==             \
                            it->polygon().size(),               \
                            "i1: " << it.index() << ": "        \
                            "i2: " << it.index() % n << ", "    \
                            << l[it.index()].size() << " != "   \
                            << it->polygon().size());           \
  }                                                             \

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Local code

namespace
{

  typedef mml::Shape<mml::Num> Shape;
  typedef mml::Point<mml::Num> Point;
  typedef mml::Point<mml::Num> Vector;
  typedef mml::Polygon<mml::Num> Polygon;

  typedef mml::Tessellation<mml::Num> Tessellation;

  class Less
  {
    public:
      bool operator()(const Point& p1, const Point& p2)
      {
        return p1.x() == p2.x()
          ? p1.y() < p2.y()
          : p1.x() < p2.x();
      }
  };

}

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Implementation

BOOST_AUTO_TEST_SUITE(tiling)

BOOST_AUTO_TEST_CASE(TRIANGULAR_TILING)
{
  TEST_TILING(mml::tiling::TRIANGULAR, 28);
}

BOOST_AUTO_TEST_CASE(SQUARE_TILING)
{
  TEST_TILING(mml::tiling::SQUARE, 16);
}

BOOST_AUTO_TEST_CASE(HEXAGONAL_TILING)
{
  TEST_TILING(mml::tiling::HEXAGONAL, 54);
}

BOOST_AUTO_TEST_CASE(SNUB_HEXAGONAL_TILING)
{
  TEST_TILING(mml::tiling::SNUB_HEXAGONAL, 831);
}

BOOST_AUTO_TEST_CASE(REFLECTED_SNUB_HEXAGONAL_TILING)
{
  TEST_TILING(mml::tiling::REFLECTED_SNUB_HEXAGONAL, 831);
}

BOOST_AUTO_TEST_CASE(TRI_HEXAGONAL_TILING)
{
  TEST_TILING(mml::tiling::TRI_HEXAGONAL, 78);
}

BOOST_AUTO_TEST_CASE(ELONGATED_TRIANGULAR_TILING)
{
  TEST_TILING(mml::tiling::ELONGATED_TRIANGULAR, 52);
}

BOOST_AUTO_TEST_CASE(SNUB_SQUARE_TILING)
{
  TEST_TILING(mml::tiling::SNUB_SQUARE, 96);
}

BOOST_AUTO_TEST_CASE(RHOMBITRIHEXAGONAL_TILING)
{
  TEST_TILING(mml::tiling::RHOMBITRIHEXAGONAL, 137);
}

BOOST_AUTO_TEST_CASE(TRUNCATED_SQUARE_TILING)
{
  TEST_TILING(mml::tiling::TRUNCATED_SQUARE, 101);
}

BOOST_AUTO_TEST_CASE(TRUNCATED_HEXAGONAL_TILING)
{
  TEST_TILING(mml::tiling::TRUNCATED_HEXAGONAL, 149);
}

BOOST_AUTO_TEST_CASE(TRUNCATED_TRIHEXAGONAL_TILING)
{
  // FIXME check::tiling: numbers ought to be checked
  TEST_TILING(mml::tiling::TRUNCATED_TRIHEXAGONAL, 268);
}

BOOST_AUTO_TEST_CASE(TRIANGULAR_LINKS)
{
  TEST_LINKS(mml::tiling::TRIANGULAR);
}

BOOST_AUTO_TEST_CASE(SQUARE_LINKS)
{
  TEST_LINKS(mml::tiling::SQUARE);
}

BOOST_AUTO_TEST_CASE(HEXAGONAL_LINKS)
{
  TEST_LINKS(mml::tiling::HEXAGONAL);
}

BOOST_AUTO_TEST_CASE(SNUB_HEXAGONAL_LINKS)
{
  TEST_LINKS(mml::tiling::SNUB_HEXAGONAL);
}

BOOST_AUTO_TEST_CASE(REFLECTED_SNUB_HEXAGONAL_LINKS)
{
  TEST_LINKS(mml::tiling::REFLECTED_SNUB_HEXAGONAL);
}

BOOST_AUTO_TEST_CASE(TRI_HEXAGONAL_LINKS)
{
  TEST_LINKS(mml::tiling::TRI_HEXAGONAL);
}

BOOST_AUTO_TEST_CASE(ELONGATED_TRIANGULAR_LINKS)
{
  TEST_LINKS(mml::tiling::ELONGATED_TRIANGULAR);
}

BOOST_AUTO_TEST_CASE(SNUB_SQUARE_LINKS)
{
  TEST_LINKS(mml::tiling::SNUB_SQUARE);
}

BOOST_AUTO_TEST_CASE(RHOMBITRIHEXAGONAL_LINKS)
{
  TEST_LINKS(mml::tiling::RHOMBITRIHEXAGONAL);
}

BOOST_AUTO_TEST_CASE(TRUNCATED_SQUARE_LINKS)
{
  TEST_LINKS(mml::tiling::TRUNCATED_SQUARE);
}

BOOST_AUTO_TEST_CASE(TRUNCATED_HEXAGONAL_LINKS)
{
  TEST_LINKS(mml::tiling::TRUNCATED_HEXAGONAL);
}

BOOST_AUTO_TEST_CASE(TRUNCATED_TRIHEXAGONAL_LINKS)
{
  TEST_LINKS(mml::tiling::TRUNCATED_TRIHEXAGONAL);
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/math/special_functions/pow.hpp>


//
// Created by Hippolyte Barraud on 14/01/2018.
//

#ifndef DIRECTORY_TEST_SEAM_CARVER_HPP
#define DIRECTORY_TEST_SEAM_CARVER_HPP

#include <vector>
#include <boost/simd.hpp>
#include <boost/simd/algorithm.hpp>

template <typename U, typename Func>
void remove_seam_simded(matrix<U> & energy, matrix<U> & workset, Func functor) {
    namespace bs = boost::simd;
    //Filing the very first row directly from the energy map values
    bs::copy_n(energy.row_at(0), energy.width, workset.row_at(0));
    for (int r = 1; r < energy.height; ++r) {

        //The first and last values of every row remains special one, but with this approach there is no branching
        workset.at(r, 0) = energy.at(r, 0) + std::min(workset.at(r - 1, 1), workset.at(r - 1, 0));
        auto current_row = workset.row_at(r), previous_row = workset.row_at(r-1);

        //Filling the array with max value
        bs::fill(current_row + 1, current_row + workset.width, std::numeric_limits<U>::max());
        //Compare each cell with top left
        bs::transform(previous_row, previous_row + workset.width - 1,current_row + 1, current_row + 1, bs::min);
        //Compare each cell with top
        bs::transform(previous_row + 1, previous_row + workset.width,current_row + 1, current_row + 1, bs::min);
        //Compare each cell with top right
        bs::transform(previous_row + 2, previous_row + workset.width + 1, current_row + 1, current_row + 1, bs::min);
        //Add to each cell the energy value
        bs::transform(energy.row_at(r) + 1, energy.row_at(r) + energy.width - 1, current_row + 1, current_row + 1, bs::plus);

        workset.at(r, energy.width - 1) = energy.at(r, energy.width - 1) + std::min(workset.at(r - 1, energy.width - 2), workset.at(r - 1, energy.width - 1));
    }

    //Find the starting point of seam for removal
    auto energy_last_row = workset.row_at(energy.height - 1);
    auto min_index = static_cast<int>(bs::min_element(energy_last_row, energy_last_row + energy.width - 1) - energy_last_row);
    energy.erase(energy.height - 1, min_index);

    // Unfortunately this part remains messy, I was unable to get it to work with SIMD instruction-set.
    // This is the main bottleneck of the implementation.
    for (int j = energy.height - 2; j >= 0; --j) {
        auto row_dp = workset.row_at(j);
        if (min_index != 0 && min_index != energy.width - 1) {
            min_index = static_cast<int>(std::distance(row_dp, std::min_element(row_dp + min_index - 1, row_dp + min_index + 2)));
        }
        else if (min_index == 0) {
            min_index = static_cast<int>(std::distance(row_dp, std::min_element(row_dp + min_index, row_dp + min_index + 2)));
        }
        else {
            min_index = static_cast<int>(std::distance(row_dp, std::min_element(row_dp + min_index - 1, row_dp + min_index + 1)));
        }
        functor(j, min_index);
        energy.erase(j, min_index);
    }
}

template <typename U>
void remove_seam_simded(matrix<U> & energy, matrix<U> & workset) {
    remove_seam_simded(energy, workset, [] (auto const&, auto const&) {});
}

#endif //DIRECTORY_TEST_SEAM_CARVER_HPP


#pragma once

#include <polyfem/Common.hpp>
#include <polyfem/ElasticityUtils.hpp>

#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/ElementBases.hpp>

#include <polyfem/AutodiffTypes.hpp>

#include <Eigen/Dense>
#include <functional>

//local assembler for incompressible model, pressure is separate (see Stokes)
namespace polyfem
{

	//displacement assembler
	class IncompressibleLinearElasticityDispacement
	{
	public:
		// res is R^{dim²}
		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 9, 1>
		assemble(const ElementAssemblyValues &vals, const int i, const int j, const QuadratureVector &da) const;

		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		compute_rhs(const AutodiffHessianPt &pt) const;

		void set_size(const int size);
		inline int size() const { return size_; }

		void set_parameters(const json &params);
		void init_multimaterial(const bool is_volume, const Eigen::MatrixXd &Es, const Eigen::MatrixXd &nus);

		void compute_von_mises_stresses(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &stresses) const;
		void compute_stress_tensor(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &tensor) const;

	private:
		int size_ = -1;

		LameParameters params_;

		void assign_stress_tensor(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, const int all_size, Eigen::MatrixXd &all, const std::function<Eigen::MatrixXd(const Eigen::MatrixXd &)> &fun) const;
	};

	//mixed, displacement and pressure
	class IncompressibleLinearElasticityMixed
	{
	public:
		// res is R^{dim}
		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		assemble(const ElementAssemblyValues &psi_vals, const ElementAssemblyValues &phi_vals, const int i, const int j, const QuadratureVector &da) const;

		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		compute_rhs(const AutodiffHessianPt &pt) const;

		void set_size(const int size);

		inline int rows() const { return size_; }
		inline int cols() const { return 1; }

		void set_parameters(const json &params);
		void init_multimaterial(const bool is_volume, const Eigen::MatrixXd &Es, const Eigen::MatrixXd &nus);

	private:
		int size_ = -1;
	};

	//pressure only part
	class IncompressibleLinearElasticityPressure
	{
	public:
		// res is R^{1}
		Eigen::Matrix<double, 1, 1>
		assemble(const ElementAssemblyValues &vals, const int i, const int j, const QuadratureVector &da) const;

		Eigen::Matrix<double, 1, 1>
		compute_rhs(const AutodiffHessianPt &pt) const
		{
			assert(false);
			return Eigen::Matrix<double, 1, 1>::Zero(1, 1);
		}

		inline int size() const { return 1; }

		void set_parameters(const json &params);
		void init_multimaterial(const bool is_volume, const Eigen::MatrixXd &Es, const Eigen::MatrixXd &nus);

	private:
		int size_ = -1;
		LameParameters params_;
	};
} // namespace polyfem