#include "CircuitSimulator.hpp"
#include "GroverSimulator.hpp"
#include "HybridSchrodingerFeynmanSimulator.hpp"
#include "ShorFastSimulator.hpp"
#include "ShorSimulator.hpp"
#include "Simulator.hpp"
#include "algorithms/Entanglement.hpp"
#include "algorithms/Grover.hpp"
#include "algorithms/QFT.hpp"
#include "dd/Export.hpp"
#include "nlohmann/json.hpp"

#include <boost/program_options.hpp>
#include <chrono>
#include <fstream>
#include <iostream>
#include <memory>
#include <string>

namespace nl = nlohmann;

int main(int argc, char** argv) {
    namespace po = boost::program_options;
    // variables initialized by boost program_options default values
    unsigned long long seed;
    unsigned int       shots;

    unsigned int                         approx_steps;
    double                               step_fidelity;
    ApproximationInfo::ApproximationWhen approx_when;

    HybridSchrodingerFeynmanSimulator::Mode mode = HybridSchrodingerFeynmanSimulator::Mode::Amplitude;
    unsigned int                            nthreads;

    po::options_description description("JKQ DDSIM by https://iic.jku.at/eda/ -- Allowed options");
    // clang-format off
    description.add_options()
        ("help,h", "produce help message")
        ("seed", po::value<>(&seed)->default_value(0), "seed for random number generator (default zero is possibly directly used as seed!)")
        ("shots", po::value<>(&shots)->default_value(0), "number of measurements (if the algorithm does not contain non-unitary gates, weak simulation is used)")
        ("pv", "display the state vector")
        ("ps", "print simulation stats (applied gates, sim. time, and maximal size of the DD)")
        ("pm", "print measurement results")
        ("pcomplex", "print additional statistics on complex numbers")
        ("dump_complex", po::value<std::string>(), "dump edge weights in final state DD to file")
        ("verbose", "Causes some simulators to print additional information to STDERR")
        ("simulate_file", po::value<std::string>(), "simulate a quantum circuit given by file (detection by the file extension)")
        ("simulate_file_hybrid", po::value<std::string>(), "simulate a quantum circuit given by file (detection by the file extension) using the hybrid Schrodinger-Feynman simulator")
        ("hybrid_mode", po::value<std::string>(), "mode used for hybrid Schrodinger-Feynman simulation (*amplitude*, dd)")
        ("nthreads", po::value<>(&nthreads)->default_value(2), "#threads used for hybrid simulation")
        ("simulate_qft", po::value<unsigned int>(), "simulate Quantum Fourier Transform for given number of qubits")
        ("simulate_ghz", po::value<unsigned int>(), "simulate state preparation of GHZ state for given number of qubits")
        ("step_fidelity", po::value<>(&step_fidelity)->default_value(1.0), "target fidelity for each approximation run (>=1 = disable approximation)")
        ("steps", po::value<>(&approx_steps)->default_value(1), "number of approximation steps")
        ("approx_when", po::value<>(&approx_when)->default_value(ApproximationInfo::FidelityDriven), "approximation method ('fidelity' (default) or 'memory'")
        ("approx_state", "do excessive approximation runs at the end of the simulation to see how the quantum state behaves")
        ("simulate_grover", po::value<unsigned int>(), "simulate Grover's search for given number of qubits with random oracle")
        ("simulate_grover_emulated", po::value<unsigned int>(), "simulate Grover's search for given number of qubits with random oracle and emulation")
        ("simulate_grover_oracle_emulated", po::value<std::string>(), "simulate Grover's search for given number of qubits with given oracle and emulation")
        ("simulate_shor", po::value<unsigned int>(), "simulate Shor's algorithm factoring this number")
        ("simulate_shor_coprime", po::value<unsigned int>()->default_value(0), "coprime number to use with Shor's algorithm (zero randomly generates a coprime)")
        ("simulate_shor_no_emulation", "Force Shor simulator to do modular exponentiation instead of using emulation (you'll usually want emulation)")
        ("simulate_fast_shor", po::value<unsigned int>(), "simulate Shor's algorithm factoring this number with intermediate measurements")
        ("simulate_fast_shor_coprime", po::value<unsigned int>()->default_value(0),"coprime number to use with Shor's algorithm (zero randomly generates a coprime)");
    // clang-format on
    po::variables_map vm;
    try {
        po::store(po::parse_command_line(argc, argv, description), vm);

        if (vm.count("help")) {
            std::cout << description;
            return 0;
        }
        po::notify(vm);
    } catch (const po::error& e) {
        std::cerr << "[ERROR] " << e.what() << "! Try option '--help' for available commandline options.\n";
        std::exit(1);
    }

    std::unique_ptr<qc::QuantumComputation> quantumComputation;
    std::unique_ptr<Simulator>              ddsim{nullptr};
    ApproximationInfo                       approx_info(step_fidelity, approx_steps, approx_when);
    const bool                              verbose = vm.count("verbose") > 0;

    if (vm.count("simulate_file")) {
        const std::string fname = vm["simulate_file"].as<std::string>();
        quantumComputation      = std::make_unique<qc::QuantumComputation>(fname);
        ddsim                   = std::make_unique<CircuitSimulator>(std::move(quantumComputation), approx_info, seed);
    } else if (vm.count("simulate_file_hybrid")) {
        const std::string fname = vm["simulate_file_hybrid"].as<std::string>();
        quantumComputation      = std::make_unique<qc::QuantumComputation>(fname);
        if (vm.count("hybrid_mode")) {
            const std::string mname = vm["hybrid_mode"].as<std::string>();
            if (mname == "amplitude") {
                mode = HybridSchrodingerFeynmanSimulator::Mode::Amplitude;
            } else if (mname == "dd") {
                mode = HybridSchrodingerFeynmanSimulator::Mode::DD;
            }
        }
        if (seed != 0) {
            ddsim = std::make_unique<HybridSchrodingerFeynmanSimulator>(std::move(quantumComputation), approx_info, seed, mode, nthreads);
        } else {
            ddsim = std::make_unique<HybridSchrodingerFeynmanSimulator>(std::move(quantumComputation), mode, nthreads);
        }
    } else if (vm.count("simulate_qft")) {
        const unsigned int n_qubits = vm["simulate_qft"].as<unsigned int>();
        quantumComputation          = std::make_unique<qc::QFT>(n_qubits);
        ddsim                       = std::make_unique<CircuitSimulator>(std::move(quantumComputation), approx_info, seed);
    } else if (vm.count("simulate_fast_shor")) {
        const unsigned int composite_number = vm["simulate_fast_shor"].as<unsigned int>();
        const unsigned int coprime          = vm["simulate_fast_shor_coprime"].as<unsigned int>();
        if (seed == 0) {
            ddsim = std::make_unique<ShorFastSimulator>(composite_number, coprime, verbose);
        } else {
            ddsim = std::make_unique<ShorFastSimulator>(composite_number, coprime, seed, verbose);
        }
    } else if (vm.count("simulate_shor")) {
        const unsigned int composite_number = vm["simulate_shor"].as<unsigned int>();
        const unsigned int coprime          = vm["simulate_shor_coprime"].as<unsigned int>();
        const bool         emulate          = vm.count("simulate_shor_no_emulation") == 0;
        if (seed == 0) {
            ddsim = std::make_unique<ShorSimulator>(composite_number, coprime, emulate, verbose, step_fidelity < 1);
        } else {
            ddsim = std::make_unique<ShorSimulator>(composite_number, coprime, seed, emulate, verbose,
                                                    step_fidelity < 1);
        }
    } else if (vm.count("simulate_grover")) {
        const unsigned int n_qubits = vm["simulate_grover"].as<unsigned int>();
        quantumComputation          = std::make_unique<qc::Grover>(n_qubits, seed);
        ddsim                       = std::make_unique<CircuitSimulator>(std::move(quantumComputation), approx_info, seed);
    } else if (vm.count("simulate_grover_emulated")) {
        ddsim = std::make_unique<GroverSimulator>(vm["simulate_grover_emulated"].as<unsigned int>(), seed);
    } else if (vm.count("simulate_grover_oracle_emulated")) {
        ddsim = std::make_unique<GroverSimulator>(vm["simulate_grover_oracle_emulated"].as<std::string>(), seed);
    } else if (vm.count("simulate_ghz")) {
        const unsigned int n_qubits = vm["simulate_ghz"].as<unsigned int>();
        quantumComputation          = std::make_unique<qc::Entanglement>(n_qubits);
        ddsim                       = std::make_unique<CircuitSimulator>(std::move(quantumComputation), approx_info, seed);
    } else {
        std::cerr << "Did not find anything to simulate. See help below.\n"
                  << description << "\n";
        std::exit(1);
    }

    if (ddsim->getNumberOfQubits() > 100) {
        std::clog << "[WARNING] Quantum computation contains quite a few qubits. You're jumping into the deep end.\n";
    }

    auto t1 = std::chrono::high_resolution_clock::now();
    auto m  = ddsim->Simulate(shots);
    auto t2 = std::chrono::high_resolution_clock::now();

    std::chrono::duration<float> duration_simulation = t2 - t1;

    if (vm.count("approx_state")) {
        // TargetFidelity
        ddsim->ApproximateByFidelity(1 / 100.0, false, false, true);
        ddsim->ApproximateByFidelity(2 / 100.0, false, false, true);
        ddsim->ApproximateByFidelity(3 / 100.0, false, false, true);
        ddsim->ApproximateByFidelity(4 / 100.0, false, false, true);
        for (int i = 6; i <= 95; i += 2) {
            ddsim->ApproximateByFidelity(i / 100.0, false, false, true);
        }
        ddsim->ApproximateByFidelity(96 / 100.0, false, false, true);
        ddsim->ApproximateByFidelity(97 / 100.0, false, false, true);
        ddsim->ApproximateByFidelity(98 / 100.0, false, false, true);
        ddsim->ApproximateByFidelity(99 / 100.0, false, false, true);
        ddsim->ApproximateByFidelity(100 / 100.0, false, false, true);

        // TargetFidelityPerLevel
        ddsim->ApproximateByFidelity(1 / 1000.0, true, false, true);
        ddsim->ApproximateByFidelity(5 / 1000.0, true, false, true);
        ddsim->ApproximateByFidelity(10 / 1000.0, true, false, true);
        ddsim->ApproximateByFidelity(20 / 1000.0, true, false, true);
        ddsim->ApproximateByFidelity(30 / 1000.0, true, false, true);
        ddsim->ApproximateByFidelity(40 / 1000.0, true, false, true);
        for (int i = 50; i <= 950; i += 10) {
            ddsim->ApproximateByFidelity(i / 1000.0, true, false, true);
        }
        ddsim->ApproximateByFidelity(960 / 1000.0, true, false, true);
        ddsim->ApproximateByFidelity(970 / 1000.0, true, false, true);
        ddsim->ApproximateByFidelity(980 / 1000.0, true, false, true);
        ddsim->ApproximateByFidelity(985 / 1000.0, true, false, true);
        ddsim->ApproximateByFidelity(990 / 1000.0, true, false, true);
        ddsim->ApproximateByFidelity(995 / 1000.0, true, false, true);
        ddsim->ApproximateByFidelity(1000 / 1000.0, true, false, true);

        // Traversal
        for (int i = 1; i < 10; i += 1) {
            ddsim->ApproximateBySampling(i, 0, false, true);
        }
        for (int i = 10; i < 100; i += 10) {
            ddsim->ApproximateBySampling(i, 0, false, true);
        }
        for (int i = 100; i < 1000; i += 100) {
            ddsim->ApproximateBySampling(i, 0, false, true);
        }
        for (int i = 1000; i < 100000; i += 1000) {
            ddsim->ApproximateBySampling(i, 0, false, true);
        }
        for (int i = 100000; i <= 1000000; i += 10000) {
            ddsim->ApproximateBySampling(i, 0, false, true);
        }

        // Traversal+Threshold
        for (int i = 1; i < 10; i += 1) {
            ddsim->ApproximateBySampling(1000000, i, false, true);
        }
        for (int i = 10; i < 100; i += 10) {
            ddsim->ApproximateBySampling(1000000, i, false, true);
        }

        for (int i = 100; i <= 5000; i += 100) {
            ddsim->ApproximateBySampling(1000000, i, false, true);
        }
    }

    nl::json output_obj;

    if (vm.count("pm")) {
        output_obj["measurement_results"] = m;
    }

    if (vm.count("pv")) {
        output_obj["state_vector"] = ddsim->getVectorPair();
    }

    if (vm.count("ps")) {
        output_obj["statistics"] = {
                {"simulation_time", duration_simulation.count()},
                {"benchmark", ddsim->getName()},
                {"n_qubits", +ddsim->getNumberOfQubits()},
                {"applied_gates", ddsim->getNumberOfOps()},
                {"max_nodes", ddsim->getMaxNodeCount()},
                {"shots", shots},
                {"distinct_results", m.size()},
                {"seed", ddsim->getSeed()},
        };

        for (const auto& [stat, value]: ddsim->AdditionalStatistics()) {
            output_obj["statistics"][stat] = value;
        }
    }

    if (vm.count("pcomplex")) {
        output_obj["complex_stats"] = ddsim->dd->cn.complexTable.getStatistics();
    }

    if (vm.count("dump_complex")) {
        auto filename = vm["dump_complex"].as<std::string>();
        auto ostream  = std::fstream(filename, std::fstream::out);
        dd::exportEdgeWeights(ddsim->root_edge, ostream);
    }

    std::cout << std::setw(2) << output_obj << std::endl;
}


