//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_TWO_SPLIT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_TWO_SPLIT_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/two_split.hpp>
#include <boost/simd/include/constants/splitfactor.hpp>
#include <boost/fusion/tuple.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_split_, tag::cpu_,
                          (A0),
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                         )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a,
                              A0 & r0,A0 & r1) const
    {
      A0 c  = Splitfactor<A0>()*a;
      r0 = c-(c-a);
      r1 = a-r0;
      return 0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_split_, tag::cpu_,
                          (A0),
                          ((scalar_<floating_<A0> >))
                          ((scalar_<floating_<A0> >))
                         )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A0 const& a2) const
    {
      A0 a1;
      two_split(a0,a1,a2);
      return a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_split_, tag::cpu_,
                            (A0),
                            ((scalar_<floating_<A0> >))
                           )
  {
    typedef typename boost::fusion::tuple<A0, A0> result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type res;
      two_split(a0,boost::fusion::at_c<0>(res),boost::fusion::at_c<1>(res));
      return res;
    }

    private :
    inline void eval(A0 const& a, A0& r0, A0& r1)const
    {
      A0 c;
      c  = Splitfactor<A0>()*a;
      r0 = c-(c-a);
      r1 = a-r0;
    }
  };
} } }

#endif


#define SDL_MAIN_HANDLED // for some reason SDL main is broken on my computer
#include <SDL2/SDL.h>
#include <imgui/imgui.h>
#include <imgui/imgui_impl_sdl.h>
#include <imgui/imgui_impl_sdlrenderer.h>
#include <stdio.h>
#include <fstream>
#include <sstream>
#include <Eigen/Dense>
#define AINI_IMPLEMENTATION
#include "aini.hpp"
#include "KerrBlackHole.hpp"
#include "physicalConstants.hpp"
#include "drawUtility.hpp"

using vec = Eigen::Vector2<double>;

static std::string readText(std::ifstream const& settingsFile)
{
	std::ostringstream sstream;
	sstream << settingsFile.rdbuf();
	return sstream.str();
}

int main(int, char**)
{
	if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_TIMER | SDL_INIT_GAMECONTROLLER) != 0)
	{
		printf("Error: %s\n", SDL_GetError());
		return -1;
	}

	// Setup window
	SDL_WindowFlags const window_flags = (SDL_WindowFlags)(SDL_WINDOW_RESIZABLE | SDL_WINDOW_ALLOW_HIGHDPI);
	SDL_Window* window = SDL_CreateWindow("Interstellar-sandbox-2d", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, 1280, 720, window_flags);

	// Setup SDL_Renderer instance
	SDL_Renderer* renderer = SDL_CreateRenderer(window, -1, SDL_RENDERER_PRESENTVSYNC | SDL_RENDERER_ACCELERATED);
	if (renderer == nullptr)
	{
		printf("Error: %s\n", SDL_GetError());
		return -1;
	}

	// Setup Dear ImGui context
	IMGUI_CHECKVERSION();
	ImGui::CreateContext();
	ImGuiIO& io = ImGui::GetIO(); (void)io;
	io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard;     // Enable Keyboard Controls

	// Setup Dear ImGui style
	ImGui::StyleColorsDark();

	// Setup Platform/Renderer backends
	ImGui_ImplSDL2_InitForSDLRenderer(window, renderer);
	ImGui_ImplSDLRenderer_Init(renderer);

	// Our state
	ImVec4 clear_color = ImVec4(0.0f, 0.0f, 0.0f, 1.00f);
	ImVec4 grid_color = ImVec4(0.0f, 1.0f, 0.0f, 1.00f);

	vec initialPos{};

	int grid_size = 20;
	int n_rays = 360;
	double pixel_scale = 1;
	double constexpr G = 1;
	double constexpr C = 1;
	float time = 0.0;
	float animation_speed = 1.0f;
	double M = 1000;
	SchwarzschildBlackHole blackHole;
	{
		std::ifstream saveFile("save.ini");
		if (saveFile.is_open())
		{
			aini::Reader reader(readText(saveFile));
			blackHole.deserialize(reader);
		}
	}

	// Main loop
	bool done = false;
	int steps = 400;
	while (!done)
	{
		// Poll and handle events (inputs, window resize, etc.)
		// You can read the io.WantCaptureMouse, io.WantCaptureKeyboard flags to tell if dear imgui wants to use your inputs.
		// - When io.WantCaptureMouse is true, do not dispatch mouse input data to your main application, or clear/overwrite your copy of the mouse data.
		// - When io.WantCaptureKeyboard is true, do not dispatch keyboard input data to your main application, or clear/overwrite your copy of the keyboard data.
		// Generally you may always pass all inputs to dear imgui, and hide them from your application based on those two flags.
		SDL_Event event;
		while (SDL_PollEvent(&event))
		{
			ImGui_ImplSDL2_ProcessEvent(&event);
			if (event.type == SDL_QUIT)
				done = true;
			if (event.type == SDL_WINDOWEVENT && event.window.event == SDL_WINDOWEVENT_CLOSE && event.window.windowID == SDL_GetWindowID(window))
				done = true;
		}

		// Start the Dear ImGui frame
		ImGui_ImplSDLRenderer_NewFrame();
		ImGui_ImplSDL2_NewFrame();
		ImGui::NewFrame();

		// 2. Show a simple window that we create ourselves. We use a Begin/End pair to created a named window.
		{
			ImGui::Begin("Settings");
			ImGui::ColorEdit3("clear color", (float*)&clear_color);
			ImGui::ColorEdit3("grid color", (float*)&grid_color);
			ImGui::SliderInt("grid size", &grid_size, 10, 100);

			ImGui::DragInt("steps", &steps, 100, 1, 0);
			ImGui::DragInt("n rays", &n_rays, 1, 1, 0);
			ImGui::DragDouble("pixel scale", &pixel_scale);
			ImGui::DragDouble("M", &M);
			ImGui::SliderFloat("Time", &time, 0, 1.0f);
			ImGui::DragFloat("animation speed", &animation_speed);
			static bool animate = false;
			ImGui::Checkbox("Animate", &animate);
			if (animate)
			{
				time += animation_speed * ImGui::GetIO().DeltaTime;
				if (time > 1.0f)
					time = 0.0f;
				if (time < 0.0f)
					time = 1.0f;
			}

			ImGui::DragDouble("initial pos x", &initialPos[0]);
			ImGui::DragDouble("initial pos y", &initialPos[1]);

			blackHole.showEditor();

			if (ImGui::Button("save"))
			{
				aini::Writer writer;
				blackHole.serialize(writer);
				std::ofstream saveFile("save.ini");
				saveFile << writer.write();
				saveFile.flush();
			}

			ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate);
			ImGui::End();
		}

		SDL_RenderClear(renderer);

		SDL_SetRenderDrawColor(renderer, (Uint8)(grid_color.x * 255), (Uint8)(grid_color.y * 255), (Uint8)(grid_color.z * 255), (Uint8)(grid_color.w * 255));
		int width, height;
		SDL_GetWindowSize(window, &width, &height);

		// newton approximation
		// see: https://fr.wikipedia.org/wiki/Tests_exp%C3%A9rimentaux_de_la_relativit%C3%A9_g%C3%A9n%C3%A9rale
		// https://fr.wikipedia.org/wiki/Loi_universelle_de_la_gravitation

		auto draw_ray = [&](vec pos, double angle)
		{
			vec dir{ cos(angle), sin(angle) };
			for (int i = 0; i < (int)(steps * time); i++)
			{
				vec const d = blackHole.pos - pos;
				auto const F = (G * M) / (d.squaredNorm());
				auto const gravity = d.normalized() * F;
				dir += gravity;
				pos += dir * C;

				SDL_RenderDrawPoint(renderer, (int)(pos[0] / pixel_scale), (int)(pos[1] / pixel_scale));
			}
		};

		double const angleStep = (M_PI * 2.0) / n_rays;
		double angle = 0;
		for (int n = 0; n < n_rays; n++)
		{
			draw_ray(initialPos, angle);
			angle += angleStep;
		}

		// draw black hole center
		SDL_SetRenderDrawColor(renderer, 255, 0, 0, 255);
		SDL_Rect brect{ (int)(blackHole.pos[0] / pixel_scale), (int)(blackHole.pos[1] / pixel_scale), 4, 4};
		SDL_RenderFillRect(renderer, &brect);
		//draw_circle(renderer, blackHole.pos[0] / pixel_scale, blackHole.pos[1] / pixel_scale, blackHole.SchwarzschildRadius() / pixel_scale);

		ImGui::Render();
		SDL_SetRenderDrawColor(renderer, (Uint8)(clear_color.x * 255), (Uint8)(clear_color.y * 255), (Uint8)(clear_color.z * 255), (Uint8)(clear_color.w * 255));
		ImGui_ImplSDLRenderer_RenderDrawData(ImGui::GetDrawData());
		SDL_RenderPresent(renderer);
	}

	// Cleanup
	ImGui_ImplSDLRenderer_Shutdown();
	ImGui_ImplSDL2_Shutdown();
	ImGui::DestroyContext();

	SDL_DestroyRenderer(renderer);
	SDL_DestroyWindow(window);
	SDL_Quit();

	return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by
//     Barend Gehrels (Geodan, Amsterdam)
//     Adam Wulkiewicz

// Original copyright notice:

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

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

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP

#include <string>
#include <vector>

#include <boost/geometry/formulas/eccentricity_sqr.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/pj_ellps.hpp>
#include <boost/geometry/srs/projections/impl/pj_param.hpp>
#include <boost/geometry/srs/projections/proj4.hpp>

namespace boost { namespace geometry { namespace projections {

namespace detail {

/* set ellipsoid parameters a and es */
template <typename T>
inline T SIXTH() { return .1666666666666666667; } /* 1/6 */
template <typename T>
inline T RA4() { return .04722222222222222222; } /* 17/360 */
template <typename T>
inline T RA6() { return .02215608465608465608; } /* 67/3024 */
template <typename T>
inline T RV4() { return .06944444444444444444; } /* 5/72 */
template <typename T>
inline T RV6() { return .04243827160493827160; } /* 55/1296 */

/* initialize geographic shape parameters */
template <typename BGParams, typename T>
inline void pj_ell_set(BGParams const& /*bg_params*/, std::vector<pvalue<T> >& parameters, T &a, T &es)
{
    T b = 0.0;
    T e = 0.0;
    std::string name;

    /* check for varying forms of ellipsoid input */
    a = es = 0.;

    /* R takes precedence */
    if (pj_param_f(parameters, "R", a)) {
        /* empty */
    } else { /* probable elliptical figure */

        /* check if ellps present and temporarily append its values to pl */
        name = pj_get_param_s(parameters, "ellps");
        if (! name.empty())
        {
            const int n = sizeof(pj_ellps) / sizeof(pj_ellps[0]);
            int index = -1;
            for (int i = 0; i < n && index == -1; i++)
            {
                if(pj_ellps[i].id == name)
                {
                    index = i;
                }
            }

            if (index == -1) {
                BOOST_THROW_EXCEPTION( projection_exception(error_unknown_ellp_param) );
            }

            pj_ellps_type const& pj_ellp = pj_ellps[index];
            parameters.push_back(pj_mkparam<T>("a", pj_ellp.major_v));
            parameters.push_back(pj_mkparam<T>(pj_ellp.ell_n, pj_ellp.ell_v));
        }
        a = pj_get_param_f(parameters, "a");
        if (pj_param_f(parameters, "es", es)) {/* eccentricity squared */
            /* empty */
        } else if (pj_param_f(parameters, "e", e)) { /* eccentricity */
            es = e * e;
        } else if (pj_param_f(parameters, "rf", es)) { /* recip flattening */
            if (!es) {
                BOOST_THROW_EXCEPTION( projection_exception(error_rev_flattening_is_zero) );
            }
            es = 1./ es;
            es = es * (2. - es);
        } else if (pj_param_f(parameters, "f", es)) { /* flattening */
            es = es * (2. - es);
        } else if (pj_param_f(parameters, "b", b)) { /* minor axis */
            es = 1. - (b * b) / (a * a);
        }     /* else es == 0. and sphere of radius a */
        if (!b)
            b = a * sqrt(1. - es);
        /* following options turn ellipsoid into equivalent sphere */
        if (pj_get_param_b(parameters, "R_A")) { /* sphere--area of ellipsoid */
            a *= 1. - es * (SIXTH<T>() + es * (RA4<T>() + es * RA6<T>()));
            es = 0.;
        } else if (pj_get_param_b(parameters, "R_V")) { /* sphere--vol. of ellipsoid */
            a *= 1. - es * (SIXTH<T>() + es * (RV4<T>() + es * RV6<T>()));
            es = 0.;
        } else if (pj_get_param_b(parameters, "R_a")) { /* sphere--arithmetic mean */
            a = .5 * (a + b);
            es = 0.;
        } else if (pj_get_param_b(parameters, "R_g")) { /* sphere--geometric mean */
            a = sqrt(a * b);
            es = 0.;
        } else if (pj_get_param_b(parameters, "R_h")) { /* sphere--harmonic mean */
            a = 2. * a * b / (a + b);
            es = 0.;
        } else {
            T tmp;
            int i = pj_param_r(parameters, "R_lat_a", tmp);
            if (i || /* sphere--arith. */
                pj_param_r(parameters, "R_lat_g", tmp)) { /* or geom. mean at latitude */

                tmp = sin(tmp);
                if (geometry::math::abs(tmp) > geometry::math::half_pi<T>()) {
                    BOOST_THROW_EXCEPTION( projection_exception(error_ref_rad_larger_than_90) );
                }
                tmp = 1. - es * tmp * tmp;
                a *= i ? .5 * (1. - es + tmp) / ( tmp * sqrt(tmp)) :
                    sqrt(1. - es) / tmp;
                es = 0.;
            }
        }
    }

    /* some remaining checks */
    if (es < 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(error_es_less_than_zero) );
    }
    if (a <= 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(error_major_axis_not_given) );
    }
}

template <BOOST_GEOMETRY_PROJECTIONS_DETAIL_TYPENAME_PX, typename T>
inline void pj_ell_set(srs::static_proj4<BOOST_GEOMETRY_PROJECTIONS_DETAIL_PX> const& bg_params,
                       std::vector<pvalue<T> >& /*parameters*/, T &a, T &es)
{
    typedef srs::static_proj4<BOOST_GEOMETRY_PROJECTIONS_DETAIL_PX> static_parameters_type;
    typedef typename srs::par4::detail::pick_ellps
        <
            static_parameters_type
        > pick_ellps;

    typename pick_ellps::model_type model = pick_ellps::model(bg_params);

    a = geometry::get_radius<0>(model);
    T b = geometry::get_radius<2>(model);
    es = 0.;
    if (a != b)
    {
        es = formula::eccentricity_sqr<T>(model);

        // Ignore all other parameters passed in string, at least for now
    }

    /* some remaining checks */
    if (es < 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(error_es_less_than_zero) );
    }
    if (a <= 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(error_major_axis_not_given) );
    }
}

template <typename T>
inline void pj_calc_ellipsoid_params(parameters<T> & p, T const& a, T const& es) {
/****************************************************************************************
    Calculate a large number of ancillary ellipsoidal parameters, in addition to
    the two traditional PROJ defining parameters: Semimajor axis, a, and the
    eccentricity squared, es.

    Most of these parameters are fairly cheap to compute in comparison to the overall
    effort involved in initializing a PJ object. They may, however, take a substantial
    part of the time taken in computing an individual point transformation.

    So by providing them up front, we can amortize the (already modest) cost over all
    transformations carried out over the entire lifetime of a PJ object, rather than
    incur that cost for every single transformation.

    Most of the parameter calculations here are based on the "angular eccentricity",
    i.e. the angle, measured from the semiminor axis, of a line going from the north
    pole to one of the foci of the ellipsoid - or in other words: The arc sine of the
    eccentricity.

    The formulae used are mostly taken from:

    Richard H. Rapp: Geometric Geodesy, Part I, (178 pp, 1991).
    Columbus, Ohio:  Dept. of Geodetic Science
    and Surveying, Ohio State University.

****************************************************************************************/

    p.a = a;
    p.es = es;

    /* Compute some ancillary ellipsoidal parameters */
    if (p.e==0)
        p.e = sqrt(p.es);  /* eccentricity */
    //p.alpha = asin (p.e);  /* angular eccentricity */

    /* second eccentricity */
    //p.e2  = tan (p.alpha);
    //p.e2s = p.e2 * p.e2;

    /* third eccentricity */
    //p.e3    = (0!=p.alpha)? sin (p.alpha) / sqrt(2 - sin (p.alpha)*sin (p.alpha)): 0;
    //p.e3s = p.e3 * p.e3;

    /* flattening */
    //if (0==p.f)
    //    p.f  = 1 - cos (p.alpha);   /* = 1 - sqrt (1 - PIN->es); */
    //p.rf = p.f != 0.0 ? 1.0/p.f: HUGE_VAL;

    /* second flattening */
    //p.f2  = (cos(p.alpha)!=0)? 1/cos (p.alpha) - 1: 0;
    //p.rf2 = p.f2 != 0.0 ? 1/p.f2: HUGE_VAL;

    /* third flattening */
    //p.n  = pow (tan (p.alpha/2), 2);
    //p.rn = p.n != 0.0 ? 1/p.n: HUGE_VAL;

    /* ...and a few more */
    //if (0==p.b)
    //    p.b  = (1 - p.f)*p.a;
    //p.rb = 1. / p.b;
    p.ra = 1. / p.a;

    p.one_es = 1. - p.es;
    if (p.one_es == 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(error_eccentricity_is_one) );
    }

    p.rone_es = 1./p.one_es;
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP


// -*- mode: c++; indent-tabs-mode: nil; -*-
//
// Copyright (c) 2009-2013 Illumina, Inc.
//
// This software is provided under the terms and conditions of the
// Illumina Open Source Software License 1.
//
// You should have received a copy of the Illumina Open Source
// Software License 1 along with this program. If not, see
// <https://github.com/sequencing/licenses/>
//

/// \file

/// \author Chris Saunders
///
#include "blt_util/nploid_genotype_util.hh"

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

unsigned
nploid_gtype_size(const unsigned ploidy) {

    static const unsigned NB1(N_BASE-1);
    const double r(boost::math::binomial_coefficient<double>(NB1+ploidy,NB1));
    assert(r>=0);
    return static_cast<unsigned>(r);
}

nploid_info::
nploid_info(const unsigned init_ploidy)
    : _ploidy(init_ploidy), _echunk(1./static_cast<double>(init_ploidy)) {

    gtype_info gi;
    gi.label.resize(_ploidy);
    ginfo_init(_ploidy,0,0,gi);

    unsigned init_index(0);
    unsigned* init_ref(_ref_gtype);
    ref_gtype_init(_ploidy,0,0,true,init_ref,init_index);
}

// recursively derive labels and base frequencies for each genotype
void
nploid_info::
ginfo_init(const unsigned init_ploidy,
           const unsigned pli,
           const unsigned init_i,
           gtype_info& gi) {

    for (unsigned i(init_i); i<N_BASE; ++i) {
        gi.efreq_levels[i] += 1;
        gi.label[pli]=id_to_base(i);
        if ((pli+1)<init_ploidy) {
            ginfo_init(init_ploidy,pli+1,i,gi);
        } else {
            _ginfo.push_back(gi);
        }
        gi.efreq_levels[i] -= 1;
    }
}

// build an index of which genotype corresponds to the reference state,
// for each possible reference base:
void
nploid_info::
ref_gtype_init(const unsigned init_ploidy,
               const unsigned pli,
               const unsigned init_i,
               const bool is_hom,
               unsigned*& ref,
               unsigned& index) {

    for (unsigned i(init_i); i<N_BASE; ++i) {
        const bool is_iter_hom(pli==0 || (is_hom && (i==init_i)));
        if ((pli+1)<init_ploidy) {
            ref_gtype_init(init_ploidy,pli+1,i,is_iter_hom,ref,index);
        } else {
            if (is_iter_hom) *ref++ = index;
            ++index;
        }
    }
}


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

/// \file StereoSessionKeypoint.cc
///

#include <boost/shared_ptr.hpp>

#include <iostream>                        // debugging
#include <algorithm>

#include <asp/Core/StereoSettings.h>
#include <asp/Sessions/Keypoint/StereoSessionKeypoint.h>

#include <vw/FileIO.h>
#include <vw/Image.h>
#include <vw/InterestPoint.h>
#include <vw/Stereo/DisparityMap.h>
#include <vw/Math.h>

using namespace vw;
using namespace vw::ip;

std::string
asp::StereoSessionKeypoint::create_subsampled_align_image(std::string const& image_file,
                                                          std::string const& suffix) {
  std::string align_image_file(m_out_prefix + std::string("-normalized-align-sub-") + suffix);

  DiskImageView<PixelGray<float> > disk_image(image_file);

  std::cout << "StereoSessionKeypoint::create_subsampled_align_image():\n";
  double sub_sampling = stereo_settings().keypoint_align_subsampling;
  write_image(align_image_file, channel_cast_rescale<uint8>(resample(disk_image, 1.0 / double(sub_sampling))), TerminalProgressCallback("asp","Subsampling:"));

  return align_image_file;
}

void asp::StereoSessionKeypoint::scale_align_matrix(Matrix<double> & align_matrix) {
  Matrix<double> scale_matrix = vw::math::identity_matrix(3);
  Matrix<double> inv_scale_matrix = vw::math::identity_matrix(3);
  double sub_sampling = stereo_settings().keypoint_align_subsampling;
  scale_matrix(0, 0) = 1.0 / double(sub_sampling);
  scale_matrix(1, 1) = scale_matrix(0, 0);
  inv_scale_matrix(0, 0) = double(sub_sampling);
  inv_scale_matrix(1, 1) = inv_scale_matrix(0, 0);
  align_matrix = align_matrix * scale_matrix;
  align_matrix = inv_scale_matrix * align_matrix;
  vw_out() << "StereoSessionKeypoint::adjust_align_matrix(): scaled alignment matrix:\n";
  vw_out() << align_matrix << std::endl;
}

void
asp::StereoSessionKeypoint::pre_preprocessing_hook(std::string const&/*input_file1*/,
                                                   std::string const&/*input_file2*/,
                                                   std::string & output_file1,
                                                   std::string & output_file2) {

  std::string left_align_image_file(m_left_image_file),
    right_align_image_file(m_right_image_file);

  double sub_sampling = stereo_settings().keypoint_align_subsampling;
  if (sub_sampling > 1) {
    left_align_image_file =
      create_subsampled_align_image(m_left_image_file, "L.tif");
    right_align_image_file =
      create_subsampled_align_image(m_right_image_file, "R.tif");
  }

  // Load the two images
  DiskImageView<PixelGray<float> > left_align_image(left_align_image_file);
  DiskImageView<PixelGray<float> > right_align_image(right_align_image_file);

  // Determine the alignment matrix using keypoint matching techniques.
  Matrix<double> align_matrix = determine_image_align(left_align_image_file,
                                                      right_align_image_file,
                                                      left_align_image,
                                                      right_align_image);
  if (sub_sampling > 1)
    scale_align_matrix(align_matrix);
  ::write_matrix(m_out_prefix + "-align.exr", align_matrix);

  DiskImageView<PixelGray<float> > left_disk_image(m_left_image_file);
  DiskImageView<PixelGray<float> > right_disk_image(m_right_image_file);
  ImageViewRef<PixelGray<float> > Limg = left_disk_image;
  ImageViewRef<PixelGray<float> > Rimg = transform(right_disk_image, HomographyTransform(align_matrix),
                                                   left_disk_image.cols(), left_disk_image.rows());

  output_file1 = m_out_prefix + "-L.tif";
  output_file2 = m_out_prefix + "-R.tif";

  write_image(output_file1, channel_cast_rescale<uint8>(Limg),
              TerminalProgressCallback("asp",""));
  write_image(output_file2, channel_cast_rescale<uint8>(Rimg),
              TerminalProgressCallback("asp",""));
}