//------------------------------------------------------------------------------
/// \file
/// \brief
/// \copyright (C) Copyright Aquaveo 2018. Distributed under FreeBSD License
/// (See accompanying file LICENSE or https://aqaveo.com/bsd/license.txt)
//------------------------------------------------------------------------------

//----- Included files ---------------------------------------------------------
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <boost/shared_ptr.hpp>
#include <xmscore/misc/DynBitset.h>
#include <xmscore/misc/XmError.h>
#include <xmscore/python/misc/PyUtils.h>
#include <xmsinterp/triangulate/TrTin.h>
#include <xmsmesh/meshing/MeMeshUtils.h>
#include <xmsmesh/meshing/MeMultiPolyMesher.h>
#include <xmsmesh/meshing/MeMultiPolyMesherIo.h>
#include <xmsmesh/meshing/MeMultiPolyTo2dm.h>
#include <xmsmesh/meshing/MePolyRedistributePts.h>

//----- Namespace declaration --------------------------------------------------
namespace py = pybind11;

//----- Python Interface -------------------------------------------------------
PYBIND11_DECLARE_HOLDER_TYPE(T, boost::shared_ptr<T>);

void initMeMeshUtils(py::module &m) {

    py::module modMeshUtils = m.def_submodule("mesh_utils");

  // ---------------------------------------------------------------------------
  // function: size_function_from_depth
  // ---------------------------------------------------------------------------
  const char* size_function_from_depth_doc = R"pydoc(
      Creates a size at each point based on the depth at the point and the min
      and max sizes the equation is  min_depth + ( (depth - min_depth) /
      (max_depth - min_depth) ) * (max_size - min_size). This is often useful for
      coastal numerical model simulations.

      Args:
          depths (iterable): The measured depths at point locations
          min_size (float): The minimum element edge size
          max_size (float): The maximum element edge size

      Returns:
        iterable: Array of sizes based on depth
  )pydoc";
    modMeshUtils.def("size_function_from_depth", [](py::iterable depths, double min_size,
                                         double max_size) -> py::iterable {

        xms::VecDbl vec_depths, vec_size;
        vec_depths = *xms::VecDblFromPyIter(depths);
        xms::meSizeFunctionFromDepth(vec_depths, vec_size, min_size, max_size);
        return xms::PyIterFromVecDbl(vec_size);
    },size_function_from_depth_doc,py::arg("depths"),py::arg("min_size"),
    py::arg("max_size"));
  // ---------------------------------------------------------------------------
  // function: smooth_size_function
  // ---------------------------------------------------------------------------
  const char* smooth_size_function_doc = R"pydoc(
      Smooths a size function. Ensures that the size function transitions over a
      sufficient distance so that the area change of adjacent elements meets the
      size ratio passed in.

      Args:
          tin (:class:`Tin <xmsinterp.triangulate.Tin>`): Points and triangles defining the connectivity of the size function.
          sizes (iterable): Array of the current sizes
          size_ratio (float): Allowable size difference between adjacent elements
          min_size (float): Minimum specified element size
          anchor_type (int): The minimum element edge size
          pts_flag (iterable): Flag to indicate if the value at the point should be adjusted (a value of true will skip the point). Leave the bitset empty to process all points.

      Returns:
        iterable: Array of smoothed sizes
  )pydoc";
    modMeshUtils.def("smooth_size_function", [](boost::shared_ptr<xms::TrTin> tin, py::iterable sizes,
                                    double size_ratio, double min_size, int anchor_type,
                                    py::iterable pts_flag) -> py::iterable {
        xms::VecFlt vec_sizes, vec_smooth_sizes;
        for (auto item : sizes) {
          vec_sizes.push_back(item.cast<float>());
        }
        xms::DynBitset bitset;
        std::vector<unsigned char> bitvals;
        for (auto item : pts_flag) {
          py::bool_ flag = item.cast<py::bool_>();
          if (flag) {
            bitvals.push_back(1);
          } else {
            bitvals.push_back(0);
          }
        }
        xms::VecBooleanToDynBitset(bitvals, bitset);

        xms::meSmoothSizeFunction(tin, vec_sizes, size_ratio, min_size, anchor_type, bitset, vec_smooth_sizes);

        if (py::isinstance<py::array>(sizes)) {
          // NOTE: This is a copy operation
          return py::array(vec_smooth_sizes.size(), vec_smooth_sizes.data());
        } else {
          // NOTE: This is a copy operation
          auto tuple_ret = py::tuple(vec_smooth_sizes.size());
          for (size_t i = 0; i < vec_smooth_sizes.size(); ++i) {
            tuple_ret[i] = vec_smooth_sizes.at(i);
          }
          return tuple_ret;
        }
    },smooth_size_function_doc, py::arg("tin"),py::arg("sizes"),
      py::arg("size_ratio"),py::arg("min_size"), py::arg("anchor_type"),
      py::arg("pts_flag")
    );
  // ---------------------------------------------------------------------------
  // function: smooth_elev_by_slope
  // ---------------------------------------------------------------------------
  const char* smooth_elev_by_slope_doc = R"pydoc(
      Smooths a elevations based on max specified slope (max_slope) preserving
      either the min or max based on anchor_type

      Args:
          tin (:class:`Tin <xmsinterp.triangulate.Tin>`): Points and triangles defining the connectivity of the elevations.
          elevations (iterable): Array of the current elevations
          max_slope (Float): Maximum allowable slope
          anchor_to_max (Bool): Indicates if you are anchoring to the max slope.
          pts_flag (iterable): Flag to indicate if the value at the point should be adjusted (a value of true will skip the point). Leave the bitset empty to process all points.

      Returns:
        iterable: Array of smoothed elevations
  )pydoc";
    modMeshUtils.def("smooth_elev_by_slope", [](boost::shared_ptr<xms::TrTin> tin, py::iterable elevations,
                                    double max_slope, bool anchor_to_max,
                                    py::iterable pts_flag) -> py::iterable {
        xms::VecFlt vec_elevations, vec_smooth_elevations;
        for (auto item : elevations) {
          vec_elevations.push_back(item.cast<float>());
        }
        xms::DynBitset bitset;
        std::vector<unsigned char> bitvals;
        for (auto item : pts_flag) {
          py::bool_ flag = item.cast<py::bool_>();
          if (flag) {
            bitvals.push_back(1);
          } else {
            bitvals.push_back(0);
          }
        }
        xms::VecBooleanToDynBitset(bitvals, bitset);

        xms::meSmoothElevBySlope(tin, vec_elevations, max_slope, anchor_to_max? 1 : 0, bitset, vec_smooth_elevations);

        if (py::isinstance<py::array>(elevations)) {
          // NOTE: This is a copy operation
          return py::array(vec_smooth_elevations.size(), vec_smooth_elevations.data());
        } else {
          // NOTE: This is a copy operation
          auto tuple_ret = py::tuple(vec_smooth_elevations.size());
          for (size_t i = 0; i < vec_smooth_elevations.size(); ++i) {
            tuple_ret[i] = vec_smooth_elevations.at(i);
          }
          return tuple_ret;
        }
    },smooth_elev_by_slope_doc, py::arg("tin"),py::arg("elevations"),
    py::arg("max_slope"),py::arg("anchor_to_max"),py::arg("pts_flag"));

  // ---------------------------------------------------------------------------
  // function: check_mesh_input_topology
  // ---------------------------------------------------------------------------
  const char* check_mesh_input_topology_doc = R"pydoc(
      Checks if the input polygons intersect one another

      Args:
          mesh_io (:class:`MultiPolyMesherIo <xmsmesh.meshing.MultiPolyMesherIo>`): Input polygons and options for generating a mesh.

      Returns:
        tuple: true if mesh inputs are topologically correct, and a string of messages.
  )pydoc";
    modMeshUtils.def("check_mesh_input_topology",
     [](xms::MeMultiPolyMesherIo &mesh_io) -> py::iterable
     {
       BSHP<xms::MeMultiPolyMesher> multiPolyMesher = xms::MeMultiPolyMesher::New();
       std::string errors;
       multiPolyMesher->CheckForIntersections(mesh_io, errors);
       bool rval(errors.empty());
       return py::make_tuple(rval, errors);
     },check_mesh_input_topology_doc, py::arg("mesh_io"));
  // ---------------------------------------------------------------------------
  // function: generate_mesh
  // ---------------------------------------------------------------------------
  const char* generate_mesh_doc = R"pydoc(
      Creates a mesh from the input polygons.

      Args:
          mesh_io (:class:`MultiPolyMesherIo <xmsmesh.meshing.MultiPolyMesherIo>`): Input polygons and options for generating a mesh.

      Returns:
        tuple: true if the mesh was generated successfully false otherwise, and a string of messages.
  )pydoc";
    modMeshUtils.def("generate_mesh",
     [](xms::MeMultiPolyMesherIo &mesh_io) -> py::iterable
     {
       BSHP<xms::MeMultiPolyMesher> multiPolyMesher = xms::MeMultiPolyMesher::New();
       bool rval = multiPolyMesher->MeshIt(mesh_io);
       std::string errors = xms::XmLog::Instance().GetAndClearStackStr();
       return py::make_tuple(rval, errors);
     },generate_mesh_doc, py::arg("mesh_io"));
  // ---------------------------------------------------------------------------
  // function: generate_2dm
  // ---------------------------------------------------------------------------
    const char* generate_2dm_doc = R"pydoc(
        Creates a mesh from the input polygons and writes it to a 2dm file.

        Args:
            mesh_io (:class:`MultiPolyMesherIo <xmsmesh.meshing.MultiPolyMesherIo>`): Input polygons and options for generating a mesh.
            file_name (str): The file name of the output 2dm file.
            precision (int, optional): The decimal point precision of the resulting mesh.

        Returns:
            tuple: true if the mesh was generated successfully false otherwise, and a string of messages.
    )pydoc";
    modMeshUtils.def("generate_2dm",
     [](xms::MeMultiPolyMesherIo &mesh_io,
        std::string file_name, int precision) -> py::tuple {
        BSHP<xms::MeMultiPolyTo2dm> mesher = xms::MeMultiPolyTo2dm::New();
        if (file_name.empty()) {
          throw py::value_error("file_name not specifed. Aborting mesh procedure.");
        }
        bool result = mesher->Generate2dm(mesh_io, file_name, precision);
        std::string errors = xms::XmLog::Instance().GetAndClearStackStr();
        return py::make_tuple(result, errors);
        },generate_2dm_doc,py::arg("mesh_io"),py::arg("file_name"),py::arg("precision")=15);

  // ---------------------------------------------------------------------------
  // function: redistribute_line
  // ---------------------------------------------------------------------------
    const char* redistribute_poly_line_doc = R"pydoc(
        Redistributes the points along a line to a constant spacing

        Args:
            poly_lin (iterable): Input poly line locations.
            size (Float): The desired spacing for point redistribution

        Returns:
            iterable: redistributed poly line locations
    )pydoc";
    modMeshUtils.def("redistribute_poly_line",
     [](py::iterable poly_line, double size) -> py::iterable {
        BSHP<xms::MePolyRedistributePts> redist(xms::MePolyRedistributePts::New());
        redist->SetConstantSizeFunc(size);
        BSHP<xms::VecPt3d> vPolyLine = xms::VecPt3dFromPyIter(poly_line);
        xms::VecPt3d rval = redist->Redistribute(*vPolyLine);
        return xms::PyIterFromVecPt3d(rval);
        },redistribute_poly_line_doc,py::arg("poly_line"),py::arg("size"));

}