void asp::StereoSessionKeypoint::pre_pointcloud_hook(std::string const& input_file,
                                                     std::string & output_file) {
  //  output_file = input_file;
  output_file = m_out_prefix + "-F-corrected.exr";
  vw_out() << "Processing disparity map to remove the earlier effects of interest point alignment.\n";

  DiskImageView<PixelMask<Vector2f> > disparity_map(input_file);

  // We used a homography to line up the images, we may want
  // to generate pre-alignment disparities before passing this information
  // onto the camera model in the next stage of the stereo pipeline.
  vw::Matrix<double> align_matrix;
  try {
    ::read_matrix(align_matrix, m_out_prefix + "-align.exr");
    std::cout << "Alignment Matrix: " << align_matrix << "\n";
  } catch (vw::IOErr const& e) {
    std::cout << "Could not read in aligment matrix: " << m_out_prefix << "-align.exr.  Exiting. \n\n";
    exit(1);
  }

  ImageViewRef<PixelMask<Vector2f> > result = stereo::transform_disparities(disparity_map, HomographyTransform(align_matrix));

  // Remove pixels that are outside the bounds of the secondary image.
  DiskImageView<PixelGray<float> > right_disk_image(m_right_image_file);
  result = stereo::disparity_range_mask(result, right_disk_image.cols(), right_disk_image.rows());

  write_image(output_file, result,
              TerminalProgressCallback("asp","") );
}


#include <iostream>

#include <vw/Image.h>
#include <vw/Cartography.h>
#include <vw/FileIO.h>
#include <vw/Camera.h>

#include <vw/Plate/PlateFile.h>

#include "orbitfoot.h"

#include <boost/format.hpp>
#include <boost/filesystem.hpp>

namespace fs = boost::filesystem;

using namespace vw;

ImageView<PixelRGBA<uint8> > render_tile(int i, int j, int renderlevel, int tile_size, std::vector<OrbitFootprint> const& affected, double rad)
{
  ImageView<PixelRGBA<uint8> > tile(tile_size, tile_size);
  if (affected.size() == 0) return tile;

  BBox2 cam_bounds(0, 0, 5725, 5725);

  std::vector<camera::PinholeModel> cams;

  BOOST_FOREACH(OrbitFootprint o, affected) {
    cams.push_back(o.camfile());
  }

  // Resolution: size of workspace, in pixels
  double resolution = tile_size << renderlevel;
  Matrix3x3 transform;
  transform(0,0) = 360.0 / resolution;
  transform(0,2) = -180.0;
  transform(1,1) = -360.0 / resolution;
  transform(1,2) = 180.0;
  transform(2,2) = 1;

  Vector2 tileoffset(tile_size*i, tile_size*j);

  //TODO: instead, get lonlat BBox from tile's col, row, and lvl
  //      and then make a mapping from 0-tile_size => lonlat.min-lonlat.max
  //
  // Vector3 llr(0, 0, rad);
  // subvector(llr, 0, 2) = Vector2(i,j) * (lonlat.max - lonlat.min) / tile_size + lonlat.min

  for (int i = 0; i < tile_size; i++) {
    for (int j = 0; j < tile_size; j++) {
      Vector3 llr = transform * hom(Vector2(i, j) + tileoffset);
      llr[2] = rad;

      Vector3 xyz = cartography::lon_lat_radius_to_xyz(llr);

      int numovers = 0;
      BOOST_FOREACH(camera::PinholeModel cam, cams) {
        Vector2 orb_px = cam.point_to_pixel(xyz);
        if (cam_bounds.contains(orb_px)) {
          numovers++;
        }
      }
      if (numovers > 0) {
        int color = int(255.0 * numovers / 20);
        color = std::min(color, 255);
        tile(i, j) = PixelRGBA<vw::uint8>(color, color, color, 255);
      }
    }
  }
  return tile;
}

int main()
{
  using std::cout;
  using std::endl;
  using std::string;

  string pinhole_file_fmt = "/byss/khusmann/data/AS15_cameras/AS15-M-%04d.lev1.pinhole";
  double moon_rad = 1737400.0;
  int tile_size = 256;

  std::vector<OrbitFootprint> orbits;
  BBox2 renderbox;

//  for (int i = 73; i < 2492; i++) {
  for (int i = 73; i < 600; i++) {
    string pinhole_file = (boost::format(pinhole_file_fmt) % i).str();
    if (!fs::exists(pinhole_file)) {
      continue;
    }

    OrbitFootprint o(pinhole_file, Vector2i(5725, 5725), moon_rad);
    renderbox.grow(o.bounding_box());
    orbits.push_back(o);
  }

  int renderlevel = 6;
  // Resolution: size of workspace, in tiles
  double resolution = 1 << renderlevel;
  Matrix3x3 transform;
  transform(0,0) = 360.0 / resolution;
  transform(0,2) = -180.0;
  transform(1,1) = -360.0 / resolution;
  transform(1,2) = 180.0;
  transform(2,2) = 1;
  transform = inverse(transform);

  BBox2 renderbox_trans;
  renderbox_trans.grow(dehom(transform * hom(renderbox.min())));
  renderbox_trans.grow(dehom(transform * hom(renderbox.max())));

  BBox2i tile_renderbox(floor(renderbox_trans.min()), ceil(renderbox_trans.max()));

  boost::shared_ptr<platefile::PlateFile> pf(new platefile::PlateFile("output.plate", "equi", "desc", tile_size, "tif", VW_PIXEL_RGBA, VW_CHANNEL_UINT8));
  platefile::Transaction tid = pf->transaction_request("tdesc", -1);

  for (int i = tile_renderbox.min().x(); i < tile_renderbox.max().x(); i++) {
    for (int j = tile_renderbox.min().y(); j < tile_renderbox.max().y(); j++) {
      std::vector<OrbitFootprint> affected;

      BOOST_FOREACH(OrbitFootprint o, orbits) {
        if (o.intersects(i, j, renderlevel)) {
            affected.push_back(o);
        }
      }

      ImageView<PixelRGBA<uint8> > rendered_tile;

      cout << "Rendering tile: (" << i << ", " << j << ") @" << renderlevel << " affected: " << affected.size() << endl;
      rendered_tile = render_tile(i, j, renderlevel, tile_size, affected, moon_rad);

      // Add to platefile
      pf->write_request();
      pf->write_update(rendered_tile, i, j, renderlevel, tid);
      pf->sync();
      pf->write_complete();
    }
  }

  return 0;
}


#include "generator/regions/region.hpp"

#include "generator/boost_helpers.hpp"
#include "generator/regions/collector_region_info.hpp"
#include "generator/regions/place_point.hpp"

#include "geometry/mercator.hpp"

#include "base/assert.hpp"

#include <algorithm>
#include <numeric>

#include <boost/geometry.hpp>

namespace generator
{
namespace regions
{

BoostPolygon MakePolygonWithRadius(BoostPoint const & point, double radius, size_t numPoints  = 16)
{
  boost::geometry::strategy::buffer::point_circle point_strategy(numPoints);
  boost::geometry::strategy::buffer::distance_symmetric<double> distance_strategy(radius);

  static boost::geometry::strategy::buffer::join_round const join_strategy;
  static boost::geometry::strategy::buffer::end_round const end_strategy;
  static boost::geometry::strategy::buffer::side_straight const side_strategy;

  boost::geometry::model::multi_polygon<BoostPolygon> result;
  boost::geometry::buffer(point, result, distance_strategy, side_strategy, join_strategy,
                          end_strategy, point_strategy);
  CHECK_EQUAL(result.size(), 1, ());
  return std::move(result.front());
}
Region::Region(FeatureBuilder1 const & fb, RegionDataProxy const & rd)
  : RegionWithName(fb.GetParams().name)
  , RegionWithData(rd)
  , m_polygon(std::make_shared<BoostPolygon>())
{
  FillPolygon(fb);
  boost::geometry::envelope(*m_polygon, m_rect);
  m_area = boost::geometry::area(*m_polygon);
}

Region::Region(PlacePoint const & place)
  : RegionWithName(place.GetMultilangName())
  , RegionWithData(place.GetRegionData())
  , m_polygon(std::make_shared<BoostPolygon>())
{
  auto const radius = GetRadiusByPlaceType(place.GetPlaceType());
  *m_polygon = MakePolygonWithRadius(place.GetPosition(), radius);
  boost::geometry::envelope(*m_polygon, m_rect);
  m_area = boost::geometry::area(*m_polygon);
}

// static
double Region::GetRadiusByPlaceType(PlaceType place)
{
  // Based on average radiuses of OSM place polygons.
  switch (place)
  {
  case PlaceType::City:
    return 0.078;
  case PlaceType::Town:
    return 0.033;
  case PlaceType::Village:
    return 0.013;
  case PlaceType::Hamlet:
    return 0.0067;
  case PlaceType::Suburb:
    return 0.016;
  case PlaceType::Neighbourhood:
  case PlaceType::IsolatedDwelling:
    return 0.0035;
  case PlaceType::Unknown:
    UNREACHABLE();
  }
  UNREACHABLE();
}

void Region::DeletePolygon()
{
  m_polygon = nullptr;
}

void Region::FillPolygon(FeatureBuilder1 const & fb)
{
  CHECK(m_polygon, ());
  boost_helpers::FillPolygon(*m_polygon, fb);
}

bool Region::IsCountry() const
{
  static auto const kAdminLevelCountry = AdminLevel::Two;
  return GetPlaceType() == PlaceType::Unknown && GetAdminLevel() == kAdminLevelCountry;
}

bool Region::IsLocality() const
{
  return GetPlaceType() != PlaceType::Unknown;
}

bool Region::Contains(Region const & smaller) const
{
  CHECK(m_polygon, ());
  CHECK(smaller.m_polygon, ());

  return boost::geometry::covered_by(smaller.m_rect, m_rect) &&
      boost::geometry::covered_by(*smaller.m_polygon, *m_polygon);
}

double Region::CalculateOverlapPercentage(Region const & other) const
{
  CHECK(m_polygon, ());
  CHECK(other.m_polygon, ());

  if (!boost::geometry::intersects(other.m_rect, m_rect))
    return 0.0;

  std::vector<BoostPolygon> coll;
  boost::geometry::intersection(*other.m_polygon, *m_polygon, coll);
  auto const min = std::min(boost::geometry::area(*other.m_polygon),
                            boost::geometry::area(*m_polygon));
  auto const binOp = [](double x, BoostPolygon const & y) { return x + boost::geometry::area(y); };
  auto const sum = std::accumulate(std::begin(coll), std::end(coll), 0., binOp);
  return (sum / min) * 100;
}

bool Region::ContainsRect(Region const & smaller) const
{
  return boost::geometry::covered_by(smaller.m_rect, m_rect);
}

BoostPoint Region::GetCenter() const
{
  BoostPoint p;
  boost::geometry::centroid(m_rect, p);
  return p;
}

bool Region::Contains(PlacePoint const & place) const
{
  CHECK(m_polygon, ());

  return Contains(place.GetPosition());
}

bool Region::Contains(BoostPoint const & point) const
{
  CHECK(m_polygon, ());

  return boost::geometry::covered_by(point, m_rect) &&
      boost::geometry::covered_by(point, *m_polygon);
}

bool FeaturePlacePointToRegion(RegionInfo const & regionInfo, FeatureBuilder1 & feature)
{
  if (!feature.IsPoint())
    return false;

  auto const center = feature.GetKeyPoint();
  BoostPolygon polygon;
  auto info = regionInfo.Get(feature.GetMostGenericOsmId());
  if (!info.HasPlaceType())
    return false;

  auto const placeType = info.GetPlaceType();
  if (placeType == PlaceType::Unknown)
    return false;

  auto const radius = Region::GetRadiusByPlaceType(placeType);
  polygon = MakePolygonWithRadius({center.x, center.y}, radius);
  auto const & outer = polygon.outer();
  FeatureBuilder1::PointSeq seq;
  std::transform(std::begin(outer), std::end(outer), std::back_inserter(seq), [](BoostPoint const & p) {
    return m2::PointD(p.get<0>(), p.get<1>());
  });
  feature.ResetGeometry();
  feature.AddPolygon(seq);
  feature.SetAreaAddHoles({});
  feature.SetRank(0);
  return true;
}
}  // namespace regions
}  // namespace generator


#include "multi_dimension_array.h"
#include <vector>
using std::vector;

#define BOOST_DISABLE_ASSERTS
#include <boost/multi_array.hpp>

#include <blitz/array.h>

#include <stdio.h>
#include <sys/time.h>