/*
 * readmatrix.cpp
 *
 *  Created on: 5-sep-2017
 *      Author: M. El-Kebir
 */

#include "readmatrix.h"
#include <boost/math/distributions/beta.hpp>

ReadMatrix::ReadMatrix()
  : BaseMatrix()
  , _var()
  , _ref()
{
}

ReadMatrix ReadMatrix::poolReads(const IntMatrix& clustering,
                                 bool relabel) const
{
  ReadMatrix newR(*this);
  newR._k = _k;
  newR._m = _m;
  newR._n = 0;
  newR._indexToCharacter.clear();
  newR._characterToIndex.clear();

  char buf[1024];

  // 1. infer character labeling
  IntVector oldCharacterToNewCharacter(_n, -1);
  int idx = 0;
  for (const IntVector& cluster : clustering)
  {
    std::string label;
    for (int i : cluster)
    {
      if (!label.empty())
        label += "_";

      label += indexToCharacter(i);
      oldCharacterToNewCharacter[i] = idx;
    }

    if (relabel)
    {
      snprintf(buf, 1024, "cluster_%d", idx + 1);
      label = buf;
    }

    newR._indexToCharacter.push_back(label);
    newR._characterToIndex[label] = idx;

    ++idx;
    ++newR._n;
  }

  // 2. pool reads
  newR._ref = IntMatrix(_k, IntVector(newR._n, 0));
  newR._var = IntMatrix(_k, IntVector(newR._n, 0));

  for (const IntVector& cluster : clustering)
  {
    for (int i : cluster)
    {
      for (int p = 0; p < _k; ++p)
      {
        int new_i = oldCharacterToNewCharacter[i];
        assert(new_i != -1);
        newR._ref[p][new_i] += _ref[p][i];
        newR._var[p][new_i] += _var[p][i];
      }
    }
  }

  return newR;
}

FrequencyMatrix ReadMatrix::toFrequencyMatrix(double alpha,
                                              int threshold) const
{
  FrequencyMatrix resF(*this);

  // compute confidence intervals
  for (int p = 0; p < _k; ++p)
  {
    for (int i = 0; i < _n; ++i)
    {
      int var = getVar(p, i);
      int ref = getRef(p, i);

      boost::math::beta_distribution<> beta_dist(1 + var, 1 + ref);

      double f_lb = boost::math::quantile(beta_dist, alpha / 2);
      double f_ub = boost::math::quantile(beta_dist, 1 - alpha / 2);

      if (var < threshold || var <= (var + ref) * 0.01)
      {
        f_lb = f_ub = 0;
      }

      f_lb *= 2;
      f_ub *= 2;

      if (f_lb > 1)
      {
        f_lb = 1;
      }
      if (f_ub > 1)
      {
        f_ub = 1;
      }

      resF.set(p, i, f_lb, f_ub);
    }
  }

  return resF;
}

ReadMatrix ReadMatrix::downSample(int nrSamplesPerAnatomicalSite,
                                  int coverage,
                                  double purity,
                                  double seqErrorRate,
                                  double fracSNVs) const
{
  std::poisson_distribution<> poisson(coverage >= 0 ? coverage : 0);

  const int new_n = _n * fracSNVs;
  IntVector snvIndices(_n, 0);
  for (int i = 1; i < _n; ++i)
  {
    snvIndices[i] = snvIndices[i-1] + 1;
  }
  std::shuffle(snvIndices.begin(), snvIndices.end(), g_rng);
  snvIndices.erase(snvIndices.begin() + new_n, snvIndices.end());

  ReadMatrix newR;
  newR._m = _m;
  newR._n = new_n;
  if (nrSamplesPerAnatomicalSite < 0)
  {
    newR._k = _k;
    newR._var = IntMatrix(_k,
                          IntVector(new_n, 0));
    newR._ref = IntMatrix(_k,
                          IntVector(new_n, 0));
  }
  else
  {
    newR._k = nrSamplesPerAnatomicalSite * _m;
    newR._var = IntMatrix(nrSamplesPerAnatomicalSite * _m,
                          IntVector(new_n, 0));
    newR._ref = IntMatrix(nrSamplesPerAnatomicalSite * _m,
                          IntVector(new_n, 0));
  }

  newR._indexToAnatomicalSite = _indexToAnatomicalSite;
  newR._anatomicalSiteToIndex = _anatomicalSiteToIndex;

  newR._anatomicalSiteIndexToSampleIndices = IntSetVector(_m);

  newR._indexToCharacter = StringVector(new_n);
  for (int i = 0; i < new_n; ++i)
  {
    newR._indexToCharacter[i] = _indexToCharacter[snvIndices[i]];
    newR._characterToIndex[newR._indexToCharacter[i]] = i;
  }

  int newP = 0;
  for (int s = 0; s < _m; ++s)
  {
    IntVector sampleIndices(anatomicalSiteIndexToSampleIndices(s).begin(),
                            anatomicalSiteIndexToSampleIndices(s).end());

    std::shuffle(sampleIndices.begin(), sampleIndices.end(), g_rng);

    int nrSamplesPerAnatomicalSite_s = nrSamplesPerAnatomicalSite < 0 ? anatomicalSiteIndexToSampleIndices(s).size() : nrSamplesPerAnatomicalSite;
    assert(nrSamplesPerAnatomicalSite_s <= sampleIndices.size());
    for (int pp = 0; pp < nrSamplesPerAnatomicalSite_s; ++pp)
    {
      int p = sampleIndices[pp];
      const std::string& pStr = _indexToSample[p];

      newR._sampleToIndex[pStr] = newP;
      newR._indexToSample.push_back(pStr);

      newR._anatomicalSiteIndexToSampleIndices[s].insert(newP);
      newR._sampleIndexToAnatomicalSiteIndex.push_back(s);

      for (int i = 0; i < new_n; ++i)
      {
        int old_i = snvIndices[i];
        if (coverage < 0)
        {
          newR._var[newP][i] = _var[p][old_i];
          newR._ref[newP][i] = _ref[p][old_i];
        }
        else
        {
          double vaf_pi = purity * double(_var[p][old_i]) / double(_var[p][old_i] + _ref[p][old_i]);
          int newCoverage = poisson(g_rng);

          std::binomial_distribution<> binom(newCoverage, vaf_pi);
          int org_var = binom(g_rng);
          int org_ref = newCoverage - org_var;

          if (g_tol.nonZero(seqErrorRate))
          {
            std::binomial_distribution<> binom_noise_var(org_var,
                                                         seqErrorRate);
            std::binomial_distribution<> binom_noise_ref(org_ref,
                                                         seqErrorRate);

            int flips_var = binom_noise_var(g_rng);
            int flips_ref = binom_noise_ref(g_rng);

            newR._var[newP][i] = org_var - flips_var + flips_ref;
            newR._ref[newP][i] = newCoverage - newR._var[newP][i];
          }
          else
          {
            newR._var[newP][i] = org_var;
            newR._ref[newP][i] = newCoverage - newR._var[newP][i];
          }
        }
      }
      ++newP;
    }
  }

  return newR;
}

std::ostream& operator<<(std::ostream& out,
                         const ReadMatrix& R)
{
  out << R._m << " #anatomical sites" << std::endl;
  out << R._k << " #samples" << std::endl;
  out << R._n << " #characters" << std::endl;
  out << "#sample_index\tsample_label\tanatomical_site_index\t"\
         "anatomical_site_label\tcharacter_index\tcharacter_label\tref\tvar"
      << std::endl;
  for (int p = 0; p < R._k; ++p)
  {
    int s = R.sampleIndexToAnatomicalSiteIndex(p);
    const std::string sStr = R.indexToAnatomicalSite(s);

    const std::string& pStr = R.indexToSample(p);
    for (int c = 0; c < R._n; ++c)
    {
      const std::string& cStr = R.indexToCharacter(c);
      out << p << "\t" << pStr << "\t"
          << s << "\t" << sStr << "\t"
          << c << "\t" << cStr << "\t"
          << R._ref[p][c] << "\t" << R._var[p][c] << std::endl;
    }
  }

  return out;
}

std::istream& operator>>(std::istream& in, ReadMatrix& R)
{
  std::string line;
  getline(in, line);

  int m = -1;
  int k = -1;
  int n = -1;

  std::stringstream ss(line);
  ss >> m;

  if (m <= 0)
  {
    throw std::runtime_error("Error: m should be nonnegative");
  }

  getline(in, line);
  ss.clear();
  ss.str(line);
  ss >> k;

  if (k < m)
  {
    throw std::runtime_error("Error: k should be at least m");
  }

  getline(in, line);
  ss.clear();
  ss.str(line);
  ss >> n;

  if (n <= 0)
  {
    throw std::runtime_error("Error: n should be nonnegative");
  }

  R._m = m;
  R._k = k;
  R._n = n;

  R._indexToAnatomicalSite = StringVector(m);
  R._indexToSample = StringVector(k);
  R._indexToCharacter = StringVector(n);
  R._sampleIndexToAnatomicalSiteIndex = IntVector(k, -1);
  R._anatomicalSiteIndexToSampleIndices = IntSetVector(m);

  R._var = IntMatrix(k, IntVector(n, 0));
  R._ref = IntMatrix(k, IntVector(n, 0));

  std::vector<std::vector<bool> > present(k, std::vector<bool>(n, false));
  while (in.good())
  {
    getline(in, line);
    if (line == "" || line[0] == '#')
      continue;

    ss.clear();
    ss.str(line);

    int p = -1;
    std::string pStr;
    int s = -1;
    std::string sStr;
    int c = -1;
    std::string cStr;
    int ref = -1;
    int var = -1;

    ss >> p >> pStr >> s >> sStr >> c >> cStr >> ref >> var;

    if (!(0 <= p && p < k) || !(0 <= s && s < m) || !(0 <= c && c < n)
        || ref < 0 || var < 0)
    {
      throw std::runtime_error("Invalid character ("
                               + boost::lexical_cast<std::string>(p)
                               + ","
                               + boost::lexical_cast<std::string>(c)
                               + ")");
    }

    if (present[p][c])
    {
      throw std::runtime_error("Duplicate character ("
                               + boost::lexical_cast<std::string>(p)
                               + ","
                               + boost::lexical_cast<std::string>(c)
                               + ")");
    }

    R._indexToAnatomicalSite[s] = sStr;
    R._anatomicalSiteToIndex[sStr] = s;
    R._indexToSample[p] = pStr;
    R._sampleToIndex[pStr] = p;
    R._indexToCharacter[c] = cStr;
    R._characterToIndex[cStr] = c;
    R._var[p][c] = var;
    R._ref[p][c] = ref;
    R._sampleIndexToAnatomicalSiteIndex[p] = s;
    R._anatomicalSiteIndexToSampleIndices[s].insert(p);

    present[p][c] = true;
  }

  return in;
}


// Copyright (C) 2012-2015 Internet Systems Consortium, Inc. ("ISC")
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <stdint.h>

#include <cassert>
#include <cstring>
#include <cstdlib>
#include <string>
#include <vector>

#include <boost/noncopyable.hpp>
#include <boost/scoped_ptr.hpp>

#include <exceptions/exceptions.h>

#include <util/buffer.h>
#include <util/encode/base32hex.h>

#include <cryptolink/cryptolink.h>
#include <cryptolink/crypto_hash.h>

#include <dns/name.h>
#include <dns/labelsequence.h>
#include <dns/nsec3hash.h>
#include <dns/rdataclass.h>
#include <dns/name_internal.h>

using namespace std;
using namespace isc::util;
using namespace isc::util::encode;
using namespace isc::cryptolink;
using namespace isc::dns;
using namespace isc::dns::rdata;

namespace {

/// \brief A derived class of \c NSEC3Hash that implements the standard hash
/// calculation specified in RFC5155.
///
/// Currently the only pre-defined algorithm in the RFC is SHA1.  So we don't
/// over-generalize it at the moment, and rather hardcode it and assume that
/// specific algorithm.
///
/// The implementation details are only open within this file, but to avoid
/// an accidental error in this implementation we explicitly make it non
/// copyable.
class NSEC3HashRFC5155 : boost::noncopyable, public NSEC3Hash {
private:
    // This is the algorithm number for SHA1/NSEC3 as defined in RFC5155.
    static const uint8_t NSEC3_HASH_SHA1 = 1;
    // For digest_ allocation
    static const size_t DEFAULT_DIGEST_LENGTH = 32;

public:
    NSEC3HashRFC5155(uint8_t algorithm, uint16_t iterations,
                     const uint8_t* salt_data, size_t salt_length) :
        algorithm_(algorithm), iterations_(iterations),
        salt_data_(NULL), salt_length_(salt_length),
        digest_(DEFAULT_DIGEST_LENGTH), obuf_(Name::MAX_WIRE)
    {
        if (algorithm_ != NSEC3_HASH_SHA1) {
            isc_throw(UnknownNSEC3HashAlgorithm, "Unknown NSEC3 algorithm: " <<
                      static_cast<unsigned int>(algorithm_));
        }

        if (salt_length > 0) {
            salt_data_ = static_cast<uint8_t*>(std::malloc(salt_length));
            if (salt_data_ == NULL) {
                throw std::bad_alloc();
            }
            std::memcpy(salt_data_, salt_data, salt_length);
        }
    }

    virtual ~NSEC3HashRFC5155() {
        std::free(salt_data_);
    }

    virtual std::string calculate(const Name& name) const;
    virtual std::string calculate(const LabelSequence& ls) const;

    virtual bool match(const generic::NSEC3& nsec3) const;
    virtual bool match(const generic::NSEC3PARAM& nsec3param) const;
    bool match(uint8_t algorithm, uint16_t iterations,
               const vector<uint8_t>& salt) const;

private:
    std::string calculateForWiredata(const uint8_t* data, size_t length) const;

    const uint8_t algorithm_;
    const uint16_t iterations_;
    uint8_t* salt_data_;
    const size_t salt_length_;

    // The following members are placeholder of work place and don't hold
    // any state over multiple calls so can be mutable without breaking
    // constness.
    mutable OutputBuffer digest_;
    mutable vector<uint8_t> vdigest_;
    mutable OutputBuffer obuf_;
};

inline void
iterateSHA1(const uint8_t* input, size_t inlength,
            const uint8_t* salt, size_t saltlen,
            OutputBuffer& output)
{
    boost::scoped_ptr<Hash> hash(CryptoLink::getCryptoLink().createHash(SHA1));
    hash->update(input, inlength);
    hash->update(salt, saltlen); // this works whether saltlen == or > 0
    hash->final(output, hash->getOutputLength());
}

string
NSEC3HashRFC5155::calculateForWiredata(const uint8_t* data,
                                       size_t length) const
{
    // We first need to normalize the name by converting all upper case
    // characters in the labels to lower ones.

    uint8_t name_buf[256];
    assert(length < sizeof (name_buf));

    const uint8_t *p1 = data;
    uint8_t *p2 = name_buf;
    while (*p1 != 0) {
        char len = *p1;

        *p2++ = *p1++;
        while (len--) {
            *p2++ = isc::dns::name::internal::maptolower[*p1++];
        }
    }

    *p2 = *p1;

    digest_.clear();
    iterateSHA1(name_buf, length,
                salt_data_, salt_length_, digest_);
    const uint8_t* dgst_data = static_cast<const uint8_t*>(digest_.getData());
    size_t dgst_len = digest_.getLength();
    for (unsigned int n = 0; n < iterations_; ++n) {
        digest_.clear();
        iterateSHA1(dgst_data, dgst_len, salt_data_, salt_length_, digest_);
    }

    vdigest_.resize(dgst_len);
    std::memcpy(&vdigest_[0], dgst_data, dgst_len);
    return (encodeBase32Hex(vdigest_));
}

string
NSEC3HashRFC5155::calculate(const Name& name) const {
    obuf_.clear();
    name.toWire(obuf_);

    return (calculateForWiredata(static_cast<const uint8_t*>(obuf_.getData()),
                                 obuf_.getLength()));
}

string
NSEC3HashRFC5155::calculate(const LabelSequence& ls) const {
    assert(ls.isAbsolute());

    size_t length;
    const uint8_t* data = ls.getData(&length);

    return (calculateForWiredata(data, length));
}

bool
NSEC3HashRFC5155::match(uint8_t algorithm, uint16_t iterations,
                        const vector<uint8_t>& salt) const
{
    return (algorithm_ == algorithm && iterations_ == iterations &&
            salt_length_ == salt.size() &&
            ((salt_length_ == 0) ||
             memcmp(salt_data_, &salt[0], salt_length_) == 0));
}

bool
NSEC3HashRFC5155::match(const generic::NSEC3& nsec3) const {
    return (match(nsec3.getHashalg(), nsec3.getIterations(),
                  nsec3.getSalt()));
}

bool
NSEC3HashRFC5155::match(const generic::NSEC3PARAM& nsec3param) const {
    return (match(nsec3param.getHashalg(), nsec3param.getIterations(),
                  nsec3param.getSalt()));
}

// A static pointer that refers to the currently usable creator.
// Only get/setNSEC3HashCreator are expected to get access to this variable
// directly.
const NSEC3HashCreator* creator;

// The accessor to the current creator.  If it's not explicitly set or has
// been reset from a customized one, the default creator will be used.
const NSEC3HashCreator*
getNSEC3HashCreator() {
    static DefaultNSEC3HashCreator default_creator;
    if (creator == NULL) {
        creator = &default_creator;
    }
    return (creator);
}

} // end of unnamed namespace