int main()
{
    size_t size = 4;
    const size_t rowlen = 30;
    vector<size_t> dims;
    dims.resize(size, rowlen);

    vector<size_t> position;
    position.resize(size, 0);

    double ****raw_array = new double***[rowlen];
    boost::multi_array<double, 4> boost_array(boost::extents[rowlen][rowlen][rowlen][rowlen]);
    blitz::Array<double, 4> blitz_array(rowlen, rowlen, rowlen, rowlen);
    blitz::TinyVector<size_t, 4> index;
    for (size_t i = 0; i < rowlen; ++i) {
        raw_array[i] = new double**[rowlen];
        index[0] = i;
        for (size_t j = 0; j < rowlen; ++j) {
            raw_array[i][j] = new double*[rowlen];
            index[1] = j;
            for (size_t k = 0; k < rowlen; ++k) {
                raw_array[i][j][k] = new double[rowlen];
                index[2] = k;
                for (size_t m = 0; m < rowlen; ++m) {
                    raw_array[i][j][k][m] = 42.0;
                    boost_array[i][j][k][m] = 42.0;

                    index[3] = m;
                    blitz_array(index) = 42.0;
                }
            }
        }
    }
    MultiDimensionArray<double> array(dims, 42.0);

    // ===== RAW ARRAY ===== //

    struct timeval begin, end, diff;
    gettimeofday(&begin, NULL);
    double sum = 0.0;
    for (size_t i = 0; i < rowlen; ++i) {
        for (size_t j = 0; j < rowlen; ++j) {
            for (size_t k = 0; k < rowlen; ++k) {
                for (size_t m = 0; m < rowlen; ++m) {
                    sum += raw_array[i][j][k][m];
                }
            }
        }
    }
    gettimeofday(&end, NULL);
    diff.tv_sec = end.tv_sec - begin.tv_sec;
    if (end.tv_usec < begin.tv_usec) {
        --diff.tv_sec;
        end.tv_usec += 1000000;
    }
    diff.tv_usec = end.tv_usec - begin.tv_usec;
    double seconds = diff.tv_sec + (diff.tv_usec / 1000000.0);

    fprintf(stderr, "raw array took %f seconds; sum=%f\n", seconds, sum);

    // ===== MultiDimensionArray ===== //

    gettimeofday(&begin, NULL);
    sum = 0.0;
    for (size_t i = 0; i < rowlen; ++i) {
        position[0] = i;
        for (size_t j = 0; j < rowlen; ++j) {
            position[1] = j;
            for (size_t k = 0; k < rowlen; ++k) {
                position[2] = k;
                for (size_t m = 0; m < rowlen; ++m) {
                    position[3] = m;
                    sum += array.at(position);
                }
            }
        }
    }
    gettimeofday(&end, NULL);
    diff.tv_sec = end.tv_sec - begin.tv_sec;
    if (end.tv_usec < begin.tv_usec) {
        --diff.tv_sec;
        end.tv_usec += 1000000;
    }
    diff.tv_usec = end.tv_usec - begin.tv_usec;
    seconds = diff.tv_sec + (diff.tv_usec / 1000000.0);

    fprintf(stderr, "MultiDimensionArray took %f seconds; sum=%f\n", seconds, sum);

    // ===== boost::multi_array ===== //

    gettimeofday(&begin, NULL);
    sum = 0.0;
    for (size_t i = 0; i < rowlen; ++i) {
        for (size_t j = 0; j < rowlen; ++j) {
            for (size_t k = 0; k < rowlen; ++k) {
                for (size_t m = 0; m < rowlen; ++m) {
                    sum += boost_array[i][j][k][m];
                }
            }
        }
    }
    gettimeofday(&end, NULL);
    diff.tv_sec = end.tv_sec - begin.tv_sec;
    if (end.tv_usec < begin.tv_usec) {
        --diff.tv_sec;
        end.tv_usec += 1000000;
    }
    diff.tv_usec = end.tv_usec - begin.tv_usec;
    seconds = diff.tv_sec + (diff.tv_usec / 1000000.0);
    fprintf(stderr, "boost::multi_array took %f seconds; sum=%f\n", seconds, sum);

    // ===== blitz::Array ===== //

    gettimeofday(&begin, NULL);
    sum = 0.0;
    for (size_t i = 0; i < rowlen; ++i) {
        index[0] = i;
        for (size_t j = 0; j < rowlen; ++j) {
            index[1] = j;
            for (size_t k = 0; k < rowlen; ++k) {
                index[2] = k;
                for (size_t m = 0; m < rowlen; ++m) {
                    index[3] = m;
                    sum += blitz_array(index);
                }
            }
        }
    }
    gettimeofday(&end, NULL);
    diff.tv_sec = end.tv_sec - begin.tv_sec;
    if (end.tv_usec < begin.tv_usec) {
        --diff.tv_sec;
        end.tv_usec += 1000000;
    }
    diff.tv_usec = end.tv_usec - begin.tv_usec;
    seconds = diff.tv_sec + (diff.tv_usec / 1000000.0);
    fprintf(stderr, "blitz::Array took %f seconds; sum=%f\n", seconds, sum);

    return 0;
}


//
/// \file  pinelis_taleb_test.cpp
/// \package how_much_data
//
/// \author Created by Joseph Dunn on 12/31/18.
/// \copyright  © 2018, 2019 Joseph Dunn. All rights reserved.
//

#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
using std::ostream;
#include <iomanip>
using std::setw;
using std::setprecision;
using std::right;
using std::fixed;
using std::scientific;
using std::defaultfloat;
#include <string>
using std::string;
using std::to_string;
#include <sstream>
using std::istringstream;
using std::ostringstream;
#include <fstream>
using std::ofstream;
#include <vector>
using std::vector;
#include <array>
using std::array;
#include <algorithm>
using std::max;
using std::min;
using std::max_element;
using std::sort;
#include <mutex>
using std::mutex;
using std::unique_lock;
#include <thread>
using std::thread;

#include <complex>
using dcomplex = std::complex<double>;
using std::real;

#include <boost/timer/timer.hpp>
using boost::timer::auto_cpu_timer;
using boost::timer::cpu_timer;
#include <boost/filesystem.hpp>
using boost::filesystem::path;
using boost::filesystem::is_directory;
using boost::filesystem::create_directory;
#include <boost/math/constants/constants.hpp>
using boost::math::constants::pi;
using boost::math::constants::one_div_root_two_pi;
using boost::math::constants::one_div_two_pi;

#include "adaptive_integration.h"
using namespace adaptive_integration;

Kronrod<double> k_big(10);
int noext = 0;
double epsabs_double = 0;
double epsrel_double = 64 * std::numeric_limits<double>::epsilon();
int limit = 2000;
int verbose_integration = 0;

/// Controller of Pinelis Taleb integral.
static IntegrationController<double> ctl_double(noext, k_big,
                                         epsabs_double, epsrel_double,
                                         limit, verbose_integration);

/// Controller of beta function integrals.
static IntegrationController<double> ctl_beta(noext, k_big,
                                                epsabs_double, epsrel_double,
                                                limit, verbose_integration);

/// Controller of mad2 integrals in pareto.
static IntegrationController<double> ctl_mad2(noext, k_big,
                                              epsabs_double, epsrel_double,
                                              limit, verbose_integration);

#include "pareto_distribution.h"
template<>
IntegrationController<double>& pareto_distribution<double>::_ctl_beta{ctl_beta};
template<>
IntegrationController<double>& pareto_distribution<double>::_ctl_mad2{ctl_mad2};

#include "student_t_distribution.h"
#include "exponential_distribution.h"
#include "lognormal_distribution.h"
#include "normal_switch_mean.h"
#include "normal_switch_stddev.h"
#include "taleb_results.h"

/// sturcture passed by imbue to ostreams to use commas in numbers
struct Numpunct: public std::numpunct<char>{
protected:
  virtual char do_thousands_sep() const{return ',';}
  virtual std::string do_grouping() const{return "\03";}
};

/// return the relative differnece between two numbers
template<typename RealType>
RealType rel_err(RealType a, RealType b) {
  return fabs(a-b)/std::max(a,b);
}

/// structure holding the results of a run for a single parameter value
struct KappaResult {
  /// constructor
  KappaResult (vector<int> ns  ///< the durations of the output
               ) : ns(ns) {}
  double param;        ///< the parameter for the run
  double mad_rel_err;  ///< the relative error of mad vs theory
  vector<int> ns;      ///< the durations saved
  vector<double> mad;  ///< the mean absolute deviation by duration
  vector<double> kappa_mad; /// the kappa_mad by duration
  /// calculate the kappa from the mad and ci variables
  void calc_kappa() {
    for (size_t i = 1; i<ns.size(); ++i) {
      kappa_mad.push_back(2- (log(ns.at(i))-log(ns.at(0)))/
                          (log(mad.at(i))-log(mad.at(0))));
    }
  }
};

 /// output the result for one parameter value to an ostream
ostream& operator<< (ostream& os, const KappaResult& k) {
  os << fixed << setw(7) << setprecision(2) << k.param
  << setw(12) << setprecision(2) << k.mad_rel_err*100 << "%";
  for (size_t i=1; i<k.mad.size(); ++i) {
    os << setw(13) << setprecision(3) << k.kappa_mad.at(i-1);
  }
  os << endl;
  return os;
}

/// structure holding the results of all parameter values
struct KappaResults {
  /// constructor
  KappaResults(const vector<int>& ns,  ///< the durations
               size_t n_params,       ///< the # of params
               string param_label,     ///< the param_label
               size_t taleb_offset     ///< the column offset into Talebs table
               )
  : ns(ns), param_label(param_label), kr(n_params, ns),
    taleb_offset(taleb_offset) {}
  /// return reference to a particular result
  KappaResult& at(size_t i) {
    unique_lock<mutex> lock(kr_mutex);
    return this->kr.at(i);
  }

  void dump_results(ostream& os, string dist_name) {
    for (auto k : kr ) {
      for (size_t j=0; j<ns.size()-1 ; ++j) {
        os << setw(15) << dist_name << ","
           << setw(15) << param_label << ","
           << setw(20) << setprecision(8) << k.param << ","
           << setw(10) << ns.at(j+1) << ","
           << setw(20) << setprecision(8) << k.kappa_mad.at(j)
           << endl;
      }
    }
  }
  vector<int> ns;           ///< the durations saved
  string param_label;       ///< the name of the parameter
  vector<KappaResult> kr;   ///< the results of the runs by duration
  size_t taleb_offset;      ///< the column offset into Taleb's table
  mutex kr_mutex;           ///< a mutex for writing results
};

/// output the results for all parameter values to an ostream
ostream& operator<< (ostream& os, KappaResults& ks) {
  os << setw(7) << right << ks.param_label
     << setw(13) << right << "mad_rel_err";
  for (int i=1; i<ks.ns.size(); ++i)
    os << setw(13) << right << "kappa_mad";
  os << endl << setw(20) << " ";
  for (int i=1; i<ks.ns.size(); ++i)
    os << setw(13) << ks.ns.at(i);
  os << endl << endl;
  for (auto kr : ks.kr)
    os << kr;
  os << endl;
  if (ks.taleb_offset >0) {
    os << setw(54) << right << "Error vs Taleb's Results" << endl << endl;
    for (size_t i=0; i<ks.kr.size(); ++i) {
      os << setw(7) << setprecision(2) << ks.kr.at(i).param << setw(13) << " ";
      for (size_t j=0; j<3; ++j) {
        double kappa_mad = ks.kr.at(i).kappa_mad.at(j);
        double err = kappa_mad - taleb_results.at(i).at(j+ks.taleb_offset);
        os << setw(13) << setprecision(3) << err;
      }
      os << endl;
    }
    os << endl;
  }
  return os;
}

/// Functor used in the integratoin of the Pinelis Taleb result
template<typename Dist>
class PinelisTalebIntegrand {
  Dist& dist;
  int n;
public:

  /// construct the integrand
  PinelisTalebIntegrand(Dist& dist, ///< [in] the distribution
                        const int n        ///< [in] the duration
                       ) : dist(dist), n(n) {}

  /// return the value of the Pinelis Taleb integrand
  double operator() (double x)
  {
    double exponent = 3.;
    double omega = pow(x/(1-x),exponent);
    double domega = exponent * pow(x/(1-x),exponent-1)*(1/pow(1-x,2));
    if (!isfinite(omega)) return 0.;
    double mean = dist.mean();
    dcomplex fac = exp(dcomplex(0,-mean * omega));
    dcomplex adj_cf = fac * dist.characteristic_function(omega);
    dcomplex adj_cfp = fac* dist.characteristic_function_prime(omega)-dcomplex(0,mean)*adj_cf;
    dcomplex pow_adj_cf = (n==1) ? 1 : pow(adj_cf,n-1);
    double ret =  n * real(pow_adj_cf * adj_cfp) * domega/omega;
    if (isnan(ret) || !isfinite(ret)) {
      ostringstream oss;
      oss << " PinelisTalebIntegrand: ret is nan or infinite" << endl
          << scientific
          << "omega        = " << omega  << endl
          << "adj_cf       =  " << adj_cf.real() << " + i*" << adj_cf.imag()  << endl
          << "adj_cf^(n-1) = " << pow_adj_cf.real() << "+ i*" << pow_adj_cf.imag()  << endl
      << "adj_cfp      = " << adj_cfp.real() << " + i*" << adj_cfp.imag()  << endl;
      throw std::runtime_error(oss.str());
    } else
      return ret;
  }