namespace isc {
namespace dns {

NSEC3Hash*
NSEC3Hash::create(const generic::NSEC3PARAM& param) {
    return (getNSEC3HashCreator()->create(param));
}

NSEC3Hash*
NSEC3Hash::create(const generic::NSEC3& nsec3) {
    return (getNSEC3HashCreator()->create(nsec3));
}

NSEC3Hash*
NSEC3Hash::create(uint8_t algorithm, uint16_t iterations,
                  const uint8_t* salt_data, size_t salt_length) {
    return (getNSEC3HashCreator()->create(algorithm, iterations,
                                          salt_data, salt_length));
}

NSEC3Hash*
DefaultNSEC3HashCreator::create(const generic::NSEC3PARAM& param) const {
    const vector<uint8_t>& salt = param.getSalt();
    return (new NSEC3HashRFC5155(param.getHashalg(), param.getIterations(),
                                 salt.empty() ? NULL : &salt[0],
                                 salt.size()));
}

NSEC3Hash*
DefaultNSEC3HashCreator::create(const generic::NSEC3& nsec3) const {
    const vector<uint8_t>& salt = nsec3.getSalt();
    return (new NSEC3HashRFC5155(nsec3.getHashalg(), nsec3.getIterations(),
                                 salt.empty() ? NULL : &salt[0],
                                 salt.size()));
}

NSEC3Hash*
DefaultNSEC3HashCreator::create(uint8_t algorithm, uint16_t iterations,
                                const uint8_t* salt_data,
                                size_t salt_length) const
{
    return (new NSEC3HashRFC5155(algorithm, iterations,
                                 salt_data, salt_length));
}

void
setNSEC3HashCreator(const NSEC3HashCreator* new_creator) {
    creator = new_creator;
}

} // namespace dns
} // namespace isc


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "../include/structures.h"
#include "../include/transformations.h"
#include "../../relative_pose_tait_bryan_wc_jacobian.h"
#include "../include/cauchy.h"
#include "../../constraint_fixed_parameter_jacobian.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

std::vector<Eigen::Affine3d> m_poses;
std::vector<Eigen::Affine3d> m_poses_desired;

std::vector<std::pair<int, int>> odo_edges;
std::vector<std::pair<int, int>> loop_edges;

std::vector<std::pair<Eigen::Affine3d, int>> georeference_data;

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

	for(size_t i = 0 ; i < 100; i++){
		TaitBryanPose p;
		p.px = i;
		p.py = -1;
		p.pz = 0.0;
		p.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.01;
		p.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.01;
		p.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.01;

		Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(p);
		m_poses.push_back(m);
	}
	for(size_t i = 0 ; i < 100; i++){
		TaitBryanPose p;
		p.px = i;
		p.py = 1;
		p.pz = 0.0;
		p.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.01;
		p.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.01;
		p.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.01;

		Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(p);
		m_poses.push_back(m);
	}
	m_poses_desired = m_poses;

	for(size_t i = 1; i < 100; i++){
		odo_edges.emplace_back(i-1,i);
	}

	for(size_t i = 101; i < 200; i++){
		odo_edges.emplace_back(i-1,i);
	}

	for(size_t i = 0; i < 100; i+=10){
		loop_edges.emplace_back(i,i+100);
	}

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	TaitBryanPose p;
	p.px = 0;
	p.py = 5;
	p.pz = 5;
	p.om = 0;
	p.fi = 0;
	p.ka = 0;
	Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(p);
	georeference_data.emplace_back(m, 4);

	p.px = 50;
	p.py = 3;
	p.pz = 4;
	m = affine_matrix_from_pose_tait_bryan(p);
	georeference_data.emplace_back(m, 54);

	p.px = 0;
	p.py = 8;
	p.pz = 5;
	m = affine_matrix_from_pose_tait_bryan(p);
	georeference_data.emplace_back(m, 102);

	p.px = 110;
	p.py = 8;
	p.pz = 7;
	m = affine_matrix_from_pose_tait_bryan(p);
	georeference_data.emplace_back(m, 197);

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("georeference-case2");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(1.0f, 0.0f, 0.0f);

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 1.0f, 0.0f);

	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 1.0f);
	glEnd();

	glColor3f(1,0,0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < odo_edges.size(); i++){
		glVertex3f(m_poses[odo_edges[i].first](0,3), m_poses[odo_edges[i].first](1,3), m_poses[odo_edges[i].first](2,3) );
		glVertex3f(m_poses[odo_edges[i].second](0,3), m_poses[odo_edges[i].second](1,3), m_poses[odo_edges[i].second](2,3) );
	}
	glEnd();

	glColor3f(0,1,0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < loop_edges.size(); i++){
		glVertex3f(m_poses[loop_edges[i].first](0,3), m_poses[loop_edges[i].first](1,3), m_poses[loop_edges[i].first](2,3) );
		glVertex3f(m_poses[loop_edges[i].second](0,3), m_poses[loop_edges[i].second](1,3), m_poses[loop_edges[i].second](2,3) );
	}
	glEnd();

	for(size_t i = 0 ; i < georeference_data.size(); i++){
		glBegin(GL_LINES);
		glColor3f(1.0f, 0.0f, 0.0f);
		glVertex3f(georeference_data[i].first(0,3), georeference_data[i].first(1,3), georeference_data[i].first(2,3));
		glVertex3f(georeference_data[i].first(0,3) + georeference_data[i].first(0,0), georeference_data[i].first(1,3) + georeference_data[i].first(1,0), georeference_data[i].first(2,3) + georeference_data[i].first(2,0));

		glColor3f(0.0f, 1.0f, 0.0f);
		glVertex3f(georeference_data[i].first(0,3), georeference_data[i].first(1,3), georeference_data[i].first(2,3));
		glVertex3f(georeference_data[i].first(0,3) + georeference_data[i].first(0,1), georeference_data[i].first(1,3) + georeference_data[i].first(1,1), georeference_data[i].first(2,3) + georeference_data[i].first(2,1));

		glColor3f(0.0f, 0.0f, 1.0f);
		glVertex3f(georeference_data[i].first(0,3), georeference_data[i].first(1,3), georeference_data[i].first(2,3));
		glVertex3f(georeference_data[i].first(0,3) + georeference_data[i].first(0,2), georeference_data[i].first(1,3) + georeference_data[i].first(1,2), georeference_data[i].first(2,3) + georeference_data[i].first(2,2));
		glEnd();
	}

	glColor3f(0.0f, 0.0f, 0.0f);
	glBegin(GL_LINES);
	for(size_t i = 0 ; i < georeference_data.size(); i++){
		glVertex3f(georeference_data[i].first(0,3), georeference_data[i].first(1,3), georeference_data[i].first(2,3));
		glVertex3f(m_poses[georeference_data[i].second](0,3), m_poses[georeference_data[i].second](1,3), m_poses[georeference_data[i].second](2,3) );
	}
	glEnd();

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'n':{
			for(size_t i = 0 ; i < m_poses.size(); i++){
				TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_poses[i]);
				pose.px += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.py += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.pz += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.om += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.01;
				pose.fi += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.01;
				pose.ka += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.01;
				m_poses[i] = affine_matrix_from_pose_tait_bryan(pose);
			}
			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			std::vector<TaitBryanPose> poses;
			std::vector<TaitBryanPose> poses_desired;

			for(size_t i = 0 ; i < m_poses.size(); i++){
				poses.push_back(pose_tait_bryan_from_affine_matrix(m_poses[i]));
			}
			for(size_t i = 0 ; i < m_poses_desired.size(); i++){
				poses_desired.push_back(pose_tait_bryan_from_affine_matrix(m_poses_desired[i]));
			}

			for(size_t i = 0 ; i < odo_edges.size(); i++){
				Eigen::Matrix<double, 6, 1> relative_pose_measurement_odo;
				relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo,
						poses_desired[odo_edges[i].first].px,
						poses_desired[odo_edges[i].first].py,
						poses_desired[odo_edges[i].first].pz,
						poses_desired[odo_edges[i].first].om,
						poses_desired[odo_edges[i].first].fi,
						poses_desired[odo_edges[i].first].ka,
						poses_desired[odo_edges[i].second].px,
						poses_desired[odo_edges[i].second].py,
						poses_desired[odo_edges[i].second].pz,
						poses_desired[odo_edges[i].second].om,
						poses_desired[odo_edges[i].second].fi,
						poses_desired[odo_edges[i].second].ka);

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_obs_eq_tait_bryan_wc_case1(
						delta,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka,
						relative_pose_measurement_odo(0,0),
						relative_pose_measurement_odo(1,0),
						relative_pose_measurement_odo(2,0),
						relative_pose_measurement_odo(3,0),
						relative_pose_measurement_odo(4,0),
						relative_pose_measurement_odo(5,0));

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka);

				int ir = tripletListB.size();

				int ic_1 = odo_edges[i].first * 6;
				int ic_2 = odo_edges[i].second * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));

					tripletListA.emplace_back(ir + row, ic_2    , -jacobian(row,6));
					tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row,7));
					tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row,8));
					tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row,9));
					tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row,10));
					tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row,11));
				}

				tripletListB.emplace_back(ir,     0, delta(0,0));
				tripletListB.emplace_back(ir + 1, 0, delta(1,0));
				tripletListB.emplace_back(ir + 2, 0, delta(2,0));
				tripletListB.emplace_back(ir + 3, 0, delta(3,0));
				tripletListB.emplace_back(ir + 4, 0, delta(4,0));
				tripletListB.emplace_back(ir + 5, 0, delta(5,0));

				tripletListP.emplace_back(ir ,    ir,     1000);
				tripletListP.emplace_back(ir + 1, ir + 1, 1000);
				tripletListP.emplace_back(ir + 2, ir + 2, 1000);
				tripletListP.emplace_back(ir + 3, ir + 3, 1000);
				tripletListP.emplace_back(ir + 4, ir + 4, 1000);
				tripletListP.emplace_back(ir + 5, ir + 5, 1000);
			}

			for(size_t i = 0 ; i < loop_edges.size(); i++){
				Eigen::Matrix<double, 6, 1> relative_pose_measurement_loop;
				relative_pose_tait_bryan_wc_case1(relative_pose_measurement_loop,
						poses_desired[loop_edges[i].first].px,
						poses_desired[loop_edges[i].first].py,
						poses_desired[loop_edges[i].first].pz,
						poses_desired[loop_edges[i].first].om,
						poses_desired[loop_edges[i].first].fi,
						poses_desired[loop_edges[i].first].ka,
						poses_desired[loop_edges[i].second].px,
						poses_desired[loop_edges[i].second].py,
						poses_desired[loop_edges[i].second].pz,
						poses_desired[loop_edges[i].second].om,
						poses_desired[loop_edges[i].second].fi,
						poses_desired[loop_edges[i].second].ka);

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_obs_eq_tait_bryan_wc_case1(
						delta,
						poses[loop_edges[i].first].px,
						poses[loop_edges[i].first].py,
						poses[loop_edges[i].first].pz,
						poses[loop_edges[i].first].om,
						poses[loop_edges[i].first].fi,
						poses[loop_edges[i].first].ka,
						poses[loop_edges[i].second].px,
						poses[loop_edges[i].second].py,
						poses[loop_edges[i].second].pz,
						poses[loop_edges[i].second].om,
						poses[loop_edges[i].second].fi,
						poses[loop_edges[i].second].ka,
						relative_pose_measurement_loop(0,0),
						relative_pose_measurement_loop(1,0),
						relative_pose_measurement_loop(2,0),
						relative_pose_measurement_loop(3,0),
						relative_pose_measurement_loop(4,0),
						relative_pose_measurement_loop(5,0));

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
						poses[loop_edges[i].first].px,
						poses[loop_edges[i].first].py,
						poses[loop_edges[i].first].pz,
						poses[loop_edges[i].first].om,
						poses[loop_edges[i].first].fi,
						poses[loop_edges[i].first].ka,
						poses[loop_edges[i].second].px,
						poses[loop_edges[i].second].py,
						poses[loop_edges[i].second].pz,
						poses[loop_edges[i].second].om,
						poses[loop_edges[i].second].fi,
						poses[loop_edges[i].second].ka);

				int ir = tripletListB.size();

				int ic_1 = loop_edges[i].first * 6;
				int ic_2 = loop_edges[i].second * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));

					tripletListA.emplace_back(ir + row, ic_2    , -jacobian(row,6));
					tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row,7));
					tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row,8));
					tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row,9));
					tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row,10));
					tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row,11));
				}

				tripletListB.emplace_back(ir,     0, delta(0,0));
				tripletListB.emplace_back(ir + 1, 0, delta(1,0));
				tripletListB.emplace_back(ir + 2, 0, delta(2,0));
				tripletListB.emplace_back(ir + 3, 0, delta(3,0));
				tripletListB.emplace_back(ir + 4, 0, delta(4,0));
				tripletListB.emplace_back(ir + 5, 0, delta(5,0));

				tripletListP.emplace_back(ir ,    ir,     1);
				tripletListP.emplace_back(ir + 1, ir + 1, 1);
				tripletListP.emplace_back(ir + 2, ir + 2, 1);
				tripletListP.emplace_back(ir + 3, ir + 3, 1);
				tripletListP.emplace_back(ir + 4, ir + 4, 1);
				tripletListP.emplace_back(ir + 5, ir + 5, 1);
			}

			for(size_t i = 0; i < georeference_data.size(); i++){
				TaitBryanPose pose_gps = pose_tait_bryan_from_affine_matrix(georeference_data[i].first);
				TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_poses[georeference_data[i].second]);

				Eigen::Matrix<double, 6, 1> relative_pose_measurement;
				relative_pose_measurement(0,0) = 0.0;
				relative_pose_measurement(1,0) = 0.0;
				relative_pose_measurement(2,0) = 0.0;
				relative_pose_measurement(3,0) = 0.0;
				relative_pose_measurement(4,0) = 0.0;
				relative_pose_measurement(5,0) = 0.0;

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_obs_eq_tait_bryan_wc_case1(
						delta,
						pose_s.px,
						pose_s.py,
						pose_s.pz,
						pose_s.om,
						pose_s.fi,
						pose_s.ka,
						pose_gps.px,
						pose_gps.py,
						pose_gps.pz,
						pose_gps.om,
						pose_gps.fi,
						pose_gps.ka,
						relative_pose_measurement(0,0),
						relative_pose_measurement(1,0),
						relative_pose_measurement(2,0),
						relative_pose_measurement(3,0),
						relative_pose_measurement(4,0),
						relative_pose_measurement(5,0));

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
						pose_s.px,
						pose_s.py,
						pose_s.pz,
						pose_s.om,
						pose_s.fi,
						pose_s.ka,
						pose_gps.px,
						pose_gps.py,
						pose_gps.pz,
						pose_gps.om,
						pose_gps.fi,
						pose_gps.ka);

				int ir = tripletListB.size();
				int ic_1 = georeference_data[i].second * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));
				}

				tripletListB.emplace_back(ir,     0, delta(0,0));
				tripletListB.emplace_back(ir + 1, 0, delta(1,0));
				tripletListB.emplace_back(ir + 2, 0, delta(2,0));
				tripletListB.emplace_back(ir + 3, 0, delta(3,0));
				tripletListB.emplace_back(ir + 4, 0, delta(4,0));
				tripletListB.emplace_back(ir + 5, 0, delta(5,0));

				tripletListP.emplace_back(ir ,    ir,     cauchy(delta(0,0), 1));
				tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1,0), 1));
				tripletListP.emplace_back(ir + 2, ir + 2, cauchy(delta(2,0), 1));
				tripletListP.emplace_back(ir + 3, ir + 3, cauchy(delta(3,0), 1));
				tripletListP.emplace_back(ir + 4, ir + 4, cauchy(delta(4,0), 1));
				tripletListP.emplace_back(ir + 5, ir + 5, cauchy(delta(5,0), 1));
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), m_poses.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(m_poses.size() * 6 , m_poses.size() * 6);
			Eigen::SparseMatrix<double> AtPB(m_poses.size() * 6 , 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;

			std::cout << "AtPA=AtPB SOLVED" << std::endl;

			for(size_t i = 0 ; i < h_x.size(); i++){
				std::cout << h_x[i] << std::endl;
			}

			if(h_x.size() == 6 * m_poses.size()){
				int counter = 0;

				for(size_t i = 0; i < m_poses.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_poses[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];
					m_poses[i] = affine_matrix_from_pose_tait_bryan(pose);
				}
			}
			std::cout << "optimizing with tait bryan finished" << std::endl;
			break;
		}
		case 'y':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			std::vector<TaitBryanPose> poses;
			std::vector<TaitBryanPose> poses_desired;

			for(size_t i = 0 ; i < m_poses.size(); i++){
				poses.push_back(pose_tait_bryan_from_affine_matrix(m_poses[i]));
			}
			for(size_t i = 0 ; i < m_poses_desired.size(); i++){
				poses_desired.push_back(pose_tait_bryan_from_affine_matrix(m_poses_desired[i]));
			}

			for(size_t i = 0 ; i < odo_edges.size(); i++){
				Eigen::Matrix<double, 6, 1> relative_pose_measurement_odo;
				relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo,
						poses_desired[odo_edges[i].first].px,
						poses_desired[odo_edges[i].first].py,
						poses_desired[odo_edges[i].first].pz,
						poses_desired[odo_edges[i].first].om,
						poses_desired[odo_edges[i].first].fi,
						poses_desired[odo_edges[i].first].ka,
						poses_desired[odo_edges[i].second].px,
						poses_desired[odo_edges[i].second].py,
						poses_desired[odo_edges[i].second].pz,
						poses_desired[odo_edges[i].second].om,
						poses_desired[odo_edges[i].second].fi,
						poses_desired[odo_edges[i].second].ka);

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_obs_eq_tait_bryan_wc_case1(
						delta,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka,
						relative_pose_measurement_odo(0,0),
						relative_pose_measurement_odo(1,0),
						relative_pose_measurement_odo(2,0),
						relative_pose_measurement_odo(3,0),
						relative_pose_measurement_odo(4,0),
						relative_pose_measurement_odo(5,0));

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka);

				int ir = tripletListB.size();

				int ic_1 = odo_edges[i].first * 6;
				int ic_2 = odo_edges[i].second * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));

					tripletListA.emplace_back(ir + row, ic_2    , -jacobian(row,6));
					tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row,7));
					tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row,8));
					tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row,9));
					tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row,10));
					tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row,11));
				}

				tripletListB.emplace_back(ir,     0, delta(0,0));
				tripletListB.emplace_back(ir + 1, 0, delta(1,0));
				tripletListB.emplace_back(ir + 2, 0, delta(2,0));
				tripletListB.emplace_back(ir + 3, 0, delta(3,0));
				tripletListB.emplace_back(ir + 4, 0, delta(4,0));
				tripletListB.emplace_back(ir + 5, 0, delta(5,0));

				tripletListP.emplace_back(ir ,    ir,     1000);
				tripletListP.emplace_back(ir + 1, ir + 1, 1000);
				tripletListP.emplace_back(ir + 2, ir + 2, 1000);
				tripletListP.emplace_back(ir + 3, ir + 3, 1000);
				tripletListP.emplace_back(ir + 4, ir + 4, 1000);
				tripletListP.emplace_back(ir + 5, ir + 5, 1000);
			}

			for(size_t i = 0 ; i < loop_edges.size(); i++){
				Eigen::Matrix<double, 6, 1> relative_pose_measurement_loop;
				relative_pose_tait_bryan_wc_case1(relative_pose_measurement_loop,
						poses_desired[loop_edges[i].first].px,
						poses_desired[loop_edges[i].first].py,
						poses_desired[loop_edges[i].first].pz,
						poses_desired[loop_edges[i].first].om,
						poses_desired[loop_edges[i].first].fi,
						poses_desired[loop_edges[i].first].ka,
						poses_desired[loop_edges[i].second].px,
						poses_desired[loop_edges[i].second].py,
						poses_desired[loop_edges[i].second].pz,
						poses_desired[loop_edges[i].second].om,
						poses_desired[loop_edges[i].second].fi,
						poses_desired[loop_edges[i].second].ka);

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_obs_eq_tait_bryan_wc_case1(
						delta,
						poses[loop_edges[i].first].px,
						poses[loop_edges[i].first].py,
						poses[loop_edges[i].first].pz,
						poses[loop_edges[i].first].om,
						poses[loop_edges[i].first].fi,
						poses[loop_edges[i].first].ka,
						poses[loop_edges[i].second].px,
						poses[loop_edges[i].second].py,
						poses[loop_edges[i].second].pz,
						poses[loop_edges[i].second].om,
						poses[loop_edges[i].second].fi,
						poses[loop_edges[i].second].ka,
						relative_pose_measurement_loop(0,0),
						relative_pose_measurement_loop(1,0),
						relative_pose_measurement_loop(2,0),
						relative_pose_measurement_loop(3,0),
						relative_pose_measurement_loop(4,0),
						relative_pose_measurement_loop(5,0));

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
						poses[loop_edges[i].first].px,
						poses[loop_edges[i].first].py,
						poses[loop_edges[i].first].pz,
						poses[loop_edges[i].first].om,
						poses[loop_edges[i].first].fi,
						poses[loop_edges[i].first].ka,
						poses[loop_edges[i].second].px,
						poses[loop_edges[i].second].py,
						poses[loop_edges[i].second].pz,
						poses[loop_edges[i].second].om,
						poses[loop_edges[i].second].fi,
						poses[loop_edges[i].second].ka);

				int ir = tripletListB.size();

				int ic_1 = loop_edges[i].first * 6;
				int ic_2 = loop_edges[i].second * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));

					tripletListA.emplace_back(ir + row, ic_2    , -jacobian(row,6));
					tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row,7));
					tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row,8));
					tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row,9));
					tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row,10));
					tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row,11));
				}

				tripletListB.emplace_back(ir,     0, delta(0,0));
				tripletListB.emplace_back(ir + 1, 0, delta(1,0));
				tripletListB.emplace_back(ir + 2, 0, delta(2,0));
				tripletListB.emplace_back(ir + 3, 0, delta(3,0));
				tripletListB.emplace_back(ir + 4, 0, delta(4,0));
				tripletListB.emplace_back(ir + 5, 0, delta(5,0));

				tripletListP.emplace_back(ir ,    ir,     1);
				tripletListP.emplace_back(ir + 1, ir + 1, 1);
				tripletListP.emplace_back(ir + 2, ir + 2, 1);
				tripletListP.emplace_back(ir + 3, ir + 3, 1);
				tripletListP.emplace_back(ir + 4, ir + 4, 1);
				tripletListP.emplace_back(ir + 5, ir + 5, 1);
			}

			for(size_t i = 0; i < georeference_data.size(); i++){
				TaitBryanPose pose_gps = pose_tait_bryan_from_affine_matrix(georeference_data[i].first);
				TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_poses[georeference_data[i].second]);

				Eigen::Matrix<double, 6, 1> relative_pose_measurement;
				relative_pose_measurement(0,0) = 0.0;
				relative_pose_measurement(1,0) = 0.0;
				relative_pose_measurement(2,0) = 0.0;
				relative_pose_measurement(3,0) = 0.0;
				relative_pose_measurement(4,0) = 0.0;
				relative_pose_measurement(5,0) = 0.0;

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_obs_eq_tait_bryan_wc_case1(
						delta,
						pose_s.px,
						pose_s.py,
						pose_s.pz,
						pose_s.om,
						pose_s.fi,
						pose_s.ka,
						pose_gps.px,
						pose_gps.py,
						pose_gps.pz,
						pose_gps.om,
						pose_gps.fi,
						pose_gps.ka,
						relative_pose_measurement(0,0),
						relative_pose_measurement(1,0),
						relative_pose_measurement(2,0),
						relative_pose_measurement(3,0),
						relative_pose_measurement(4,0),
						relative_pose_measurement(5,0));

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
						pose_s.px,
						pose_s.py,
						pose_s.pz,
						pose_s.om,
						pose_s.fi,
						pose_s.ka,
						pose_gps.px,
						pose_gps.py,
						pose_gps.pz,
						pose_gps.om,
						pose_gps.fi,
						pose_gps.ka);

				int ir = tripletListB.size();
				int ic_1 = georeference_data[i].second * 6;
				int ic_2 = m_poses.size() * 6 + i * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));

					tripletListA.emplace_back(ir + row, ic_2    , -jacobian(row,6));
					tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row,7));
					tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row,8));
					tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row,9));
					tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row,10));
					tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row,11));
				}

				tripletListB.emplace_back(ir,     0, delta(0,0));
				tripletListB.emplace_back(ir + 1, 0, delta(1,0));
				tripletListB.emplace_back(ir + 2, 0, delta(2,0));
				tripletListB.emplace_back(ir + 3, 0, delta(3,0));
				tripletListB.emplace_back(ir + 4, 0, delta(4,0));
				tripletListB.emplace_back(ir + 5, 0, delta(5,0));

				tripletListP.emplace_back(ir ,    ir,     cauchy(delta(0,0), 1));
				tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1,0), 1));
				tripletListP.emplace_back(ir + 2, ir + 2, cauchy(delta(2,0), 1));
				tripletListP.emplace_back(ir + 3, ir + 3, cauchy(delta(3,0), 1));
				tripletListP.emplace_back(ir + 4, ir + 4, cauchy(delta(4,0), 1));
				tripletListP.emplace_back(ir + 5, ir + 5, cauchy(delta(5,0), 1));
			}

			for(size_t i = 0; i < georeference_data.size(); i++){
				int ir = tripletListB.size();
				int ic = m_poses.size() * 6 + i * 6;
				TaitBryanPose pose_gps = pose_tait_bryan_from_affine_matrix(georeference_data[i].first);
				double residual;
				residual_constraint_fixed_optimization_parameter(residual, pose_gps.px, pose_gps.px);
				Eigen::Matrix<double, 1, 1> jacobian;
				residual_constraint_fixed_optimization_parameter_jacobian(jacobian, pose_gps.px, pose_gps.px);

				tripletListA.emplace_back(ir, ic  , -jacobian(0,0));
				tripletListB.emplace_back(ir, 0, residual);
				tripletListP.emplace_back(ir, ir, 1000000000);

				residual_constraint_fixed_optimization_parameter(residual, pose_gps.py, pose_gps.py);
				residual_constraint_fixed_optimization_parameter_jacobian(jacobian, pose_gps.py, pose_gps.py);
				tripletListA.emplace_back(ir + 1, ic + 1, -jacobian(0,0));
				tripletListB.emplace_back(ir + 1, 0, residual);
				tripletListP.emplace_back(ir + 1, ir + 1, 1000000000);

				residual_constraint_fixed_optimization_parameter(residual, pose_gps.pz, pose_gps.pz);
				residual_constraint_fixed_optimization_parameter_jacobian(jacobian, pose_gps.pz, pose_gps.pz);
				tripletListA.emplace_back(ir + 2, ic + 2, -jacobian(0,0));
				tripletListB.emplace_back(ir + 2, 0, residual);
				tripletListP.emplace_back(ir + 2, ir + 2, 1000000000);

				residual_constraint_fixed_optimization_parameter(residual, pose_gps.om, pose_gps.om);
				residual_constraint_fixed_optimization_parameter_jacobian(jacobian, pose_gps.om, pose_gps.om);
				tripletListA.emplace_back(ir + 3, ic + 3, -jacobian(0,0));
				tripletListB.emplace_back(ir + 3, 0, residual);
				tripletListP.emplace_back(ir + 3, ir + 3, 1000000000);

				residual_constraint_fixed_optimization_parameter(residual, pose_gps.fi, pose_gps.fi);
				residual_constraint_fixed_optimization_parameter_jacobian(jacobian, pose_gps.fi, pose_gps.fi);
				tripletListA.emplace_back(ir + 4, ic + 4, -jacobian(0,0));
				tripletListB.emplace_back(ir + 4, 0, residual);
				tripletListP.emplace_back(ir + 4, ir + 4, 1000000000);

				residual_constraint_fixed_optimization_parameter(residual, pose_gps.ka, pose_gps.ka);
				residual_constraint_fixed_optimization_parameter_jacobian(jacobian, pose_gps.ka, pose_gps.ka);
				tripletListA.emplace_back(ir + 5, ic + 5, -jacobian(0,0));
				tripletListB.emplace_back(ir + 5, 0, residual);
				tripletListP.emplace_back(ir + 5, ir + 5, 1000000000);
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), m_poses.size() * 6 + georeference_data.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(m_poses.size() * 6 + georeference_data.size() * 6, m_poses.size() * 6 + georeference_data.size() * 6);
			Eigen::SparseMatrix<double> AtPB(m_poses.size() * 6 + georeference_data.size() * 6 , 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;

			std::cout << "AtPA=AtPB SOLVED" << std::endl;

			for(size_t i = 0 ; i < h_x.size(); i++){
				std::cout << h_x[i] << std::endl;
			}

			if(h_x.size() == 6 * m_poses.size() + georeference_data.size() * 6){
				int counter = 0;

				for(size_t i = 0; i < m_poses.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_poses[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];
					m_poses[i] = affine_matrix_from_pose_tait_bryan(pose);
				}
				std::cout << "optimizing with tait bryan finished" << std::endl;
			}else{
				std::cout << "optimizing with tait bryan FAILED" << std::endl;
			}

			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: add noise to poses" << std::endl;
	std::cout << "t: optimize (Tait-Bryan and relative pose to GPS (half A))" << std::endl;
	std::cout << "y: optimize (Tait-Bryan and relative pose to GPS (full A))" << std::endl;
}