  void operator() (std::vector<double>& xs)
  {
    for (double& x : xs)
      x = operator() (x);
  }
};

/// Functor used in the integration of the Pinelis Taleb result for pareto dist.
template<>
class PinelisTalebIntegrand<pareto_distribution<> > {
  pareto_distribution<>& dist;
  int n;
public:

  /// construct the integrand
  PinelisTalebIntegrand(pareto_distribution<>& dist, ///< [in] the distribution
                        const int n        ///< [in] the duration
  ) : dist(dist), n(n) {}

  /// return the value of the Pinelis Taleb integrand for pareto distribution
  /// Uses an adjusted contour of integration
  double operator() (double x) const
  {
    const dcomplex i{0.,1.};
    double exponent = 2.;
    dcomplex omega = (x<=.5) ? pow(2*x,exponent) : 1. -i * (x - .5)/(1.-x);
    dcomplex domega =(x<=.5) ? exponent * pow(2,exponent) * pow(x,exponent-1)
                             : - .5 * i / pow(1-x,2);
    if (!isfinite(omega.real()) || !isfinite(omega.imag()) ) return 0.;
    double mean = dist.mean();
    dcomplex fac = exp(-i * mean * omega);
    dcomplex adj_cf = fac * dist.characteristic_function(omega);
    dcomplex adj_cfp = fac* dist.characteristic_function_prime(omega)- i * mean * adj_cf;
    dcomplex pow_adj_cf = (n==1) ? 1 : pow(adj_cf,n-1);
    double ret =  n * real(pow_adj_cf * adj_cfp * domega/omega);
    if (isnan(ret) || !isfinite(ret)) {
      ostringstream oss;
      oss << " PinelisTalebIntegrand: ret is nan or infinite" << endl
          << "omega        = " << omega.real() << " + i*" << omega.imag()  << endl
          << "adj_cf       =  " << adj_cf.real() << " + i*" << adj_cf.imag()  << endl
          << "adj_cf^(n-1) = " << pow_adj_cf.real() << "+ i*" << pow_adj_cf.imag()  << endl
          << "adj_cfp      = " << adj_cfp.real() << " + i*" << adj_cfp.imag()  << endl;
      throw std::runtime_error(oss.str());
    } else
      return ret;
  }

  void operator() (std::vector<double>& xs) const
  {
    for (double& x : xs)
      x = operator() (x);
  }
};

/// Functor used in the integration of the Pinelis Taleb result for exp. dist.
template<>
class PinelisTalebIntegrand<exponential_distribution<> > {
  const exponential_distribution<>& dist;
  int n;
public:

  /// construct the integrand
  PinelisTalebIntegrand(const exponential_distribution<>& dist, ///< [in] the distribution
                        const int n        ///< [in] the duration
  ) : dist(dist), n(n) {}

  /// return the value of the Pinelis Taleb integrand for exp distribution
  /// Uses an adjusted contour of integration
  double operator() (double x) const
  {
    const dcomplex i{0.,1.};
    const double exponent = 2.;
    dcomplex omega = (x<=.5) ? pow(2*x,exponent) : 1. -i * (x - .5)/(1.-x);
    dcomplex domega =(x<=.5) ? exponent * pow(2,exponent) * pow(x,exponent-1)
                             : - .5 * i / pow(1-x,2);
    if (!isfinite(omega.real()) || !isfinite(omega.imag()) ) return 0.;
    double mean = dist.mean();
    dcomplex fac = exp(-i * mean * omega);
    dcomplex adj_cf = fac * dist.characteristic_function(omega);
    dcomplex adj_cfp = fac* dist.characteristic_function_prime(omega)- i * mean * adj_cf;
    dcomplex pow_adj_cf = (n==1) ? 1 : pow(adj_cf,n-1);
    double ret =  n * real(pow_adj_cf * adj_cfp * domega/omega);
    if (isnan(ret) || !isfinite(ret)) {
      ostringstream oss;
      oss << " PinelisTalebIntegrand: ret is nan or infinite" << endl
          << "omega        = " << omega.real() << " + i*" << omega.imag()  << endl
          << "adj_cf       =  " << adj_cf.real() << " + i*" << adj_cf.imag()  << endl
          << "adj_cf^(n-1) = " << pow_adj_cf.real() << "+ i*" << pow_adj_cf.imag()  << endl
          << "adj_cfp      = " << adj_cfp.real() << " + i*" << adj_cfp.imag()  << endl;
      throw std::runtime_error(oss.str());
    } else
      return ret;
  }

  void operator() (std::vector<double>& xs) const
  {
    for (double& x : xs)
      x = operator() (x);
  }
};

/// set up integrand and calculate the results for one parameter value
template<typename Dist>
void calculate_kappa(
                     vector<int> ns,  ///< [in] the durations to calculate
                     Dist dist,       ///< [in] the distribution
                     KappaResult& k,  ///< [out] the results
                     int verbose = 0 ///< [in] flag for trace
                     )
{
  if (verbose)
    cout << dist << endl;
  for (int n : ns) {
    PinelisTalebIntegrand<Dist> pti(dist, n);
    const double lower_limit = 0.;
    const double upper_limit = 1;
    vector<double> points;
    double result;
    double error;
    int neval;
    IntegrationController<double>::TerminationCode t_c;
    int last;
    double mad;

    do {
      if (points.size()==0)
        points = {lower_limit, upper_limit};
      else {
        // the last iteration failed to find a non-zero value
        // split first interval in two.
        // this happens when n gets very large
        points.insert(points.begin(),.5*(points.at(0)+points.at(1)));
      }
      ctl_double.integrate(pti, points, result, error, neval, t_c, last);
      if (verbose > 3) {
        Fmt<double> fmt;
        double rsum=0, esum=0;
        for (int i=0; i<last; i++) {
          rsum += ctl_double.subs.at(i).r;
          esum += ctl_double.subs.at(i).e;
        }

        if (t_c > 0)
          cout << ctl_double.tc_msg(t_c) << ":" << endl;
        cout << "Integral from " << fmt << points.front()
        << " to " << fmt << points.back()
        << " = " << fmt << result
        << ", with absolute error = " << fmt << error << endl
        << "Number of evaluations = " << neval
        << ", subintervals = " << last << endl
        << "rsum = " << fmt << rsum << ", esum = " << fmt << esum << endl;
        print_subs_summary(cout, ctl_double.subs, last, points);
      }
      if (verbose>=4){
        print_subs(cout, ctl_double.subs, last, points);

      }
      mad = (-2./pi<double>())*result;
    } while (false);
    if (verbose)
      cout << "n = " << n
           << ", mad = " << mad
           << ", error = " << error
           << ", neval = " << neval
           << ", t_c = " << ctl_double.tc_msg(t_c) << endl;
    k.mad.push_back(mad);
  }

  k.calc_kappa();
  k.mad_rel_err = rel_err(k.mad.at(0), dist.mad());
}

/// show proper usage after improper command line argument
void show_usage(path p     ///< [in] the path of the executable
                ) {
  cerr << "Usage: " << p.filename().string() << endl;
}

/// main program for pinelis-taleb run
int main(int argc, const char * argv[]) {
  path p(argv[0]);
  if ( argc !=1  ) {
    show_usage(p);
    return 1;
  }
/*
  {
    Kronrod<double> k_big(10);
    int noext = 0;
    double epsabs_double = std::numeric_limits<double>::epsilon()/128;
    double epsrel_double = boost::math::tools::root_epsilon<double>();
    int limit = 1000;
    int verbose_integration = 0;
    IntegrationController<double> cf_ctl(noext, k_big,
                                         epsabs_double, epsrel_double,
                                         limit, verbose_integration);
    cout << endl;
    double sigma = 1;

    int type = 2;    // Use special contour
    lognormal_distribution<> lnd(0,sigma, cf_ctl, type);
    vector<int> ns{1000000};
    KappaResult kr(ns);
    kr.param = sigma;
    calculate_kappa(ns, lnd, kr, 4);
    ofstream out("../output/pinelis_taleb_trace.out");
    ns ={1, 2, 30, 100, 1000, 1000000};
    out << setw(16) << "sigma,"
        << setw(11) << "n,"
        << setw(16) << "s,"
    << setw(20) << "integrand" << endl;
    for (auto n : ns) {
      PinelisTalebIntegrand<lognormal_distribution<> > pti(lnd, n);
      for (int i=0; i<1000; i++) {
        double s = (i+.5)/1000;
        out << setw(15) << sigma << ","
            << setw(10) << n << ","
            << setw(15) << s << ","
            << setw(20) << pti(s) << endl;
      }
    }

  }
*/

  // Add commas to the cout format
  cout.imbue({std::locale(), new Numpunct});

  // for pareto and student t distribution use the same alphas as Taleb
  vector<double> alphas;
  for (size_t i=0; i<taleb_results.size(); ++i)
    alphas.push_back(taleb_results.at(i).at(0));
  vector<double> lambdas{.5, 1, 2};

  // The durations for which MAD will be calculated
  vector<int> ns{1, 2, 30, 100, 1000, 1000000};

  string out_dir{"../output"};
  if (!is_directory(out_dir))
    create_directory(out_dir);
  ostringstream oss;
  oss << out_dir << "/pinelis_taleb_test.out";
  cout << "Writing to file " << oss.str() << endl << endl;
  ofstream out{oss.str()};
  out.imbue({std::locale(), new Numpunct});

  string dump_file_name = out_dir + "/pinelis_taleb_dump.out";
  cout << "Writing to file " << dump_file_name << endl << endl;
  cout.flush();
  ofstream dump{dump_file_name};
  dump << setw(15) << "distribution" << ","
  << setw(15) << "param_name" << ","
  << setw(20) << "param_value" << ","
  << setw(10) << "n" << ","
  << setw(20) << "kappa_mad"
  << endl;

  auto_cpu_timer t(out);
  {
    KappaResults ks_pareto(ns, alphas.size(), "alpha", 1);
    for (size_t i=0; i<alphas.size(); ++i) {
      double alpha=alphas.at(i);
      pareto_distribution<> pd(alpha);
      KappaResult kr(ns);
      kr.param = alpha;
      calculate_kappa(ns, pd, kr, true);
      ks_pareto.at(i) = kr;
    }
    out << "Pareto Distribution" << endl << endl;
    out << ks_pareto;
    ks_pareto.dump_results(dump, "pareto");
    out.flush();
  }

  {
    KappaResults ks_student(ns, alphas.size(), "alpha",4);
    for (size_t i=0; i<alphas.size(); ++i) {
      double alpha = alphas.at(i);
      student_t_distribution<> td(alpha);
      KappaResult kr(ns);
      kr.param = alpha;
      calculate_kappa(ns, td, kr, true);
      ks_student.at(i)=kr;
    }
    out << "Student t Distribution" << endl << endl;
    out << ks_student;
    ks_student.dump_results(dump, "student_t");
  }

  {
    KappaResults ks_exponential(ns, lambdas.size(), "lambda", 0);
    for (size_t i=0; i<lambdas.size(); ++i) {
      double lambda = lambdas.at(i);
      exponential_distribution<> ed(lambda);
      KappaResult kr(ns);
      kr.param = lambda;
      calculate_kappa(ns, ed, kr, true);
      ks_exponential.at(i) = kr;
    }
    out << "Exponential Distribution" << endl << endl;
    out << ks_exponential;
    ks_exponential.dump_results(dump, "exponential");
  }

  {
    Kronrod<double> k_big(10);
    int noext = 0;
    double epsabs_double = std::numeric_limits<double>::epsilon()/128;
    double epsrel_double = boost::math::tools::root_epsilon<double>();
    int limit = 1000;
    int verbose_integration = 0;
    IntegrationController<double> cf_ctl(noext, k_big,
                                         epsabs_double, epsrel_double,
                                         limit, verbose_integration);
    cout << endl;
    vector<double> sigmas = {.01, .1, .5, 1, 2, 5};
    KappaResults ks_lognormal(ns, sigmas.size(), "sigma",0);
    for (size_t i=0; i<sigmas.size(); ++i) {
      double sigma = sigmas.at(i);
      int type = 2;    // Use special contour
      lognormal_distribution<> lnd(0,sigma, cf_ctl, type);
      KappaResult kr(ns);
      kr.param = sigma;
      calculate_kappa(ns, lnd, kr, true);
      ks_lognormal.at(i)=kr;
    }
    out << "Lognormal Distribution" << endl << endl;
    out << ks_lognormal;
    ks_lognormal.dump_results(dump, "lognormal");
  }
  {
    vector<double> ds = {0, 1, 2, 3, 4, 5};
    KappaResults ks_normal_switch_mean(ns, ds.size(), "d", 0);
    for (size_t j=0; j<ds.size(); ++j) {
      double d = ds.at(j);
      normal_switch_mean<> nsm(d);
      KappaResult kr(ns);
      kr.param = d;
      calculate_kappa(ns, nsm, kr, true);
      ks_normal_switch_mean.at(j) = kr;
    }
    out << "Normal w Switching Mean" << endl << endl;
    out << ks_normal_switch_mean;
    ks_normal_switch_mean.dump_results(dump, "normal_switching_mean");
  }

  {
    vector<double> as = {1, 2, 3, 4, 5};
    KappaResults ks_normal_switch_stddev(ns, as.size(), "a", 0);
    for (size_t j=0; j<as.size(); ++j) {
      double a = as.at(j);
      normal_switch_stddev<> nss(a, .1);
      KappaResult kr(ns);
      kr.param = a;
      calculate_kappa(ns, nss, kr, true);
      ks_normal_switch_stddev.at(j) = kr;
    }
    out << "Normal w Switching Std. Dev." << endl << endl;
    out << ks_normal_switch_stddev;
    ks_normal_switch_stddev.dump_results(dump, "normal_switching_stddev");
  }

  return 0;
}