/***
 *  $Id$
 **
 *  File: graph_cc.hpp
 *  Created: May 9, 2012
 *
 *  Author: Olga Wodo, Baskar Ganapathysubramanian
 *  Copyright (c) 2012 Olga Wodo, Baskar Ganapthysubramanian
 *  See accompanying LICENSE.
 *
 *  This file is part of GraSPI.
 */

#ifndef GRAPH_CC_HPP
#define GRAPH_CC_HPP

#include "graspi_types.hpp"
#include "graspi_predicates.hpp"

#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>

namespace graspi{
    /// find the connected components
    ///
    /// This function finds the connected components where each component consists of vertices of the same color
    /// @param G is the graph
    /// @param C is the vector with the colors of vertices
    /// @param cVV is the vector storing the indices of the connected component of each vertex, this vector is initialized and filled as a result of this function execution
    inline void identify_connected_components(
                                              graspi::graph_t* G,
                                              const vertex_colors_t& C,
                                              vertex_ccs_t& vCC ){

        connect_same_color pred(*G, C);
        boost::filtered_graph<graspi::graph_t,connect_same_color> FG(*G, pred);

        int size_of_G = boost::num_vertices(*G);
        vCC.resize(size_of_G, 0);

        boost::connected_components(FG, &vCC[0]);
    }//identify_connected_components

    /// This function determines the number of connected components
    ///
    /// This function determines the number of connected components once the connected components are identified for each vertex in the graph
    /// @param vCC is the vector storing the indices of the connected component of each vertex
    /// @param d_g is the structure storing basic informations about the graph dimensionality
    /// return the number of connected components (without the meta vertices)
    inline int determine_number_of_CCs(
                                       const graspi::vertex_ccs_t& vCC,
                                       const graspi::dim_g_t& d_g){

        int max_index = 0;
        // find max index
        for (unsigned int i = 0; i < vCC.size(); ++i) {
            std::cout << "id,cc: " << i << " " << vCC[i] << std::endl;
            if(vCC[i] > max_index)
                max_index = vCC[i];
        }
        int number_of_conn_comp =  max_index + 1;
        // correct nOfCC - subtract meta vertices that are identified as separate CC
        return number_of_conn_comp - d_g.n_meta_total();
    }//determine_number_of_CCs