#pragma once

#include <polyfem/ElementAssemblyValues.hpp>

#include <Eigen/Sparse>
#include <vector>
#include <iostream>
#include <cmath>
#include <memory>

namespace polyfem
{
	class MassMatrixAssembler
	{
	public:
		void assemble(
			const bool is_volume,
			const int size,
			const int n_basis,
			const std::vector< ElementBases > &bases,
			const std::vector< ElementBases > &gbases,
			StiffnessMatrix &mass) const;
	};
}


/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * 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.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 *          Erik Nelson            ( eanelson@eecs.berkeley.edu )
 */

#include <geometry/essential_matrix_solver.h>

#include <camera/camera.h>
#include <camera/camera_extrinsics.h>
#include <camera/camera_intrinsics.h>
#include <geometry/eight_point_algorithm_solver.h>
#include <geometry/rotation.h>
#include <matching/feature_match.h>
#include <matching/distance_metric.h>
#include <matching/pairwise_image_match.h>
#include <math/random_generator.h>
#include <ransac/fundamental_matrix_ransac_problem.h>
#include <ransac/ransac.h>
#include <ransac/ransac_options.h>

#include <Eigen/Core>
#include <gtest/gtest.h>
#include <gflags/gflags.h>

namespace bsfm {

using Eigen::Matrix3d;
using Eigen::Vector3d;

namespace {
const int kImageWidth = 1920;
const int kImageHeight = 1080;
const double kVerticalFov = 0.5 * M_PI;
const int kFeatureMatches = 20;
} //\namespace

TEST(EssentialMatrixSolver, TestEssentialMatrixNoiseless) {

  // Create a random number generator.
  math::RandomGenerator rng(0);

  // Create some noiseless feature matches by generating 3D points and
  // projecting them into two cameras.
  Camera camera1;
  Camera camera2;

  // Give the two cameras the same intrinsics.
  CameraIntrinsics intrinsics;
  intrinsics.SetImageLeft(0);
  intrinsics.SetImageTop(0);
  intrinsics.SetImageWidth(kImageWidth);
  intrinsics.SetImageHeight(kImageHeight);
  intrinsics.SetVerticalFOV(kVerticalFov);
  intrinsics.SetFU(intrinsics.f_v());
  intrinsics.SetCU(0.5 * kImageWidth);
  intrinsics.SetCV(0.5 * kImageHeight);

  camera1.SetIntrinsics(intrinsics);
  camera2.SetIntrinsics(intrinsics);

  // Rotate and translate the second camera w.r.t. the first.
  CameraExtrinsics extrinsics1, extrinsics2;
  camera1.SetExtrinsics(extrinsics1);

  extrinsics2.Translate(-2.0, 1.0, 1.0);
  Vector3d euler_angles(Vector3d::Random()*D2R(20.0));
  extrinsics2.Rotate(EulerAnglesToMatrix(euler_angles));
  camera2.SetExtrinsics(extrinsics2);

  // Create a bunch of points in 3D, project, and match.
  FeatureMatchList feature_matches;
  while (feature_matches.size() < kFeatureMatches) {
    // Make some 3D points in front of both cameras.
    const double x = rng.DoubleUniform(-5.0, 5.0);
    const double y = rng.DoubleUniform(-5.0, 5.0);
    const double z = rng.DoubleUniform(4.0, 10.0);

    // Project the 3D point into each camera;
    double u1 = 0.0, v1 = 0.0;
    double u2 = 0.0, v2 = 0.0;
    const bool in_camera1 = camera1.WorldToImage(x, y, z, &u1, &v1);
    const bool in_camera2 = camera2.WorldToImage(x, y, z, &u2, &v2);

    // Make sure the point is visible to both cameras.
    if (!(in_camera1 && in_camera2)) {
      continue;
    }

    // Store this as a feature match.
    FeatureMatch match;
    match.feature1_.u_ = u1;
    match.feature1_.v_ = v1;
    match.feature2_.u_ = u2;
    match.feature2_.v_ = v2;
    feature_matches.push_back(match);
  }

  // Use the eight point algorithm to find the fundamental matrix for the set of
  // points.
  EightPointAlgorithmSolver ep_solver;
  FundamentalMatrixSolverOptions options;
  ep_solver.SetOptions(options);

  Matrix3d F;
  ASSERT_TRUE(ep_solver.ComputeFundamentalMatrix(feature_matches, F));

  // Make sure the computed fundamental matrix is valid.
  for (size_t ii = 0; ii < feature_matches.size(); ++ii) {
    const double u1 = feature_matches[ii].feature1_.u_;
    const double v1 = feature_matches[ii].feature1_.v_;
    const double u2 = feature_matches[ii].feature2_.u_;
    const double v2 = feature_matches[ii].feature2_.v_;
    Vector3d x1(u1, v1, 1);
    Vector3d x2(u2, v2, 1);
    EXPECT_NEAR(0.0, x2.transpose() * F * x1, 1e-8);
  }

  // Matches are noiseless so we shouldn't need RANSAC.
  // Try to get the essential matrix from the fundamental matrix.
  EssentialMatrixSolver e_solver;
  Matrix3d E = e_solver.ComputeEssentialMatrix(F, camera1.Intrinsics(),
                                               camera2.Intrinsics());

  // Extract the pose of camera 2 from E. This pose will be relative to the pose
  // of camera 1, so to test we will need the delta between cameras 1 and 2.
  Pose relative_pose;
  ASSERT_TRUE(e_solver.ComputeExtrinsics(E, feature_matches,
                                         camera1.Intrinsics(),
                                         camera2.Intrinsics(), relative_pose));

  // Both 'delta' and 'relative_pose' are in world frame.
  Pose c1(camera1.Rt());
  Pose c2(camera2.Rt());
  Pose delta = c1.Delta(c2);

  // Delta's translation is not normalized, but the essential matrix can only
  // give us normalized translations. Normalize for comparison.
  delta.SetTranslation(delta.Translation().normalized());

  EXPECT_TRUE(delta.IsApprox(relative_pose));
}

}  //\namespace bsfm


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#define NT2_UNIT_MODULE "nt2::meta::floating"

#include <boost/tr1/functional.hpp>
#include <nt2/sdk/config/types.hpp>
#include <nt2/sdk/meta/floating.hpp>
#include <boost/type_traits/is_same.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>

////////////////////////////////////////////////////////////////////////////////
// Test any with 1 types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL (  unary_floating_t32
                  , (nt2::int32_t)(nt2::int16_t)(nt2::int8_t)(float)
                    (nt2::uint32_t)(nt2::uint16_t)(nt2::uint8_t)(bool)
                  )
{
  using nt2::meta::floating;
  using std::tr1::result_of;
  using boost::is_same;

  NT2_TEST( (is_same<float, typename result_of<floating(T)>::type >::value ) );
}

NT2_TEST_CASE_TPL (  unary_floating_t64
                  , (nt2::int64_t)(nt2::uint64_t)(double)
                  )
{
  using nt2::meta::floating;
  using std::tr1::result_of;
  using boost::is_same;

  NT2_TEST( (is_same<double, typename result_of<floating(T)>::type >::value ) );
}

////////////////////////////////////////////////////////////////////////////////
// Test any with 2 types, one being 64 bits
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL ( binary_floating_t64
                  , (nt2::int64_t)(nt2::uint64_t)(double)
                  )
{
  using nt2::meta::floating;
  using std::tr1::result_of;
  using boost::is_same;

  NT2_TEST( (is_same<double, typename result_of<floating(T,double)>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,float )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,nt2::uint64_t )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,nt2::uint32_t )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,nt2::uint16_t )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,nt2::uint8_t )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,nt2::int64_t )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,nt2::int32_t )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,nt2::int16_t )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,nt2::int8_t )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,bool )>::type >::value ) );

  NT2_TEST( (is_same<double, typename result_of<floating(double,T)>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(float,T )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(nt2::uint64_t,T )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(nt2::uint32_t,T )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(nt2::uint16_t,T )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(nt2::uint8_t,T )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(nt2::int64_t,T )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(nt2::int32_t,T )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(nt2::int16_t,T )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(nt2::int8_t,T )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(bool,T )>::type >::value ) );
}

////////////////////////////////////////////////////////////////////////////////
// Test any with 2 types, one being 32 or less  bits
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL ( binary_floating_t32
                  , (nt2::int32_t)(nt2::int16_t)(nt2::int8_t)(float)
                    (nt2::uint32_t)(nt2::uint16_t)(nt2::uint8_t)(bool)
                  )
{
  using nt2::meta::floating;
  using std::tr1::result_of;
  using boost::is_same;

  NT2_TEST( (is_same<double, typename result_of<floating(T,double)>::type >::value ) );
  NT2_TEST( (is_same<float , typename result_of<floating(T,float )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,nt2::uint64_t )>::type >::value ) );
  NT2_TEST( (is_same<float , typename result_of<floating(T,nt2::uint32_t )>::type >::value ) );
  NT2_TEST( (is_same<float , typename result_of<floating(T,nt2::uint16_t )>::type >::value ) );
  NT2_TEST( (is_same<float , typename result_of<floating(T,nt2::uint8_t )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(T,nt2::int64_t )>::type >::value ) );
  NT2_TEST( (is_same<float , typename result_of<floating(T,nt2::int32_t )>::type >::value ) );
  NT2_TEST( (is_same<float , typename result_of<floating(T,nt2::int16_t )>::type >::value ) );
  NT2_TEST( (is_same<float , typename result_of<floating(T,nt2::int8_t )>::type >::value ) );
  NT2_TEST( (is_same<float , typename result_of<floating(T,bool )>::type >::value ) );

  NT2_TEST( (is_same<double, typename result_of<floating(double,T)>::type >::value ) );
  NT2_TEST( (is_same<float, typename result_of<floating(float,T )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(nt2::uint64_t,T )>::type >::value ) );
  NT2_TEST( (is_same<float, typename result_of<floating(nt2::uint32_t,T )>::type >::value ) );
  NT2_TEST( (is_same<float, typename result_of<floating(nt2::uint16_t,T )>::type >::value ) );
  NT2_TEST( (is_same<float, typename result_of<floating(nt2::uint8_t,T )>::type >::value ) );
  NT2_TEST( (is_same<double, typename result_of<floating(nt2::int64_t,T )>::type >::value ) );
  NT2_TEST( (is_same<float, typename result_of<floating(nt2::int32_t,T )>::type >::value ) );
  NT2_TEST( (is_same<float, typename result_of<floating(nt2::int16_t,T )>::type >::value ) );
  NT2_TEST( (is_same<float, typename result_of<floating(nt2::int8_t,T )>::type >::value ) );
  NT2_TEST( (is_same<float, typename result_of<floating(bool,T )>::type >::value ) );
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_CONTAINER_CONTAINER_HPP__
#define __OPTIMISATION2_CONTAINER_CONTAINER_HPP__