    /// This function determines the basic statistics of graph in terms of connected components
    ///
    /// This function determines the statistics of the graph related to the connected components
    /// @param G is the input graph
    /// @param d_g is the structure storing basic informations about the graph dimensionality
    /// @param CCs is structure storing information about individual components (e.g. if the CC is connected to meta vertex)
    /// @param C is the vector with the colors of vertices
    /// @param vCC is the vector storing the indices of the connected component of each vertex
    inline void
    determine_basic_stats_of_ccs( graspi::graph_t* G,
                                 const graspi::dim_g_t& d_g,
                                 graspi::ccs_t& CCs,
                                 const graspi::vertex_colors_t& C,
                                 const graspi::vertex_ccs_t&   vCCs ){

        std::vector<int>::const_iterator it = max_element( vCCs.begin(),
                                                          vCCs.end() );
        CCs.resize( (*it)+1 );

#ifdef DEBUG
        std::cout << "Total number of connected components" << *it << std::endl;
#endif
        for (unsigned int  i = 0; i < vCCs.size(); i++){
            CCs[vCCs[i]].color = C[i];
            CCs[vCCs[i]].size++;
        }

//        unsigned int size_of_G = boost::num_vertices(*G);

        // determine all vertices connected to the bottom electrode
        std::set<int> comp_conn_to_electrode;
        graspi::vertex_t source = d_g.id_blue(); // bottom electrode index
        graspi::graph_t::adjacency_iterator u, u_end;
        boost::tie(u, u_end) = boost::adjacent_vertices(source, *G);
        for (; u != u_end; ++u) {
            comp_conn_to_electrode.insert(vCCs[*u]);
        }

#ifdef DEBUG
        std::cout << "Id of connected components connected to bottom " << std::endl;
        copy(comp_conn_to_electrode.begin(), comp_conn_to_electrode.end(),
             std::ostream_iterator<int>(std::cout, " "));
        std::cout << std::endl;
#endif

        // pass info about connectivity to bottom electrode to data of components
        for(unsigned int  i = 0; i < CCs.size(); i++ ){
            if( comp_conn_to_electrode.find(i) != comp_conn_to_electrode.end() )
                CCs[i].if_connected_to_electrode += 1;
        }

        // determine all vertices connected to the top electrode
        comp_conn_to_electrode.clear();
        source = d_g.id_red(); // top electrode index
        boost::tie(u, u_end) = boost::adjacent_vertices(source, *G);
        for (; u != u_end; ++u) {
            comp_conn_to_electrode.insert(vCCs[*u]);
        }

#ifdef DEBUG
        std::cout << "Id of connected components conn to top " << std::endl;
        std::set<int>::iterator its = comp_conn_to_electrode.begin();
        for (int i = 0; i < comp_conn_to_electrode.size(); i++){
            int id = *its;
            std::cout << id << " " << CCs[id].color << std::endl;
            its++;
        }
        std::cout << std::endl;
#endif

        // pass info about connectivity to top electrode to data of components
        for(unsigned int  i = 0; i < CCs.size(); i++ ){
            if( comp_conn_to_electrode.find(i) != comp_conn_to_electrode.end() )
                CCs[i].if_connected_to_electrode += 2;
        }
    }//determine_basic_stats_of_ccs

}
#endif


//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//

#include "pxr/pxr.h"
#include "pxr/base/gf/rotation.h"
#include "pxr/base/tf/pyUtils.h"
#include "pxr/base/tf/wrapTypeHelpers.h"
#include "pxr/base/tf/pyContainerConversions.h"

#include <boost/python/class.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/operators.hpp>
#include <boost/python/return_arg.hpp>

#include <string>

using namespace boost::python;

using std::string;

PXR_NAMESPACE_USING_DIRECTIVE

namespace {

void SetAxisHelper( GfRotation &rotation, const GfVec3d &axis )
{
    rotation.SetAxisAngle( axis, rotation.GetAngle() );
}

void SetAngleHelper( GfRotation &rotation, double angle )
{
    rotation.SetAxisAngle( rotation.GetAxis(), angle );
}

static tuple
_DecomposeRotation3(const GfMatrix4d &rot,
                    const GfVec3d &TwAxis,
                    const GfVec3d &FBAxis,
                    const GfVec3d &LRAxis,
                    double handedness,
                    double thetaTwHint,
                    double thetaFBHint,
                    double thetaLRHint,
                    bool useHint)
{
    double angle[3] = {thetaTwHint, thetaFBHint, thetaLRHint};

    GfRotation::DecomposeRotation(rot, TwAxis, FBAxis, LRAxis, handedness,
                                  &(angle[0]),
                                  &(angle[1]),
                                  &(angle[2]),
                                  NULL /* thetaSwHint */,
                                  useHint,
                                  NULL /* swShift */);

    return make_tuple(angle[0], angle[1], angle[2]);
}

static tuple
_DecomposeRotation(const GfMatrix4d &rot,
                    const GfVec3d &TwAxis,
                    const GfVec3d &FBAxis,
                    const GfVec3d &LRAxis,
                    double handedness,
                    const object &thetaTwHint,
                    const object &thetaFBHint,
                    const object &thetaLRHint,
                    const object &thetaSwHint,
                    bool useHint,
                    const object & swShiftIn)
{
    double angle[4] = {
        thetaTwHint.ptr() != Py_None ?
            boost::python::extract<double>(thetaTwHint) : 0.0,
        thetaFBHint.ptr() != Py_None ?
            boost::python::extract<double>(thetaFBHint) : 0.0,
        thetaLRHint.ptr() != Py_None ?
            boost::python::extract<double>(thetaLRHint) : 0.0,
        thetaSwHint.ptr() != Py_None ?
            boost::python::extract<double>(thetaSwHint) : 0.0
        };
    double swShift = swShiftIn.ptr() != Py_None ?
        boost::python::extract<double>(swShiftIn) : 0.0;

    GfRotation::DecomposeRotation(
        rot, TwAxis, FBAxis, LRAxis, handedness,
        thetaTwHint.ptr() != Py_None ? &(angle[0]) : NULL,
        thetaFBHint.ptr() != Py_None ? &(angle[1]) : NULL,
        thetaLRHint.ptr() != Py_None ? &(angle[2]) : NULL,
        thetaSwHint.ptr() != Py_None ? &(angle[3]) : NULL,
        useHint,
        swShiftIn.ptr() != Py_None ? &swShift : NULL);

    return make_tuple(angle[0], angle[1], angle[2], angle[3]);
}

static string _Repr(GfRotation const &self) {
    return TF_PY_REPR_PREFIX + "Rotation(" + TfPyRepr(self.GetAxis()) + ", " +
        TfPyRepr(self.GetAngle()) + ")";
}

#if PY_MAJOR_VERSION == 2
static GfRotation __truediv__(const GfRotation &self, double value)
{
    return self / value;
}

static GfRotation __itruediv__(GfRotation &self, double value)
{
    return self /= value;
}
#endif

} // anonymous namespace

void wrapRotation()
{
    typedef GfRotation This;

    class_<This> ( "Rotation", "3-space rotation", init<>())

        .def(init<const GfVec3d &, double>())
        .def(init<const GfQuaternion &>())
        .def(init<const GfQuatd &>())
        .def(init<const GfVec3d &, const GfVec3d &>())

        .def( TfTypePythonClass() )

        .def("SetAxisAngle", &This::SetAxisAngle, return_self<>(),
             (args("axis"), args("angle")))
        .def("SetQuat", &This::SetQuat, return_self<>(), (args("quat")))
        .def("SetQuaternion", &This::SetQuaternion, return_self<>(),
             (args("quaternion")))
        .def("SetRotateInto", &This::SetRotateInto, return_self<>(),
             (args("rotateFrom"), args("rotateTo")))
        .def("SetIdentity", &This::SetIdentity, return_self<>())

        .add_property("axis", make_function(&This::GetAxis, return_value_policy<copy_const_reference>()),
                      SetAxisHelper )
        .add_property("angle", &This::GetAngle, SetAngleHelper)

        .def("GetAxis", &This::GetAxis, return_value_policy<copy_const_reference>())
        .def("GetAngle", &This::GetAngle)

        .def("GetQuaternion", &This::GetQuaternion)
        .def("GetQuat", &This::GetQuat)

        .def("GetInverse", &This::GetInverse)

        .def("Decompose", &This::Decompose)

        .def("DecomposeRotation3", _DecomposeRotation3,
             (arg("rot"),
              arg("twAxis"),
              arg("fbAxis"),
              arg("lrAxis"),
              arg("handedness"),
              arg("thetaTwHint") = 0.0,
              arg("thetaFBHint") = 0.0,
              arg("thetaLRHint") = 0.0,
              arg("useHint") = false)
             )
        .staticmethod("DecomposeRotation3")

        .def("DecomposeRotation", _DecomposeRotation,
             (arg("rot"),
              arg("twAxis"),
              arg("fbAxis"),
              arg("lrAxis"),
              arg("handedness"),
              arg("thetaTwHint"),
              arg("thetaFBHint"),
              arg("thetaLRHint"),
              arg("thetaSwHint") = object(),
              arg("useHint") = false,
              arg("swShift") = object())
             )
        .staticmethod("DecomposeRotation")

        .def("RotateOntoProjected", &This::RotateOntoProjected)
        .staticmethod("RotateOntoProjected")

        .def("TransformDir", (GfVec3f (This::*)( const GfVec3f & ) const)&This::TransformDir )
        .def("TransformDir", (GfVec3d (This::*)( const GfVec3d & ) const)&This::TransformDir )

        .def( str(self) )
        .def( self == self )
        .def( self != self )
        .def( self *= self )
        .def( self *= double() )
        .def( self /= double() )
        .def( self * self )
        .def( self * double() )
        .def( double() * self )
        .def( self / double() )

 #if PY_MAJOR_VERSION == 2
        // Needed only to support "from __future__ import division" in
        // python 2. In python 3 builds boost::python adds this for us.
        .def("__truediv__", __truediv__ )
        .def("__itruediv__", __itruediv__ )
#endif

       .def("__repr__", _Repr)

        ;
    to_python_converter<std::vector<This>,
        TfPySequenceToPython<std::vector<This> > >();

}