#include <cstddef>
#include <libv/lma/lm/trait/accessor.hpp>
#include <libv/lma/lm/trait/adl.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/utility/enable_if.hpp>
#include "residual.hpp"
#include "../ba/nan_error.hpp"

namespace lma
{
  inline bool is_invalid(double d) { return !std::isfinite(d);}
  inline bool is_valid(double d) { return !is_invalid(d); }

  template<class> struct Rows;
  template<class> struct Cols;

  template<class Tag, std::size_t I, std::size_t J> struct ContainerOption {};

  template<class Tag, std::size_t X, std::size_t Y> struct Blocker;

  template<class Float, class Tag>
  const Float& make_view(const Float& residual, Tag, typename boost::enable_if<boost::is_floating_point<Float>>::type* =0)
  {
    return residual;
  }

  template<class Float> const Float& transpose(const Float& f)
  {
    return f;
  }

  template<class Float>
  Float squared_norm(Float d, typename boost::enable_if<boost::is_floating_point<Float>>::type* =0)
  { return d*d;}

  template<class Float, size_t N>
  Eigen::Map<Eigen::Matrix<Float,N,1>> array_to_eigen(std::array<Float,N>& array)
  {
    return Eigen::Map<Eigen::Matrix<Float,N,1>>(array.data());
  }

////////////////////////////////////////////////////////
  template<class Float> void apply_small_increment(Float& obj, Float delta, const v::numeric_tag<0> &, const Adl&, typename boost::enable_if<boost::is_floating_point<Float>>::type* =0)
  {
    obj += delta;
  }
  template<class Float> void apply_increment(Float& o, const Float* inc, const Adl&, typename boost::enable_if<boost::is_floating_point<Float>>::type* =0)
  {
    o += inc[0];
  }
////////////////////////////////////////////////////////

  namespace detail
  {
    template<class Obj, class Float> void internal_apply_increment(Obj& o, const Float* inc)
    {
      apply_increment(o,inc,Adl());
    }

    template<class Obj, class Float, int I> void internal_apply_small_increment(Obj& o, Float flt, const v::numeric_tag<I> &tag)
    {
      apply_small_increment(o,flt,tag,Adl());
    }
  }
////////////////////////////////////////////////////////

  template <class T, int n> void invert_symmetric_matrix(T a[n][n], T ai[n][n])
  {
    for(int i = 0; i < n; ++i)
    {
      if(a[i][i] > 0)
      {
      	a[i][i] = std::sqrt(a[i][i]);
      	for(int j = i + 1; j < n; ++j)
      	  a[j][i] = a[j][i] / a[i][i];
      	for(int j = i + 1; j < n; ++j)
      	  for(int k = j; k < n; ++k)
      	    a[k][j] -= a[k][i] * a[j][i];
      }
    }

    for(int i = 0; i < n; ++i)
    {
      if(a[i][i] != 0)
	      a[i][i] = 1.0f / a[i][i];
    }
    for(int i = 1; i < n; ++i)
    {
      if(a[i][i] != 0)
      {
      	for(int j = 0; j < i; ++j)
      	{
      	  T sum  = 0;
      	  for(int  k = j; k < i; ++k)
      	    sum += (a[i][k] * a[k][j]);
      	  a[i][j] = - sum * a[i][i];
      	}
      }
    }

    for(int i = 0; i < n; ++i)
    {
      for(int j = i; j < n; ++ j)
      {
      	ai[i][j] = 0;
      	for(int k  = j; k < n; ++k)
      	  ai[i][j] += a[k][i] * a[k][j];
      	ai[j][i] = ai[i][j];
      }
    }
  }

  template <class T, int n> void invert_symmetric_matrix(T * a, T * ai)
  {
    invert_symmetric_matrix<T,n>((T(*)[n]) a, (T(*)[n]) ai);
  }

  template<class Matrix, class Vector>
  Vector llt(Matrix u, Vector x, int size)
  {
    for(int i = 0 ; i < size ; ++i)
    {
      for(int k = 0 ; k < i ; ++k)
        u(i,i) -= u(k,i) * u(k,i);

      assert(u(i,i)>0);
      if (u(i,i)<=0) throw ZeroOrInfiniteError("LLT");
      u(i,i) = std::sqrt(u(i,i));

      for(int j = i + 1; j < size ; ++j)
      {
        for(int k = 0 ; k < i ; ++k)
          u(i,j) -= u(k,i) * u(k,j);
        u(i,j) /= u(i,i);
      }
    }

    for(int j = 0 ; j < size; ++j)
    {
      for(int i = 0 ; i < j ; ++i)
        x[j] -= u(i,j) * x[i];
      x[j] /= u(j,j);
    }

    for(int j = size - 1 ; j >=0  ; --j)
    {
      for(int i = j+1 ; i < size ; ++i)
        x[j] -= u(j,i) * x[i];
      x[j] /= u(j,j);
    }
    return x;
  }

  template<class VecD, class MatD>
  void ldlt_solve(VecD& x, const MatD& a, const VecD& b)
  {
    x = llt(a,b,b.size());
  }

  inline double dot(double a, double b)
  {
    return a*b;
  }

}

#include "eigen.hpp"

#ifdef USE_TOON
#include "toon.hpp"
#endif

#endif


/**
 * @file 	GraphParser.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jul 14, 2017
 */

#include <boost/spirit/include/qi.hpp>
#include "GraphParser.h"

using std::string;
using namespace boost::spirit;
using qi::int_;
using qi::double_;
using qi::phrase_parse;
using ascii::space;

GraphParser::GraphParser(string file) {
	input_file.open(file);
	opened_successfully = !input_file.fail();
}

bool GraphParser::read_first_line(int& num_vertices, int& num_edges) {

	string line;
	getline(input_file, line);	// first line is read

	auto it = line.begin();
	int n, m;
	bool success = phrase_parse(it, line.end(),
			int_[([&n](int j){ n = j; })]
				 >> int_[([&m](int j){ m = j; })], space);

	if (!success || it != line.end()) return false;
	num_vertices = n;
	num_edges = 2*m;

	return true;
}

/*
 * Implementation of the graph parser read method by means of boost::spirit
 */
bool GraphParser::read_edge_data(Edge edges[], weight_type weights[]) {

	assert(NULL != edges);
	assert(NULL != weights);

	// we use boost::spirit to parse each line
	string line;

	int i = 0;

	// read line by line
	while (getline(input_file, line)) {

		auto it = line.begin();
		int start;
		int end;
		double weight;
		bool success = phrase_parse(it, line.end(),
				int_[([&start](int j){ start = j; })]
					 >> int_[([&end](int j){ end = j; })]
							 >> double_[([&weight](weight_type j){ weight = j; })], space);

		if (success && it == line.end()) {

			// since graph is undirected we add both directions for each edge
			edges[2*i] = Edge(start-1, end-1);
			edges[2*i+1] = Edge(end-1, start-1);
			weights[2*i] = weight;
			weights[2*i+1] = weight;

		} else {
			return false;
		}
		++i;
	}

	return true;
}


/*
 DepSpawn: Data Dependent Spawn library
 Copyright (C) 2012-2021 Carlos H. Gonzalez, Basilio B. Fraguela. Universidade da Coruna

 Distributed under the MIT License. (See accompanying file LICENSE)
*/

///
/// \author   Carlos H. Gonzalez  <cgonzalezv@udc.es>
/// \author   Basilio B. Fraguela <basilio.fraguela@udc.es>
///

#include <iostream>
#include <cstdlib>
#include <sstream>
#include <blitz/array.h>
#include "depspawn/depspawn.h"
#include "common_io.cpp"  // This is only for serializing parallel prints

using namespace depspawn;
using namespace blitz;

#define N     30000
#define CHUNK (N/10)

Array<int , 1> r(10), a(N);

std::stringstream Log;

void addreduce(Array<int, 1>& result, const Array<int , 1>& values)
{
  const long long int dest_addr = ((long long int)&result(0));
  const int nchunk = (dest_addr - ((long long int)&r(0))) / sizeof(int);

  if( (&values(0)) == (&r(0)) ) {
    LOG("B Final reduction");
  } else {
    LOG("B chk " << nchunk);
  }

  result(0) = 0;

  for(int i = 0; i < values.size(); i++)
    result(0) += values(i);

  if( (&values(0)) == (&r(0)) ) {
    LOG("Added " << values.size() << " vals->" << result(0) << " . Final");
  } else {
    //LOG("Added " << values.size() << " vals->" << result(0) << " into addr " << dest_addr << " chk" << nchunk);
    LOG("E chk " << nchunk);
  }
}

void print(const Array<int , 1>& v, const char * msg = 0)
{
  if(msg)
    std::cout << msg << " : ";
  std::cout << "[ ";
  for(int i = 0; i < 10; i++)
    std::cout << v(i) << ' ';
  std::cout << "]\n";
}

int main(int argc, char **argv)
{ int i;
  Array<int, 1> result(1);

  const int nthreads = (argc == 1) ? 8 : atoi(argv[1]);

  set_threads(nthreads);

  std::cout << "Running with " << nthreads << " threads" << std::endl;

  for (i = 0; i < N; i++) {
    a(i) = i;
  }

  for(i=0; i< N; i+=CHUNK) {
    //LOG("Sp " << (i/10));
    spawn(addreduce, r(Range(i/CHUNK, i/CHUNK)), a(Range(i, i+CHUNK-1)));
  }

  LOG("Dependent spawn");

  spawn(addreduce, result(Range(0, 0)), r);

  wait_for_all();

  //print(a, "First ten values of array a to add");
  //print(r, "Ten partial reductions of ten consecutive elements");

  //std::cout << "The addition of the values 0 to 99 is " << result(0) << std::endl;
  //std::cout << "Should be " << 99 * 50 << std::endl;

  std::cout << Log.str();
  const bool test_ok = (result(0) == ((N/2)*(N-1)));
  std::cout << "TEST " << (test_ok ? "SUCCESSFUL" : "UNSUCCESSFUL") << std::endl;

  return !test_ok;
}


/*
 * @file test_Hermite.cpp
 * @date Feb 11, 2015
 * @author Abel Gawel, Renaud Dube
 */

#include <gtest/gtest.h>
#include <curves/CubicHermiteSE3Curve.hpp>
#include "gtsam/nonlinear/Expression.h"
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include "gtsam/nonlinear/ExpressionFactor.h"
#include <gtsam/base/numericalDerivative.h>
#include "kindr/minimal/testing-gtsam.h"

#include "curves/helpers.hpp"

#include <Eigen/Core>
#include <boost/assign/list_of.hpp>

#define DIM 6

using namespace curves;
using namespace gtsam;
using namespace std;

typedef typename CubicHermiteSE3Curve::ValueType ValueType;
typedef typename CubicHermiteSE3Curve::Coefficient Coefficient;
typedef typename Eigen::Matrix<double, 6,1> Vector6d;
typedef typename CubicHermiteSE3Curve::DerivativeType DerivativeType;
typedef typename kindr::minimal::HermiteTransformation<double> HermiteTransformation;
typedef kindr::minimal::QuatTransformationTemplate<double> SE3;
typedef SE3::Rotation SO3;

// Test the HermiteTransfoamtion Coefficient struct
// todo move this test together with the HermiteTransformation to minkindr_gtsam
TEST(CurvesTestSuite, testHermiteTransformationStruct) {

  using gtsam::Expression;
  gtsam::Values values;

  // Create some values
  for (int i=1; i<6; ++i) {
    SO3 Rval;
    Eigen::Vector3d tval;
    Eigen::Vector3d wval;
    Eigen::Vector3d vval;
    Vector6d dval;

    Rval.setRandom();
    tval.setRandom();
    wval.setRandom();
    vval.setRandom();
    dval << vval, wval;
    ValueType T(Rval, tval);

    HermiteTransformation HT(T, dval);
    values.insert(i, HT);
    kindr::minimal::EHermiteTransformation EHT(i);
    EXPECT_TRUE(EHT.value(values).getTransformation() == HT.getTransformation());
    EXPECT_TRUE(EHT.value(values).getTransformationDerivative() ==
        HT.getTransformationDerivative());

    const double fd_step = 1e-9;
    const double tolerance = 1e-6;
    SCOPED_TRACE("Testing Expression Jacobians.");
    gtsam::testExpressionJacobians(EHT, values, fd_step, tolerance);
  }
}

// Test the HermiteTransfoamtion Coefficient expressions
TEST(CurvesTestSuite, testHermiteTransformationExpressions) {
  using gtsam::Expression;
  gtsam::Values values;

  // Create some values
  SO3 Rval;
  Eigen::Vector3d tval;
  Eigen::Vector3d wval;
  Eigen::Vector3d vval;
  Vector6d dval;

  Rval.setRandom();
  tval.setRandom();
  wval.setRandom();
  vval.setRandom();
  dval << vval, wval;
  ValueType T(Rval, tval);

  HermiteTransformation HT(T, dval);
  values.insert(1, HT);
  values.insert(2, T);
  kindr::minimal::EHermiteTransformation EHT(1);
  kindr::minimal::ETransformation ET(2);

  kindr::minimal::ETransformation eTransform =
      transformationFromHermiteTransformation(EHT);

  kindr::minimal::EVector3 eAngVel =
      angularVelocitiesFromHermiteTransformation(EHT);

  kindr::minimal::EVector3 eVel =
      velocitiesFromHermiteTransformation(EHT);

  const double fd_step = 1e-9;
  const double tolerance = 1e-6;
  SCOPED_TRACE("Testing Expression Jacobians.");
  testExpressionJacobians(EHT, values, fd_step, tolerance);
  testExpressionJacobians(eTransform, values, fd_step, tolerance);
  testExpressionJacobians(eAngVel, values, fd_step, tolerance);
  testExpressionJacobians(eVel, values, fd_step, tolerance);

}

TEST(CurvesTestSuite, testCubicHermiteVelocityScale) {
  HermiteTransformation H1, H2;

  SE3 T1(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(0,0,0));
  SE3 T2(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(10,20,30));
  SE3 Ti(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(2.5,5,7.5));

  Vector3 v1, v2, vr1, vr2, vi, vri;
  v1 << 1,2,3;
  v2 = v1;
  vi = v1;
  vr1 << 0,0,0;
  vr2 = vr1;
  vri = vr1;

  double t1, t2, ti;
  t1 = 0;
  t2 = 10;
  ti = 2.5;

  double alpha = (ti-t1)/(t2-t1);
  Expression<SE3> eT1(1), eT2(2);
  Expression<Vector3> ev1(3), ev2(4), evr1(5), evr2(6);
  gtsam::Values values;
  values.insert(1, T1);
  values.insert(2, T2);
  values.insert(3, v1);
  values.insert(4, v2);
  values.insert(5, vr1);
  values.insert(6, vr2);

  Expression<Vector3> eInterpolated = kindr::minimal::hermiteInterpolation<Vector3>(
      kindr::minimal::translationFromTransformation(eT1),
      ev1,
      kindr::minimal::translationFromTransformation(eT2),
      ev2,
      alpha,
      t2 - t1);

  Vector3 TiExpectedTrans = eInterpolated.value(values);
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(Ti.getPosition(), TiExpectedTrans, 1e-8));

  Expression<SO3> erInterpolated = kindr::minimal::hermiteQuaternionInterpolation(
        kindr::minimal::rotationFromTransformation(eT1),
        evr1,
        kindr::minimal::rotationFromTransformation(eT2),
        evr2,
        alpha,
        t2 - t1);

  SO3 TiExpectedRot = erInterpolated.value(values);
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(Ti.getRotation().vector(), TiExpectedRot.vector(), 1e-8));

}

// test for correct keys and evaluation function in Slerp SE3 curves
TEST(CurvesTestSuite, testCubicHermiteEvaluation) {

  // setup three SE3 objects
  const Time t[] = {0, Time(1e9), Time(2e9)};
  std::vector<Time> times(t,t+3);
  std::vector<SE3> values;
  for (size_t i = 0; i<3; ++i) {
    SO3 Rval;
    Eigen::Vector3d tval;
//    Rval = SO3(1,SO3::Vector3(0,0,0));
    Rval.setRandom();
    tval.setRandom();
    SE3 T(Rval, tval);
    values.push_back(T);
  }

  // interpolate curve
  CubicHermiteSE3Curve curve;
  curve.extend(times, values);
  curve.print("curve ");
  // fill retrieved coefficients in gtsam values container
  Values gtsamValues;
  curve.initializeGTSAMValues(&gtsamValues);

  // ensure that getValuexpression and evaluate produce same values
  for (Time ti = 0; ti < 2e9; ti+=0.1e9) {
    std::vector<std::string> pose, pose2;
    SE3 eval = curve.evaluate(ti);
    Expression<ValueType> expressionC = curve.getValueExpression(ti);
    ValueType resultC = expressionC.value(gtsamValues);

    if (!gtsam::traits<SE3>::Equals(eval, resultC, 1e-8)) {
      std::cout << "ti " << ti*1e-9 << std::endl;
      gtsam::traits<SE3>::Print(eval, "eval ");
      gtsam::traits<SE3>::Print(resultC, "expression ");
    }
    EXPECT_TRUE(gtsam::traits<SE3>::Equals(eval, resultC, 1e-8));

  }
}

#if 0
// test optimization of gtsam factor graph using expressions
TEST(CurvesTestSuite, testHermiteExpressionGTSAMoptimization) {

  CubicHermiteSE3Curve curve;

  // Populate coefficients
  const double t[] = {0, 10, 20, 30, 40};
  std::vector<Time> times(t,t+5);
  std::vector<ValueType> coefficients;
  coefficients.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(0,0,0)));
  coefficients.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(10,0,0)));
  coefficients.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(20,0,0)));
  coefficients.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(30,0,0)));
  coefficients.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(40,0,0)));

  // Populate measurements
  const double tmeas[] = {4, 8, 12, 16, 20, 24, 28, 32, 36};
  std::vector<Time> measTimes(tmeas,tmeas+9);
  std::vector<ValueType> measurements;
  measurements.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(4,0,0)));
  measurements.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(8,0,0)));
  measurements.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(12,0,0)));
  measurements.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(16,0,0)));
  measurements.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(20,0,0)));
  measurements.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(24,0,0)));
  measurements.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(28,0,0)));
  measurements.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(32,0,0)));
  measurements.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(36.0,0,0)));

  // Fit a curve
  std::vector<gtsam::Key> outKeys;
  curve.extend(times, coefficients, &outKeys);

  // Populate GTSAM values
  Values initials, expected;
  curve.initializeGTSAMValues(outKeys, &expected);

  for(size_t i=0; i< coefficients.size(); i++) {
    Eigen::Matrix<double, 6, 1> derivatives;
    derivatives << 0,0,0,0,0,0;
    ValueType pose(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(0,0,0));
    kindr::minimal::HermiteTransformation<double> init(pose, derivatives);
    initials.insert(outKeys[i],init);
  }

  // Noise models
  gtsam::Vector6 measNoise;
  Eigen::Matrix<double, 6, 1> priorNoise;
  priorNoise << 0,0,0,0,0,0;
  measNoise << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
  SharedNoiseModel measNoiseModel = noiseModel::Diagonal::Sigmas(measNoise);
  SharedNoiseModel priorNoiseModel = noiseModel::Diagonal::Sigmas(priorNoise);

  // Get expressions and build the graph
  NonlinearFactorGraph graph;
  for(size_t i=0; i < measurements.size(); i++) {
    Expression<ValueType> predicted(curve.getValueExpression(measTimes[i]));

    ExpressionFactor<ValueType> f(measNoiseModel,
                                  ValueType(measurements[i]),
                                  predicted);
    graph.add(f);
    // Assert that error is null for expected values
    std::vector<Matrix> H(2);
    Vector error = f.unwhitenedError(expected, H);
    EXPECT_TRUE(EIGEN_MATRIX_NEAR(error, gtsam::Vector6::Zero(), 1e-5));
  }

  // Add a prior of 0,0,0 on the first coefficient
  Expression<Coefficient> leaf1(outKeys[0]);
  Expression<ValueType> prior = kindr::minimal::transformationFromHermiteTransformation(leaf1);
  Eigen::Matrix<double, 6, 1> derivatives;
  derivatives << 0,0,0,0,0,0;
  ValueType priorPose(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(0,0,0));
  graph.add(ExpressionFactor<ValueType>(priorNoiseModel,priorPose,prior));

  // Optimize
  gtsam::LevenbergMarquardtParams params;
  params.setVerbosity("LINEAR");
  gtsam::Values result = gtsam::LevenbergMarquardtOptimizer(graph, initials, params).optimize();
  ASSERT_TRUE(expected.equals(result,1e-5));
}

ExpressionFactor<ValueType>
getFactorRelativeMeasurement(const CubicHermiteSE3Curve& curve,
                             Time timeA, Time timeB,
                             ValueType measurement,
                             noiseModel::Diagonal::shared_ptr noiseModel) {
  Expression<ValueType> TA(curve.getValueExpression(timeA));
  Expression<ValueType> TB(curve.getValueExpression(timeB));
  Expression<ValueType> predicted = kindr::minimal::invertAndCompose(TA,TB);
  return ExpressionFactor<ValueType>(noiseModel, measurement, predicted);
}

TEST(CurvesTestSuite, testHermiteSE3RelativeExpression){
  CubicHermiteSE3Curve curve;
  // Data taken from the MITb dataset
  // Populate expected
  const double t[] = {0, 1, 2};
  std::vector<Time> times(t,t+3);
  std::vector<ValueType> expected;
  expected.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(0,0,0)));
  expected.push_back(ValueType(SO3(0.999967,SO3::Vector3(0,0,0.00760239)),SE3::Position(2.03998,0.00824351,0)));
  expected.push_back(ValueType(SO3(0.999803,SO3::Vector3(0,0,0.019962)),SE3::Position(4.26771,0.0576598,0)));

  // Populate initials
  std::vector<ValueType> initials;
  initials.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(0,0,0)));
  initials.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(0,0,0)));
  initials.push_back(ValueType(SO3(1,SO3::Vector3(0,0,0)),SE3::Position(0,0,0)));

  // Populate measurements
  const double tmeas[] = {1, 2};
  std::vector<Time> measTimes(tmeas,tmeas+2);
  std::vector<ValueType> measurements;
  measurements.push_back(ValueType(SO3(0.99997,SO3::Vector3(0,0,0.0072259)),SE3::Position(2.0393,0.003006,0)));
  measurements.push_back(ValueType(SO3(0.99991,SO3::Vector3(0,0,0.013399)),SE3::Position(2.2275,0.023206,0)));

  // Fit curve
  std::vector<gtsam::Key> outKeys;
  curve.extend(times, initials, &outKeys);

  //Noise models
  Vector6 measNoise;
  measNoise << 0.1, 0.1, 0.001, 0.1*M_PI/180, 0.1*M_PI/180, 0.5*M_PI/180;
  noiseModel::Diagonal::shared_ptr measNoiseModel = noiseModel::Diagonal::Sigmas(measNoise);
  Vector6 priNoise;
  priNoise << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(priNoise);

  NonlinearFactorGraph graph;
  Values gtsamInitial, gtsamExpected;
  curve.initializeGTSAMValues(&gtsamInitial);
  for(size_t i=0; i< outKeys.size(); i++) {
    gtsamExpected.insert(outKeys[i],expected[i]);
  }

  //prior
  Expression<ValueType> predictedPrior = curve.getValueExpression(times[0]);
  ExpressionFactor<ValueType> f(priorNoise, initials[0], predictedPrior);
  graph.push_back(f);

  //relative measurements
  graph.push_back(getFactorRelativeMeasurement(curve, measTimes[0]-1, measTimes[0],
                                               measurements[0], measNoiseModel));
  graph.push_back(getFactorRelativeMeasurement(curve, measTimes[1]-1, measTimes[1],
                                               measurements[1], measNoiseModel));

  // optimize the trajectory
  gtsam::LevenbergMarquardtParams params;
  gtsam::Values result = gtsam::LevenbergMarquardtOptimizer(graph, gtsamInitial, params).optimize();

//  gtsamExpected.print("expected:");
//  result.print("result: ");
  ASSERT_TRUE(gtsamExpected.equals(result, 0.5));
}
#endif