#include "../Base64SurveyRegistry.hpp"
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/archive/iterators/ostream_iterator.hpp>
#include <sstream>
#include <string>
#include <iostream>
#include <vector>

using namespace boost::archive::iterators;

struct Boost
{
    std::string encode(const std::string &bytes)
    {
        using namespace boost::archive::iterators;
        using It = base64_from_binary<transform_width<std::string::const_iterator, 6, 8>>;
        auto base64 = std::string(It(bytes.begin()), It(bytes.end()));
        // Add padding.
        return base64.append((3 - bytes.size() % 3) % 3, '=');
    }

    std::string decode(const std::string &base64)
	{
        using namespace boost::archive::iterators;
        using It = transform_width<binary_from_base64<std::string::const_iterator>, 8, 6>;
        auto binary = std::string(It(base64.begin()), It(base64.end()));
        // Remove padding.
        auto length = base64.size();
        if (binary.size() > 2 && base64[length - 1] == '=' && base64[length - 2] == '=')
        {
            binary.erase(binary.end() - 2, binary.end());
        }
        else if (binary.size() > 1 && base64[length - 1] == '=')
        {
            binary.erase(binary.end() - 1, binary.end());
        }
        return binary;
    }
};

BASE64_REGISTER_ENCODER(Boost);
BASE64_REGISTER_DECODER(Boost);


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

/*
 Copyright (C) 2008 Andreas Gaida
 Copyright (C) 2008, 2009 Ralph Schreyer
 Copyright (C) 2008, 2009 Klaus Spanderen

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

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

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

#include <ql/pricingengines/barrier/fdhestonbarrierengine.hpp>
#include <ql/methods/finitedifferences/stepconditions/fdmstepconditioncomposite.hpp>
#include <ql/methods/finitedifferences/utilities/fdmdividendhandler.hpp>
#include <ql/methods/finitedifferences/meshers/fdmhestonvariancemesher.hpp>
#include <ql/methods/finitedifferences/utilities/fdmdirichletboundary.hpp>
#include <ql/methods/finitedifferences/utilities/fdminnervaluecalculator.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearoplayout.hpp>
#include <ql/methods/finitedifferences/meshers/fdmmeshercomposite.hpp>
#include <ql/methods/finitedifferences/meshers/fdmblackscholesmesher.hpp>
#include <ql/pricingengines/barrier/fdhestonrebateengine.hpp>
#include <ql/pricingengines/vanilla/fdhestonvanillaengine.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

    FdHestonBarrierEngine::FdHestonBarrierEngine(
            const boost::shared_ptr<HestonModel>& model,
            Size tGrid, Size xGrid, Size vGrid, Size dampingSteps,
            const FdmSchemeDesc& schemeDesc,
            const boost::shared_ptr<LocalVolTermStructure>& leverageFct)
    : GenericModelEngine<HestonModel,
                        DividendBarrierOption::arguments,
                        DividendBarrierOption::results>(model),
      tGrid_(tGrid), xGrid_(xGrid),
      vGrid_(vGrid), dampingSteps_(dampingSteps),
      schemeDesc_(schemeDesc),
      leverageFct_(leverageFct) {
    }

    void FdHestonBarrierEngine::calculate() const {

        // 1. Mesher
        const boost::shared_ptr<HestonProcess>& process = model_->process();
        const Time maturity = process->time(arguments_.exercise->lastDate());

        // 1.1 The variance mesher
        const Size tGridMin = 5;
        const boost::shared_ptr<FdmHestonVarianceMesher> varianceMesher(
			boost::make_shared<FdmHestonVarianceMesher>(vGrid_, process, maturity,
                                        std::max(tGridMin, tGrid_/50)));

        // 1.2 The equity mesher
        const boost::shared_ptr<StrikedTypePayoff> payoff =
            boost::dynamic_pointer_cast<StrikedTypePayoff>(arguments_.payoff);

        Real xMin=Null<Real>();
        Real xMax=Null<Real>();
        if (   arguments_.barrierType == Barrier::DownIn
            || arguments_.barrierType == Barrier::DownOut) {
            xMin = std::log(arguments_.barrier);
        }
        if (   arguments_.barrierType == Barrier::UpIn
            || arguments_.barrierType == Barrier::UpOut) {
            xMax = std::log(arguments_.barrier);
        }

        const boost::shared_ptr<Fdm1dMesher> equityMesher(
            new FdmBlackScholesMesher(
                xGrid_,
                FdmBlackScholesMesher::processHelper(
                    process->s0(), process->dividendYield(),
                    process->riskFreeRate(), varianceMesher->volaEstimate()),
                maturity, payoff->strike(),
                xMin, xMax, 0.0001, 1.5,
                std::make_pair(Null<Real>(), Null<Real>()),
                arguments_.cashFlow));

        const boost::shared_ptr<FdmMesher> mesher (
			boost::make_shared<FdmMesherComposite>(equityMesher, varianceMesher));

        // 2. Calculator
        boost::shared_ptr<FdmInnerValueCalculator> calculator(
			boost::make_shared<FdmLogInnerValue>(payoff, mesher, 0));

        // 3. Step conditions
        std::list<boost::shared_ptr<StepCondition<Array> > > stepConditions;
        std::list<std::vector<Time> > stoppingTimes;

        // 3.1 Step condition if discrete dividends
        boost::shared_ptr<FdmDividendHandler> dividendCondition(
			boost::make_shared<FdmDividendHandler>(arguments_.cashFlow, mesher,
                                   process->riskFreeRate()->referenceDate(),
                                   process->riskFreeRate()->dayCounter(), 0));

        if(!arguments_.cashFlow.empty()) {
            stepConditions.push_back(dividendCondition);
            stoppingTimes.push_back(dividendCondition->dividendTimes());
        }

        QL_REQUIRE(arguments_.exercise->type() == Exercise::European,
                   "only european style option are supported");

        boost::shared_ptr<FdmStepConditionComposite> conditions(
			boost::make_shared<FdmStepConditionComposite>(stoppingTimes, stepConditions));

        // 4. Boundary conditions
        FdmBoundaryConditionSet boundaries;
        if (   arguments_.barrierType == Barrier::DownIn
            || arguments_.barrierType == Barrier::DownOut) {
            boundaries.push_back(
				boost::make_shared<FdmDirichletBoundary>(mesher, arguments_.rebate, 0,
                                         FdmDirichletBoundary::Lower));

        }
        if (   arguments_.barrierType == Barrier::UpIn
            || arguments_.barrierType == Barrier::UpOut) {
            boundaries.push_back(
				boost::make_shared<FdmDirichletBoundary>(mesher, arguments_.rebate, 0,
                                         FdmDirichletBoundary::Upper));
        }

        // 5. Solver
        FdmSolverDesc solverDesc = { mesher, boundaries, conditions,
                                     calculator, maturity,
                                     tGrid_, dampingSteps_ };

        boost::shared_ptr<FdmHestonSolver> solver(boost::make_shared<FdmHestonSolver>(
                    Handle<HestonProcess>(process), solverDesc, schemeDesc_,
                    Handle<FdmQuantoHelper>(), leverageFct_));

        const Real spot = process->s0()->value();
        results_.value = solver->valueAt(spot, process->v0());
        results_.delta = solver->deltaAt(spot, process->v0());
        results_.gamma = solver->gammaAt(spot, process->v0());
        results_.theta = solver->thetaAt(spot, process->v0());

        // 6. Calculate vanilla option and rebate for in-barriers
        if (   arguments_.barrierType == Barrier::DownIn
            || arguments_.barrierType == Barrier::UpIn) {
            // Cast the payoff
            boost::shared_ptr<StrikedTypePayoff> payoff =
                    boost::dynamic_pointer_cast<StrikedTypePayoff>(
                                                            arguments_.payoff);
            // Calculate the vanilla option
            boost::shared_ptr<DividendVanillaOption> vanillaOption(
				boost::make_shared<DividendVanillaOption>(payoff,arguments_.exercise,
                                          dividendCondition->dividendDates(),
                                          dividendCondition->dividends()));
            vanillaOption->setPricingEngine(boost::shared_ptr<PricingEngine>(
				boost::make_shared<FdHestonVanillaEngine>(*model_, tGrid_, xGrid_,
                                              vGrid_, dampingSteps_,
                                              schemeDesc_)));
            // Calculate the rebate value
            boost::shared_ptr<DividendBarrierOption> rebateOption(
				boost::make_shared<DividendBarrierOption>(arguments_.barrierType,
                                          arguments_.barrier,
                                          arguments_.rebate,
                                          payoff, arguments_.exercise,
                                          dividendCondition->dividendDates(),
                                          dividendCondition->dividends()));
            const Size xGridMin = 20;
            const Size vGridMin = 10;
            const Size rebateDampingSteps
                = (dampingSteps_ > 0) ? std::min(Size(1), dampingSteps_/2) : 0;
            rebateOption->setPricingEngine(
				boost::make_shared<FdHestonRebateEngine>(*model_, tGrid_,
                                             std::max(xGridMin, xGrid_/4),
                                             std::max(vGridMin, vGrid_/4),
                                             rebateDampingSteps,
                                             schemeDesc_));

            results_.value = vanillaOption->NPV()   + rebateOption->NPV()
                                                    - results_.value;
            results_.delta = vanillaOption->delta() + rebateOption->delta()
                                                    - results_.delta;
            results_.gamma = vanillaOption->gamma() + rebateOption->gamma()
                                                    - results_.gamma;
            results_.theta = vanillaOption->theta() + rebateOption->theta()
                                                    - results_.theta;
        }
    }
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/posmax.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" posmax",  STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::posmax;
  using r_t = decltype(posmax(T()));

  // specific values tests
  STF_ULP_EQUAL(posmax(bs::Inf<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(posmax(bs::Minf<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(posmax(bs::Mone<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(posmax(bs::Nan<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(posmax(bs::One<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(posmax(bs::Zero<T>()), bs::Zero<r_t>(), 0);
}


#include "optimization_schw.hpp"
#include "datatypes.hpp"
#include <boost/python.hpp>

namespace gd {
using namespace boost::python;

void py_export_schw_opt2() {
	class_<OptimizationProblemSchw >("OptimizationProblemSchw", init<double_matrix, double_matrix, double_vector, double_vector, double_vector, double, double, bool, bool, bool>())
		.def("optimize", (&OptimizationProblemSchw::optimize))
			.def("likelihood", (&OptimizationProblemSchw::_likelihood))
			.def("dfdx", (&OptimizationProblemSchw::_dfdx))
			.def("hessian", (&OptimizationProblemSchw::_hessian))
			;

	class_<OptimizationMatrixChiSquare>("OptimizationMatrixChiSquare", init<double_matrix, double_vector>())
			.def("logp", (&OptimizationMatrixChiSquare::_logp))
			.def("dlogpdx", (&OptimizationMatrixChiSquare::_dlogpdx))
			;
	class_<OptimizationMatrix>("OptimizationMatrix", init<double_matrix, double_vector>())
			.def("logp", (&OptimizationMatrix::_logp))
			.def("dlogpdx", (&OptimizationMatrix::_dlogpdx))
			;
	class_<OptimizationMatrixForegroundConditional>("OptimizationMatrixForegroundConditional", init<double_matrix, double_matrix, double_vector, double_vector>())
			.def("logp", (&OptimizationMatrixForegroundConditional::_logp))
			.def("dlogpdx", (&OptimizationMatrixForegroundConditional::_dlogpdx))
			;
	class_<OptimizationMatrixN>("OptimizationMatrixN", init<double_matrix, double_vector, double_vector>())
			.def("logp", (&OptimizationMatrixN::_logp))
			.def("dlogpdx", (&OptimizationMatrixN::_dlogpdx))
			;
	class_<OptimizationQP>("OptimizationQP", init<double_matrix, double_vector>())
			.def("logp", (&OptimizationQP::_logp))
			.def("dlogpdx", (&OptimizationQP::_dlogpdx))
			;
	class_<OptimizationNormalize>("OptimizationNormalize", init<double, double>())
			.def("logp", (&OptimizationNormalize::_logp))
			.def("dlogpdx", (&OptimizationNormalize::_dlogpdx))
			;
	class_<OptimizationNormalizeMass>("OptimizationNormalizeMass", init<double_vector, double, double>())
			.def("logp", (&OptimizationNormalizeMass::_logp))
			.def("dlogpdx", (&OptimizationNormalizeMass::_dlogpdx))
			;
	class_<OptimizationEntropy>("OptimizationEntropy", init<double>())
			.def("logp", (&OptimizationEntropy::_logp))
			.def("dlogpdx", (&OptimizationEntropy::_dlogpdx))
			;
}

};


/**
 * @file
 */

#ifndef WAVE_GEOMETRY_GEOMETRY_HPP
#define WAVE_GEOMETRY_GEOMETRY_HPP

#include <Eigen/Geometry>

#include "core.hpp"

#include "src/util/math/CrossMatrix.hpp"

#include "src/geometry/forward_declarations.hpp"
#include "src/geometry/type_traits.hpp"

// Frames
#include "src/geometry/frames/frame_traits.hpp"
#include "src/geometry/frames/frame_cast.hpp"
#include "src/geometry/frames/Framed.hpp"

#include "src/geometry/op/CommonExpressions.hpp"

// Geometric leaves and bases
#include "src/geometry/base/CompoundBase.hpp"
#include "src/geometry/base/ScalarBase.hpp"
#include "src/geometry/base/AffineBase.hpp"
#include "src/geometry/base/VectorBase.hpp"
#include "src/geometry/base/TransformBase.hpp"
#include "src/geometry/base/RotationBase.hpp"
#include "src/geometry/leaf/Scalar.hpp"
#include "src/geometry/leaf/MatrixRotation.hpp"
#include "src/geometry/leaf/QuaternionRotation.hpp"
#include "src/geometry/leaf/AngleAxisRotation.hpp"
#include "src/geometry/base/RelativeRotationBase.hpp"
#include "src/geometry/leaf/RelativeRotation.hpp"
#include "src/geometry/base/PointBase.hpp"
#include "src/geometry/leaf/Point.hpp"
#include "src/geometry/base/TranslationBase.hpp"
#include "src/geometry/leaf/Translation.hpp"
#include "src/geometry/leaf/Zero.hpp"
#include "src/geometry/leaf/Identity.hpp"
#include "src/geometry/leaf/MatrixRigidTransform.hpp"
#include "src/geometry/leaf/CompactRigidTransform.hpp"
#include "src/geometry/base/RigidTransformBase.hpp"
#include "src/geometry/base/TwistBase.hpp"
#include "src/geometry/leaf/Twist.hpp"

// Expressions
#include "src/geometry/op/Sum.hpp"
#include "src/geometry/op/Subtract.hpp"
#include "src/geometry/op/Rotate.hpp"
#include "src/geometry/op/Transform.hpp"
#include "src/geometry/op/Compose.hpp"
#include "src/geometry/op/ExpMap.hpp"
#include "src/geometry/op/LogMap.hpp"
#include "src/geometry/op/BoxPlus.hpp"
#include "src/geometry/op/BoxMinus.hpp"
#include "src/geometry/op/Minus.hpp"
#include "src/geometry/op/Scale.hpp"
#include "src/geometry/op/Product.hpp"
#include "src/geometry/op/Divide.hpp"
#include "src/geometry/op/Inverse.hpp"

#endif  // WAVE_GEOMETRY_GEOMETRY_HPP


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>

#include "Tudat/Astrodynamics/Gravitation/tabulatedGravityFieldVariations.h"

namespace tudat
{

namespace gravitation
{

//! Class constructor
TabulatedGravityFieldVariations::TabulatedGravityFieldVariations(
        const std::map< double, Eigen::MatrixXd >& cosineCoefficientCorrections,
        const std::map< double, Eigen::MatrixXd >& sineCoefficientCorrections,
        const int minimumDegree, const int minimumOrder,
        const std::shared_ptr< interpolators::InterpolatorSettings >interpolatorType ):
    GravityFieldVariations( minimumDegree, minimumOrder,
                            minimumDegree + cosineCoefficientCorrections.begin( )->second.rows( ) - 1,
                            minimumOrder + cosineCoefficientCorrections.begin( )->second.cols( ) - 1 ),
    interpolatorType_( interpolatorType )
{
    // Create interpolator for tabulated coefficients.
    resetCoefficientInterpolator( cosineCoefficientCorrections, sineCoefficientCorrections );
}

//! Function to (re)set the tabulated spherical harmonic coefficients.
void TabulatedGravityFieldVariations::resetCoefficientInterpolator(
        const std::map< double, Eigen::MatrixXd >& cosineCoefficientCorrections,
        const std::map< double, Eigen::MatrixXd >& sineCoefficientCorrections )
{
    // Set current coefficient tables.
    cosineCoefficientCorrections_ = cosineCoefficientCorrections;
    sineCoefficientCorrections_ = sineCoefficientCorrections;

    // Check consistency of map sizes.
    if( cosineCoefficientCorrections_.size( ) != sineCoefficientCorrections_.size( ) )
    {
        throw std::runtime_error( "Error when resetting tabulated gravity field corrections, sine and cosine data size incompatible" );
    }

    // Create iterators over maps.
    std::map< double, Eigen::MatrixXd >::iterator cosineIterator =
            cosineCoefficientCorrections_.begin( );
    std::map< double, Eigen::MatrixXd >::iterator sineIterator =
            sineCoefficientCorrections_.begin( );

    // Declare map to store concatenated (horizontally) [cosine|sine] coefficients for
    // given discrete times.
    std::map< double, Eigen::MatrixXd > sineCosinePairMap;

    // Declare matrix to set concatenated (horizontally) [cosine|sine] coefficients at
    // current time in loop.
    Eigen::MatrixXd currentCoefficients = Eigen::MatrixXd::Zero( numberOfDegrees_, 2 * numberOfOrders_ );

    // Iterate over all times in correction maps.
    for( unsigned int i = 0; i < cosineCoefficientCorrections_.size( ); i++ )
    {
        // Check whether input times are consistent
        if( cosineIterator->first != sineIterator->first )
        {
            std::string errorMessage = "Error when resetting tabulated gravity field corrections, sine and cosine data time differ by" +
                    std::to_string(  cosineIterator->first - sineIterator->first );
            throw std::runtime_error( errorMessage );
        }
        // Check whether matrix sizes are consistent.
        if( ( cosineIterator->second.rows( ) != numberOfDegrees_ ) ||
                ( sineIterator->second.rows( ) != numberOfDegrees_ ) ||
                ( cosineIterator->second.cols( ) != numberOfOrders_ ) ||
                ( sineIterator->second.cols( ) != numberOfOrders_ ) )
        {
            std::string errorMessage = "Error when resetting tabulated gravity field corrections, sine and cosine blocks of inconsistent size";
            throw std::runtime_error( errorMessage );
        }

        // Concatenate cosine and sine matrices
        currentCoefficients.block( 0, 0, numberOfDegrees_, numberOfOrders_ ) =
                cosineIterator->second;
        currentCoefficients.block( 0, numberOfOrders_, numberOfDegrees_, numberOfOrders_ ) =
                sineIterator->second;

        // Set as entry in map of concatenated matrices
        sineCosinePairMap[ cosineIterator->first ] = currentCoefficients;

        // Increment iterators.
        cosineIterator++;
        sineIterator++;
    }

    // Create interpolator
    variationInterpolator_ =
            interpolators::createOneDimensionalInterpolator< double, Eigen::MatrixXd >(
                sineCosinePairMap, interpolatorType_ );
}

//! Function for calculating corrections by interpolating tabulated corrections.
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > TabulatedGravityFieldVariations::
calculateSphericalHarmonicsCorrections(
        const double time )
{
    // Interpolate corrections
    Eigen::MatrixXd cosineSinePair = variationInterpolator_->interpolate( time );

    // Split interpolated concatenated matrix and return.
    return std::make_pair( cosineSinePair.block( 0, 0, numberOfDegrees_, numberOfOrders_ ),
                           cosineSinePair.block( 0, numberOfOrders_, numberOfDegrees_, numberOfOrders_ ) );
}

} // namespace gravitation

} // namespace tudat


//
// ortho view generation tool
// ulrich.krispel@fraunhofer.at
//

#include <vector>
#include <string>
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <limits>
#include <boost/program_options.hpp>

#include "types.h"
#include "projection.h"
#include "meanshift.h"
#include "extraction.h"

#include "../lib/frozen/frozen.h"

namespace po = boost::program_options;

// configuration variables
double WINDOW_SIZE_NORMAL = 0.3;
double WINDOW_SIZE_DISTANCE = 0.1;

int main(int ac, char* av[])
{
    SphericalPanoramaImageProjection projection;
    myIFS ingeometry;
    double resolution = 1.0;        // default: 1 mm/pixel
    double scalefactor = 1.0;       // m
    bool   exportOBJ = false;
    bool   exportSphere = false;
    bool   exportQuadGeometry = false;
    std::cout << "OrthoGen orthographic image generator for DuraArk" << std::endl;
    std::cout << "developed by Fraunhofer Austria Research GmbH" << std::endl;
    std::string output = "ortho";

    try
    {
        po::options_description desc("commandline options");
        desc.add_options()
            ("help", "show this help message")
            ("im", po::value< std::string >(), "input panoramic image [.jpg]")
            ("ig", po::value< std::string >(), "input geometry [.OBJ]")
            ("res", po::value< double >(), "resolution [mm/pixel], default 1mm/pixel")
            ("trans", po::value< std::vector<double> >()->multitoken(), "transformation [x,y,z]")
            ("rot", po::value< std::vector<double> >()->multitoken(), "rotation quaternion [w,x,y,z]")
            ("elevation", po::value< std::vector<double> >()->multitoken(), "elevation angle bounds [min..max], default [-PI/2..PI/2]" )
            ("azimuth", po::value< std::vector<double> >()->multitoken(), "azimuth angle bounds [min..max], default [0..2*PI]")
            ("exgeom", po::value< bool >(), "export (textured) geometry [OBJ] 0/1, default 0 (false)")
            ("exquad", po::value< bool >(), "export extracted quads as (textured) geometry [OBJ] 0/1, default 0 (false)")
            ("exsphere", po::value< bool >(), "export panoramic sphere [OBJ] 0/1, default 0 (false)")
            ("scale", po::value< std::string >(), "scale of input coordinates (mm/cm/m), default m")
            ("ncluster", po::value< double >(), "geometry element normal direction clustering window size, default 0.3")
            ("dcluster", po::value< double >(), "geometry element planar distance clustering window size in m, default 0.1")
            ("output", po::value< std::string >(), "output filename [.jpg] will be appended")
            ;

        po::variables_map vm;
        po::store(po::parse_command_line(ac, av, desc, po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
        po::notify(vm);

        if (vm.count("help"))
        {
            std::cout << desc << "\n";
            return 0;
        }

        if (vm.count("im"))
        {
            Image img;
            if (loadJPEG(vm["im"].as<std::string>().c_str(), img))
            {
                projection.setPanoramicImage(img);
            }
            else
            {
                std::cout << "Error: could not open "
                    << vm["im"].as<std::string>() << std::endl;
            }
        }

        if (vm.count("ig"))
        {
            // load geometry
            ingeometry = IFS::loadOBJ<myIFS>(vm["ig"].as<std::string>());
            if (!ingeometry.isValid())
                std::cout << "Error: could not open "
                          << vm["ig"].as<std::string>() << std::endl;
        }

        if (vm.count("res"))
        {
            resolution = vm["res"].as<double>();
        }

        if (vm.count("ncluster"))
        {
            WINDOW_SIZE_NORMAL = vm["ncluster"].as<double>();
        }

        if (vm.count("dcluster"))
        {
            WINDOW_SIZE_DISTANCE = vm["dcluster"].as<double>();
        }

        if (vm.count("trans"))
        {
            std::vector<double> trans = vm["trans"].as<std::vector<double> >();
            projection.setPosition(Vec3d(trans[0], trans[1], trans[2]));
        }

        if (vm.count("rot"))
        {
            std::vector<double> rot = vm["rot"].as<std::vector<double> >();
            if (rot.size() == 4)
            {
                Quaterniond quaternion(rot[0], rot[1], rot[2], rot[3]);
                std::cout << " applying quaternion [" << quaternion.x() << ","
                    << quaternion.y() << "," << quaternion.z() << "," << quaternion.w()
                    << "]" << std::endl;
                projection.applyRotation(quaternion);
            }
            else
            {
                std::cout << " --rot Error: quaternion is not composed of 4 values."
                          << std::endl;
            }
        }

        if (vm.count("elevation"))
        {
            std::vector<double> el = vm["elevation"].as<std::vector<double> >();
            projection.elevationRange[0] = el[0];
            projection.elevationRange[1] = el[1];
        }

        if (vm.count("azimuth"))
        {
            std::vector<double> az = vm["azimuth"].as<std::vector<double> >();
            projection.azimuthRange[0] = az[0];
            projection.azimuthRange[1] = az[1];
        }

        if (vm.count("exgeom"))
        {
            exportOBJ = vm["exgeom"].as<bool>();
        }
        if (vm.count("exquad"))
        {
            exportQuadGeometry = vm["exgeom"].as<bool>();
        }
        if (vm.count("exsphere"))
        {
            exportSphere = vm["exsphere"].as<bool>();
        }
        if (vm.count("scale"))
        {
            std::string s = vm["scale"].as<std::string>();
            transform(s.begin(), s.end(), s.begin(), toupper);
            if (s.compare("MM") == 0) scalefactor = 1000.0;
            if (s.compare("CM") == 0) scalefactor = 100.0;
            if (s.compare("DM") == 0) scalefactor = 10.0;
            if (s.compare("M") == 0)  scalefactor = 1.0;
            if (s.compare("KM") == 0) scalefactor = 0.001;
        }

        if (vm.count("output"))
        {
            output = vm["output"].as<std::string>();
        }
    }
    catch(std::exception& e)
    {
        std::cerr << "error: " << e.what() << "\n";
        return 1;
    }

    if (projection.isValid() && ingeometry.isValid())
    {
        const Image &img = projection.img();
        std::cout << "input image is " << img.width() << " x " << img.height() << " pixels." << std::endl;
        std::cout << "input geometry consists of " << ingeometry.vertices.size() << " vertices and " << ingeometry.faces.size() << " faces." << std::endl;
        std::cout << "using a resolution of " << resolution << "mm/pixel" << std::endl;

        // set up resolution scale from mm to actual scale / pixel
        resolution = resolution * scalefactor / 1000;

        // PROCESS OUTPUT
        if (exportSphere)
        {
            double radius1m = 0.5 * scalefactor;
            std::cout << "- exporting projected panorama OBJ.." << std::endl;
            myIFS sphere = projection.exportTexturedSphere(radius1m, 100);
            sphere.materials.push_back(IFS::Material("sphere", "sphere.jpg"));
            sphere.facematerial.push_back(sphere.materials.size() - 1);
            IFS::exportOBJ(sphere, "sphere", "# OrthoGen panoramic sphere\n");
            saveJPEG("sphere.jpg", projection.img());

            // projection.exportPointCloud(radius1m, 1000);
        }

        // EXTRACT QUADS FROM GEOMETRY
        std::cout << "Using a meanshift size of " << WINDOW_SIZE_NORMAL << " for normal clustering." << std::endl;
        std::cout << "Using a meanshift size of " << WINDOW_SIZE_DISTANCE << " for plane distance clustering." << std::endl;

        std::vector<Quad3Dd> quads;
        std::vector<Triangle> triangles;
        extract_quads(ingeometry, scalefactor, triangles, quads, WINDOW_SIZE_DISTANCE, WINDOW_SIZE_NORMAL);

        myIFS outgeometry;                      // ifs with input geometry with texture coords
        outgeometry.useTextureCoordinates = true;

        std::cout << "Exporting " << quads.size() << " ortho views." << std::endl;
        {
            myIFS quadgeometry;                     // ifs with orthophoto quads

            quadgeometry.useTextureCoordinates = true;
            quadgeometry.texcoordinates.push_back(Vec2d(0, 1));
            quadgeometry.texcoordinates.push_back(Vec2d(0, 0));
            quadgeometry.texcoordinates.push_back(Vec2d(1, 0));
            quadgeometry.texcoordinates.push_back(Vec2d(1, 1));
            int qid = 0;
            for (auto const &q : quads)
            {
                // create orthophoto projection
                Image orthophoto = q.performProjection(projection, resolution);
                {
                    std::ostringstream oss;
                    if (quads.size() > 1)
                        oss << output << "_" << qid << ".jpg";
                    else
                        oss << output << ".jpg";
                    saveJPEG(oss.str().c_str(), orthophoto);
                    std::cout << oss.str() << " : " << orthophoto.width() << "x" << orthophoto.height() << " n: " << q.pose.Z << std::endl;
                }

                std::ostringstream matname, texname;
                matname << "ortho" << qid;
                texname << "ortho_" << qid << ".jpg";

                if (exportQuadGeometry)
                {
                    // create quad geometry, quads always use the same texcoordinates
                    myIFS::IFSINDICES face;
                    myIFS::IFSINDICES facetc;
                    face.push_back(quadgeometry.vertex2index(q.V[0]));
                    facetc.push_back(0);
                    face.push_back(quadgeometry.vertex2index(q.V[1]));
                    facetc.push_back(1);
                    face.push_back(quadgeometry.vertex2index(q.V[2]));
                    facetc.push_back(2);
                    face.push_back(quadgeometry.vertex2index(q.V[3]));
                    facetc.push_back(3);
                    quadgeometry.faces.push_back(face);
                    quadgeometry.facetexc.push_back(facetc);
                    quadgeometry.materials.push_back(IFS::Material(matname.str(), texname.str()));
                    quadgeometry.facematerial.push_back(quadgeometry.materials.size() - 1);
                    assert(quadgeometry.faces.size() == quadgeometry.facetexc.size() && quadgeometry.faces.size() == quadgeometry.facematerial.size());
                }

                // create output triangles for this quad
                if (exportOBJ)
                {
                    outgeometry.materials.push_back(IFS::Material(matname.str(), texname.str()));
                    for (size_t i : q.tri_id)
                    {
                        myIFS::IFSINDICES face;
                        myIFS::IFSINDICES facetc;
                        const Triangle &T = triangles[i];

                        // add triangle vertices
                        face.push_back(outgeometry.vertex2index(T.p));
                        facetc.push_back(outgeometry.texcoordinates.size());
                        outgeometry.texcoordinates.push_back(q.point2tex(T.p));
                        face.push_back(outgeometry.vertex2index(T.q));
                        facetc.push_back(outgeometry.texcoordinates.size());
                        outgeometry.texcoordinates.push_back(q.point2tex(T.q));
                        face.push_back(outgeometry.vertex2index(T.r));
                        facetc.push_back(outgeometry.texcoordinates.size());
                        outgeometry.texcoordinates.push_back(q.point2tex(T.r));

                        // add triangle
                        outgeometry.faces.push_back(face);
                        outgeometry.facetexc.push_back(facetc);
                        outgeometry.facematerial.push_back(outgeometry.materials.size() - 1);
                        assert(outgeometry.faces.size() == outgeometry.facetexc.size() && outgeometry.faces.size() == outgeometry.facematerial.size());
                    }
                }

                ++qid;
            }

            if (exportQuadGeometry)
            {
                IFS::exportOBJ(quadgeometry, "quadgeometry", "# OrthoGen textured quads\n");
            }
        }

        if (exportOBJ)
        {
            IFS::exportOBJ(outgeometry, "outgeometry", "# OrthoGen textured model\n");
        }

        return 0;
    }
    else
    {
        if (!projection.isValid()) { std::cout << "Error: invalid panoramic image " << std::endl; }
        if (!ingeometry.isValid()) { std::cout << "Error: invalid geometry " << std::endl; }
    }

    std::cout << "--help for options." << std::endl;

}


#include <algorithm>
#include <atomic>
#include <chrono>
#include <iostream>
#include <sstream>
#include <thread>

#include <sophus/se3.hpp>

#include <tbb/concurrent_unordered_map.h>

#include <pangolin/display/image_view.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/image/image.h>
#include <pangolin/image/image_io.h>
#include <pangolin/image/typed_image.h>
#include <pangolin/pangolin.h>

#include <CLI/CLI.hpp>

#include <visnav/calibration.h>
#include <visnav/common_types.h>
#include <visnav/keypoints.h>
#include <visnav/map_utils.h>
#include <visnav/matching_utils.h>
#include <visnav/vo_utils.h>

#include <visnav/gui_helper.h>
#include <visnav/tracks.h>

#include <visnav/serialization.h>

#include <boost/filesystem.hpp>
#include <dirent.h>
#include <fstream>
#include <limits>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <stdio.h>
#include <sys/stat.h>
using namespace visnav;
using namespace std;
using namespace cv;
using namespace Eigen;

///////////////////////////////////////////////////////////////////////////////
/// Declarations
///////////////////////////////////////////////////////////////////////////////
void load_data(const std::string& path);
void draw_image_overlay(pangolin::View& v, size_t view_id);
void change_display_to_image(const FrameCamId& fcid);
void convert_to_gray(const string& dataset_path);
double compute_residual(const Sophus::SE3<double> T_c2_c1);
bool next_step();
void optimize();
void optimize_single(Sophus::SE3<double>& T_c2_c1);
void draw_scene();
// void store_timestamp();
void record_pose(Sophus::SE3<double> current_pose);
void initialize_world_cord_and_transformation();
double compute_var(Mat img1, Mat img2, Mat depth);
void window_optimize(Sophus::SE3<double>& T_w_c1, Sophus::SE3<double>& T_w_c2,
                     Sophus::SE3<double>& T_w_c3, Sophus::SE3<double>& T_w_c4);
void sized_window_optimize(int offset = 0);

///////////////////////////////////////////////////////////////////////////////
/// Constants
///////////////////////////////////////////////////////////////////////////////

constexpr int UI_WIDTH = 200;
constexpr int NUM_CAMS = 1;
const intrinsic intr = {525.0, 525.0, 319.5, 239.5, 5000};

///////////////////////////////////////////////////////////////////////////////
/// Variables
///////////////////////////////////////////////////////////////////////////////

/// map with structure (frame id, (timestamp, depth, rgb))
std::map<int, vector<string>> timestamp_depth_rgb;

/// current frame id
size_t current_frame = 300;

/// number of frames in the optimization window
size_t window_size = 4;

/// transforamtion matrix frome current frame to next frame
Sophus::SE3<double> T_c2_c1(MatrixXd::Identity(3, 3), Vector3d(0, 0, 0));

/// initial pose
// Sophus::SE3<double> T_c1_w;

/// camera pose of the frame after current frame
Sophus::SE3<double> current_pose;

///
Matrix<double, 4, 1> world_cord;  //?

/// 3D points of the current frame
// std::vector<Matrix<double, 3, 1>> current_point;

/// bool flag recording wether last optimization converge
bool convergence = true;

/// the frame till which all optimizations are converged
Mat key_frame;

/// pose of the key frame
Sophus::SE3<double> key_pose(Matrix3d::Identity(), Vector3d(0, 0, 0));

/// id of key frame
size_t key_id = current_frame;

/// map storing the relative poses of each sector of trajectory
// map<size_t, map<size_t, Sophus::SE3<double>>> pose_map;

/// map storing the keyposes of each sector of trajectory
// map<size_t, Sophus::SE3<double>> keypose_map;

/// vector stores all relative poses regarding to keyframe
vector<Sophus::SE3<double>> pose(600);

/// all absolute poses
vector<Sophus::SE3<double>> abs_pose(600);

/// flag recording the last optimized frame, avoiding a frame is optimized
/// multiple times, eventhough the trans is large
size_t last_optimized_frame;

size_t last_failed_frame = 0;

std::string dataset_path = "../data/rgbd_dataset_freiburg1_desk";
string timestamp_txt = dataset_path + "/timestamp.txt";
string pose_txt = dataset_path + "/pose_" + ".txt";
ofstream pose_file;
vector<string> time_storage;
bool first_pose = true;

///////////////////////////////////////////////////////////////////////////////
/// GUI parameters
///////////////////////////////////////////////////////////////////////////////

pangolin::Var<bool> ui_show_hidden("ui.show_extra_options", false, true);

///////////////////////////////////////////////////////////////////////////////
/// GUI buttons
///////////////////////////////////////////////////////////////////////////////

// if you enable this, next_step is called repeatedly until completion
pangolin::Var<bool> continue_next("ui.continue_next", false, true);

using Button = pangolin::Var<std::function<void(void)>>;

Button next_step_btn("ui.next_step", &next_step);

//////////////////////////////////////////////
/// Image display options

pangolin::Var<int> show_frame1("ui.show_frame1", 0, 0, 1500);
pangolin::Var<int> show_cam1("ui.show_cam1", 0, 0, NUM_CAMS - 1);
pangolin::Var<int> show_frame2("ui.show_frame2", 0, 0, 1500);
pangolin::Var<int> show_cam2("ui.show_cam2", 1, 0, NUM_CAMS - 1);
pangolin::Var<bool> lock_frames("ui.lock_frames", true, true);

pangolin::Var<bool> show_cameras3d("hidden.show_cameras", true, true);
pangolin::Var<bool> show_points3d("hidden.show_points", true, true);
pangolin::Var<bool> show_old_points3d("hidden.show_old_points3d", true, true);
///////////////////////////////////////////////////////////////////////////////
/// Main
///////////////////////////////////////////////////////////////////////////////
bool dirExists(const std::string& path) {
  struct stat info;
  if (stat(path.c_str(), &info) == 0 && info.st_mode & S_IFDIR) {
    return true;
  }
  return false;
}

int main(int argc, char** argv) {
  bool show_gui = true;

  time_t t = time(0);
  char tmp[32] = {};
  strftime(tmp, sizeof(tmp), "%Y-%m-%d %H:%M:%S", localtime(&t));
  string pose_txt = dataset_path + "/pose_" + tmp + ".txt";
  pose_file.open(pose_txt);
  initialize_world_cord_and_transformation();

  CLI::App app{"RGB-D slam"};

  app.add_option("--show-gui", show_gui, "Show GUI");
  app.add_option("--dataset-path", dataset_path,
                 "Dataset path. Default: " + dataset_path);

  try {
    app.parse(argc, argv);
  } catch (const CLI::ParseError& e) {
    return app.exit(e);
  }

  // load gray images
  load_data(dataset_path);
  // store the timestamp
  // store_timestamp();

  if (show_gui) {
    cout << "in gui " << endl;
    pangolin::CreateWindowAndBind("Main", 1800, 1000);

    glEnable(GL_DEPTH_TEST);

    // main parent display for images and 3d viewer
    pangolin::View& main_view =
        pangolin::Display("main")
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0)
            .SetLayout(pangolin::LayoutEqualVertical);

    // parent display for images
    pangolin::View& img_view_display =
        pangolin::Display("images").SetLayout(pangolin::LayoutEqual);
    main_view.AddDisplay(img_view_display);

    // main ui panel
    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0,
                                          pangolin::Attach::Pix(UI_WIDTH));

    // extra options panel
    pangolin::View& hidden_panel = pangolin::CreatePanel("hidden").SetBounds(
        0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH),
        pangolin::Attach::Pix(2 * UI_WIDTH));
    ui_show_hidden.Meta().gui_changed = true;

    // 2D image views
    std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
    // here 2 for the image and depth
    while (img_view.size() < 2) {
      std::shared_ptr<pangolin::ImageView> iv(new pangolin::ImageView);

      size_t idx = img_view.size();
      img_view.push_back(iv);

      img_view_display.AddDisplay(*iv);
      iv->extern_draw_function =
          std::bind(&draw_image_overlay, std::placeholders::_1, idx);
    }

    // 3D visualization (initial camera view optimized to see full map)
    pangolin::OpenGlRenderState camera(
        pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000),
        pangolin::ModelViewLookAt(-3.4, -3.7, -8.3, 2.1, 0.6, 0.2,
                                  pangolin::AxisNegY));

    pangolin::View& display3D =
        pangolin::Display("scene")
            .SetAspect(-640 / 480.0)
            .SetHandler(new pangolin::Handler3D(camera));
    main_view.AddDisplay(display3D);
    //

    while (!pangolin::ShouldQuit()) {
      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

      if (ui_show_hidden.GuiChanged()) {
        hidden_panel.Show(ui_show_hidden);
        const int panel_width = ui_show_hidden ? 2 * UI_WIDTH : UI_WIDTH;
        main_view.SetBounds(0.0, 1.0, pangolin::Attach::Pix(panel_width), 1.0);
      }

      // display3D.Activate(camera);
      // glClearColor(0.95f, 0.95f, 0.95f, 1.0f);  // light gray background

      // draw_scene();

      img_view_display.Activate();
      if (lock_frames) {
        // in case of locking frames, chaning one should change the other
        if (show_frame1.GuiChanged()) {
          change_display_to_image(FrameCamId(show_frame1, 0));
          change_display_to_image(FrameCamId(show_frame1, 1));
        } else if (show_frame2.GuiChanged()) {
          change_display_to_image(FrameCamId(show_frame2, 0));
          change_display_to_image(FrameCamId(show_frame2, 1));
        }
      }
      if (show_frame1.GuiChanged() || show_cam1.GuiChanged()) {
        auto frame_id = static_cast<FrameId>(show_frame1);
        if (timestamp_depth_rgb.find(frame_id) != timestamp_depth_rgb.end()) {
          pangolin::TypedImage img =
              pangolin::LoadImage(timestamp_depth_rgb[frame_id][2]);

          img_view[0]->SetImage(img);
        } else {
          img_view[0]->Clear();
        }
      }
      if (show_frame2.GuiChanged() || show_cam2.GuiChanged()) {
        auto frame_id = static_cast<FrameId>(show_frame2);
        if (timestamp_depth_rgb.find(frame_id) != timestamp_depth_rgb.end()) {
          pangolin::GlPixFormat fmt;
          fmt.glformat = GL_LUMINANCE;
          fmt.gltype = GL_UNSIGNED_BYTE;
          fmt.scalable_internal_format = GL_LUMINANCE8;
          pangolin::TypedImage img =
              pangolin::LoadImage(timestamp_depth_rgb[frame_id][1]);

          img_view[1]->SetImage(img);
        } else {
          img_view[1]->Clear();
        }
      }
      pangolin::FinishFrame();

      if (continue_next) {
        // stop if there is nothing left to do
        continue_next = next_step();
      } else {
        // if the gui is just idling, make sure we don't burn too much CPU
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
      }
    }
  }
}

bool next_step() {
  // stop when the current frame is the second last frame
  if (current_frame >= timestamp_depth_rgb.size() - 1) {
    cout << "next step ended!" << endl;
    return false;
  }

  cout << "current frame: " << current_frame << endl;
  cout << "key id: " << key_id << endl;
  sized_window_optimize();
  // optimize();

  // reinitialize every x frames
  /*
  if (current_frame % 999 == 0 && current_frame != 0) {
    cout << "reinitializing" << endl;
    key_pose = pose[current_frame];
    Sophus::SE3<double> identity(Matrix3d::Identity(), Vector3d(0, 0, 0));
    for (size_t i = 0; i < window_size; i++) {
      pose[current_frame + i] = identity;
    }
    first_pose = true;

    sized_window_optimize();

    // cout << "relative pose:" << pose[current_frame].matrix3x4() << endl;
    // cout << pose[current_frame + 1].matrix3x4() << endl;
    // cout << "absolute pose;" << (pose[current_frame] *
  key_pose).matrix3x4()
    //      << endl;
    // cout << (pose[current_frame + 1] * key_pose).matrix3x4() << endl;
    // cout << "" << endl;
  }*/

  double max_trans = 0.04, max_rot = 0.04;

  Sophus::SE3<double> pose_current = pose[current_frame] * key_pose;
  Sophus::SE3<double> pose_last;
  if (!first_pose)
    pose_last = abs_pose[current_frame - 1];
  else
    pose_last = pose_current;

  Quaterniond q_current, q_last;
  q_current = pose_current.so3().matrix();
  q_last = pose_last.so3().matrix();
  double wx = abs(q_current.x() - q_last.x());
  double wy = abs(q_current.y() - q_last.y());
  double wz = abs(q_current.z() - q_last.z());

  double dx = abs((pose_current.matrix() - pose_last.matrix())(0, 3));
  double dy = abs((pose_current.matrix() - pose_last.matrix())(1, 3));
  double dz = abs((pose_current.matrix() - pose_last.matrix())(2, 3));

  if (wx > max_rot || wy > max_rot || wz > max_rot || dx > max_trans ||
      dy > max_trans || dz > max_trans) {
    cout << "last failed frame:" << last_failed_frame << endl;
    if (current_frame == last_failed_frame) {  // if frame still not qualified
                                               // after reinitialization
      cout << "give up optimization for frame " << current_frame << endl;
      pose[current_frame] = abs_pose[current_frame - 1] *
                            abs_pose[current_frame - 2].inverse() *
                            pose[current_frame - 1];

      abs_pose[current_frame] = pose[current_frame] * key_pose;
      record_pose(abs_pose[current_frame]);
      current_frame++;  // cf + 1
    } else {
      last_failed_frame = current_frame;
    }
    cout << "translation or rotation too large, reinitializing" << endl;
    cout << pose_current.translation().transpose() << endl;

    key_pose = abs_pose[current_frame - 1];  // cf or cf-1
    key_id = current_frame - 1;              // cf or cf-1
    cout << "new key pose taken at frame " << current_frame - 1 << endl;

    // re-initialize
    current_frame--;  // cf or cf-1
    Sophus::SE3<double> identity(Matrix3d::Identity(), Vector3d(0, 0, 0));
    for (size_t i = 0; i < window_size; i++) {
      pose[current_frame + i] = identity;
    }
    first_pose = true;  // make sure the first reinitialized pose is fixed
    sized_window_optimize();
    // optimize();
  } else {
    abs_pose[current_frame] = pose[current_frame] * key_pose;
    record_pose(abs_pose[current_frame]);  // T_c_w
    cout << "pose of " << current_frame << " recorded" << endl;
  }

  // update next incoming frame:
  Sophus::SE3<double> T_last_w = pose[current_frame + window_size - 1];
  Sophus::SE3<double> T_second_last_w = pose[current_frame + window_size - 2];
  pose[current_frame + window_size] =
      T_last_w * T_second_last_w.inverse() * T_last_w;
  // pose[current_frame + window_size] = T_last_w;

  /*
  optimize(T_c2_c1);
  cout << T_c2_c1.matrix() << endl;
  //current_pose = T_c2_c1 * current_pose;
  current_pose = T_c2_c1 * key_pose;
  record_pose(current_pose);
  */

  // record_pose(pose[current_frame]);

  current_frame++;
  first_pose = false;
  change_display_to_image(FrameCamId(current_frame, 0));
  cout << "" << endl;
  return true;
}

void sized_window_optimize(int offset) {
  if (window_size < 2) {
    cerr << "window size smaller than 2" << endl;
    abort();
  }

  ceres::Problem problem;
  std::vector<ceres::ResidualBlockId> residual_block_ids;

  Mat gray_current =
      imread(timestamp_depth_rgb.at(current_frame + offset)[2], 0);
  normalize(gray_current, gray_current, 1, 0, NORM_MINMAX, CV_32F);
  // gray_current.convertTo(gray_current, CV_32F);
  Sophus::SE3<double>& T_c1_w = pose[current_frame + offset];

  problem.AddParameterBlock(T_c1_w.data(), 7,
                            new Sophus::test::LocalParameterizationSE3);
  if (current_frame + offset == 0 || first_pose == true) {
    cout << "current frame is fixed" << endl;
    problem.SetParameterBlockConstant(T_c1_w.data());
  }
  Mat gray_window, depth;
  int height = gray_current.size().height;
  int width = gray_current.size().width;
  double gray_window_array[height * width];
  for (int j = 0; j < 1; j++) {
    for (size_t frame = 1; frame < window_size; frame++) {
      gray_window =
          imread(timestamp_depth_rgb.at(current_frame + offset + frame)[2], 0);
      depth = imread(
          timestamp_depth_rgb.at(current_frame + offset + frame - 1)[1], 2);
      normalize(gray_window, gray_window, 1, 0, NORM_MINMAX, CV_32F);
      // gray_window.convertTo(gray_window, CV_32F);
      Sophus::SE3<double>& T_c2_w = pose[current_frame + offset + frame];

      problem.AddParameterBlock(T_c2_w.data(), 7,
                                new Sophus::test::LocalParameterizationSE3);

      for (int i = 0; i < height; i++) {
        for (int j = 0; j < width; j++) {
          gray_window_array[i * width + j] = gray_window.at<float>(i, j);
        }
      }

      for (int u = 0; u < height; u++) {
        for (int v = 0; v < width; v++) {
          Matrix<double, 3, 1> p_c1;
          if (depth.at<uint16_t>(u, v) != 0) {
            p_c1(2) = (depth.at<uint16_t>(u, v) / intr.factor);
            p_c1(0) = (u - intr.cx) * p_c1(2) / intr.fx;
            p_c1(1) = (v - intr.cy) * p_c1(2) / intr.fy;

            // double var1_2 = compute_var(gray1, gray2, depth1, T_w_c1,
            // T_w_c2);
            Matrix<double, 3, 1> p_c1_2 = T_c2_w * T_c1_w.inverse() * p_c1;
            double u_2 = intr.fx * (p_c1_2[0] / p_c1_2[2]) + intr.cx;
            double v_2 = intr.fy * (p_c1_2[1] / p_c1_2[2]) + intr.cy;
            if (u_2 >= 0 && u_2 < height && v_2 >= 0 && v_2 < width) {
              // double residual =gray_window.at<float>(u_2, v_2) -
              // gray_current.at<float>(u, v); cout << residual << endl;

              ceres::CostFunction* cost_function =
                  new ceres::AutoDiffCostFunction<
                      newPhotoConsistencyCostFunctor, 1, 7, 7>(
                      new newPhotoConsistencyCostFunctor(
                          gray_current, intr, u, v, gray_window_array, p_c1));
              ceres::LossFunction* loss;
              loss = new ceres::HuberLoss(0.5);
              // loss = NULL;
              ceres::ResidualBlockId r_id = problem.AddResidualBlock(
                  cost_function, loss, T_c1_w.data(), T_c2_w.data());
              residual_block_ids.push_back(r_id);
            }
          }
        }
      }
    }
  }
  ceres::Problem::EvaluateOptions options;
  options.residual_blocks = residual_block_ids;
  double total_cost = 0.0;
  vector<double> evaluated_residuals;
  // vector<ceres::ResidualBlockId> removeId_vec;
  problem.Evaluate(options, &total_cost, &evaluated_residuals, nullptr,
                   nullptr);

  ceres::Problem::Options pro_options;
  pro_options.enable_fast_removal = true;

  for (size_t i = 0; i < evaluated_residuals.size(); i++) {
    if (evaluated_residuals[i] > 0.2) {
      // problem.RemoveResidualBlock(residual_block_ids[i]);
    }
  }

  cout << "solving" << endl;
  ceres::Solver::Options ceres_options;
  ceres_options.max_num_iterations = 50;
  ceres_options.max_solver_time_in_seconds = 60;
  ceres_options.minimizer_type = ceres::TRUST_REGION;
  ceres_options.linear_solver_type = ceres::SPARSE_SCHUR;
  ceres_options.num_threads = std::thread::hardware_concurrency();
  // ceres_options.update_state_every_iteration = true;
  ceres::Solver::Summary summary;
  ceres::Solve(ceres_options, &problem, &summary);
  cout << summary.BriefReport() << endl;
  // cout << summary.FullReport() << endl;
  cout << summary.total_time_in_seconds << endl;
}

double compute_var(Mat img1, Mat img2, Mat depth) {
  int height = img1.rows;
  int width = img1.cols;
  double residual_sum = 0;
  double var_old = 999;
  double var_new = 1;
  vector<double> residuals;

  // compute residual of each pixel
  for (int u = 0; u < height; u++) {
    for (int v = 0; v < width; v++) {
      Eigen::Matrix<double, 3, 1> p_c1;
      p_c1(2) = depth.at<uint16_t>(u, v) / intr.factor;
      p_c1(0) = (u + intr.cx) * p_c1(2) / intr.fx;
      p_c1(1) = (v + intr.cy) * p_c1(2) / intr.fy;

      Matrix<double, 3, 1> p_c2 = T_c2_c1 * p_c1;
      int u_next = int(intr.fx * p_c2(0) / p_c2(2) - intr.cx + 0.5);
      int v_next = int(intr.fy * p_c2(1) / p_c2(2) - intr.cy + 0.5);

      if (u_next >= 0 && u_next < height && v_next >= 0 && v_next < width) {
        double residual = img2.at<float>(u_next, v_next) - img1.at<float>(u, v);
        residuals.push_back(residual * residual);
      }
    }
  }
  while (abs(var_new - var_old) > 0.001 * var_old) {
    var_old = var_new;
    for (int i = 0; i < (int)residuals.size(); i++) {
      double r = residuals[i];  // square
      residual_sum += r * 6 / (5 + r / var_old);
    }
    var_new = residual_sum / residuals.size();
    // cout << "old:" << var_old << " new:" << var_new << endl;
  }

  return var_new;
}

void optimize() {
  Mat gray_current = imread(timestamp_depth_rgb.at(current_frame)[2],
                            0);  // read rgb as gray
  Mat gray_next = imread(timestamp_depth_rgb.at(current_frame + 1)[2], 0);
  Mat depth = imread(timestamp_depth_rgb.at(current_frame)[1], 2);

  normalize(gray_current, gray_current, 1, 0, NORM_MINMAX, CV_32F);
  normalize(gray_next, gray_next, 1, 0, NORM_MINMAX, CV_32F);
  // gray_current.convertTo(gray_current, CV_32F);
  // gray_next.convertTo(gray_next, CV_32F);

  int height = depth.size().height;
  int width = depth.size().width;

  Sophus::SE3<double>& T_c1_w = pose[current_frame];
  Sophus::SE3<double>& T_c2_w = pose[current_frame + 1];

  if (1) {
    key_pose = current_pose;
    key_id = current_frame;
  }
  // cout << "current frame id:" << current_frame << endl;
  //  cout << "compute between " << key_id << " and " << current_frame + 1
  //  << endl;

  double var = compute_var(gray_current, gray_next, depth);
  cout << "var:" << var << endl;

  ceres::Problem problem;
  problem.AddParameterBlock(T_c1_w.data(), 7,
                            new Sophus::test::LocalParameterizationSE3);
  problem.AddParameterBlock(T_c2_w.data(), 7,
                            new Sophus::test::LocalParameterizationSE3);

  // convert Mat to 1D array, maybe easier way?
  double gray_next_array[height * width];
  for (int i = 0; i < height; i++) {
    for (int j = 0; j < width; j++) {
      gray_next_array[i * width + j] = gray_next.at<float>(i, j);
    }
  }

  for (int u = 0; u < height; u++) {
    for (int v = 0; v < width; v++) {
      Matrix<double, 3, 1> p_c1;
      if (depth.at<uint16_t>(u, v) != 0) {
        p_c1(2) = (depth.at<uint16_t>(u, v) / intr.factor);
        p_c1(0) = (u + intr.cx) * p_c1(2) / intr.fx;
        p_c1(1) = (v + intr.cy) * p_c1(2) / intr.fy;
        Matrix<double, 3, 1> p_c2 = T_c2_w * T_c1_w.inverse() * p_c1;
        double u_next = intr.fx * (p_c2[0] / p_c2[2]) - intr.cx;
        double v_next = intr.fy * (p_c2(1) / p_c2(2)) - intr.cy;

        if (u_next >= 0 && u_next < height && v_next >= 0 && v_next < width) {
          ceres::CostFunction* cost_function =
              new ceres::AutoDiffCostFunction<PhotoConsistencyCostFunctor, 1, 7,
                                              7>(
                  new PhotoConsistencyCostFunctor(gray_current, intr, u, v,
                                                  gray_next_array, p_c1, var));
          ceres::LossFunction* loss;
          // loss = new ceres::HuberLoss(0.5);
          loss = NULL;
          problem.AddResidualBlock(cost_function, loss, T_c1_w.data(),
                                   T_c2_w.data());
        }
      }
    }
  }
  // solve
  ceres::Solver::Options ceres_options;
  ceres_options.max_num_iterations = 50;
  ceres_options.minimizer_type = ceres::TRUST_REGION;
  ceres_options.linear_solver_type = ceres::SPARSE_SCHUR;
  ceres_options.num_threads = std::thread::hardware_concurrency();
  // ceres_options.update_state_every_iteration = true;
  ceres::Solver::Summary summary;
  ceres::Solve(ceres_options, &problem, &summary);
  cout << summary.BriefReport() << endl;
  cout << summary.total_time_in_seconds << endl;
}

void window_optimize(Sophus::SE3<double>& T_c1_w, Sophus::SE3<double>& T_c2_w,
                     Sophus::SE3<double>& T_c3_w, Sophus::SE3<double>& T_c4_w) {
  Mat gray1 = imread(timestamp_depth_rgb.at(current_frame)[2],
                     0);  // read rgb as gray
  Mat gray2 = imread(timestamp_depth_rgb.at(current_frame + 1)[2], 0);
  Mat gray3 = imread(timestamp_depth_rgb.at(current_frame + 2)[2], 0);
  Mat gray4 = imread(timestamp_depth_rgb.at(current_frame + 3)[2], 0);

  Mat depth1 = imread(timestamp_depth_rgb.at(current_frame)[1], 2);
  Mat depth2 = imread(timestamp_depth_rgb.at(current_frame + 1)[1], 2);
  Mat depth3 = imread(timestamp_depth_rgb.at(current_frame + 2)[1], 2);

  // gray_current.convertTo(gray_current, CV_8UC1, 1.0 / 255, 0);  //
  // normalize gray_next.convertTo(gray_next, CV_8UC1, 1.0 / 255, 0);
  normalize(gray1, gray1, 1, 0, NORM_MINMAX, CV_32F);
  normalize(gray2, gray2, 1, 0, NORM_MINMAX, CV_32F);
  normalize(gray3, gray3, 1, 0, NORM_MINMAX, CV_32F);
  normalize(gray4, gray4, 1, 0, NORM_MINMAX, CV_32F);

  ceres::Problem problem;

  int height = depth1.size().height;
  int width = depth1.size().width;

  problem.AddParameterBlock(T_c1_w.data(), 7,
                            new Sophus::test::LocalParameterizationSE3);
  problem.AddParameterBlock(T_c2_w.data(), 7,
                            new Sophus::test::LocalParameterizationSE3);
  problem.AddParameterBlock(T_c3_w.data(), 7,
                            new Sophus::test::LocalParameterizationSE3);
  problem.AddParameterBlock(T_c4_w.data(), 7,
                            new Sophus::test::LocalParameterizationSE3);

  if (current_frame == 0) {
    cout << "current frame is 0" << endl;
    problem.SetParameterBlockConstant(T_c1_w.data());
  }

  // convert Mat to 1D array, maybe easier way?
  double gray_2_array[height * width];
  double gray_3_array[height * width];
  double gray_4_array[height * width];
  for (int i = 0; i < height; i++) {
    for (int j = 0; j < width; j++) {
      gray_2_array[i * width + j] = gray2.at<float>(i, j);
      gray_3_array[i * width + j] = gray3.at<float>(i, j);
      gray_4_array[i * width + j] = gray4.at<float>(i, j);
    }
  }

  // std::vector<ceres::ResidualBlockId> residual_block_ids;
  // current_point.clear();
  for (int u = 0; u < height; u++) {
    for (int v = 0; v < width; v++) {
      // Frame 1
      Matrix<double, 3, 1> p_c1;
      if (depth1.at<uint16_t>(u, v) != 0) {
        p_c1(2) = (depth1.at<uint16_t>(u, v) / intr.factor);
        p_c1(0) = (u + intr.cx) * p_c1(2) / intr.fx;
        p_c1(1) = (v + intr.cy) * p_c1(2) / intr.fy;

        // Frame 1 and Frame 2
        // double var1_2 = compute_var(gray1, gray2, depth1, T_w_c1,
        // T_w_c2);
        Matrix<double, 3, 1> p_c1_2 = T_c2_w * T_c1_w.inverse() * p_c1;
        double u_2 = intr.fx * (p_c1_2[0] / p_c1_2[2]) - intr.cx;
        double v_2 = intr.fy * (p_c1_2[1] / p_c1_2[2]) - intr.cy;
        if (u_2 >= 0 && u_2 < height && v_2 >= 0 && v_2 < width) {
          // current_point.push_back(p_c2);
          ceres::CostFunction* cost_function =
              new ceres::AutoDiffCostFunction<newPhotoConsistencyCostFunctor, 1,
                                              7, 7>(
                  // new PhotoConsistencyCostFunctor(gray1, intr, u, v,
                  // gray_2_array, p_c1, var1_2));
                  new newPhotoConsistencyCostFunctor(gray1, intr, u, v,
                                                     gray_2_array, p_c1));
          ceres::LossFunction* loss;
          loss = new ceres::HuberLoss(0.5);
          problem.AddResidualBlock(cost_function, loss, T_c1_w.data(),
                                   T_c2_w.data());
        }

        // Frame 1 and Frame 3
        // double var1_3 = compute_var(gray1, gray3, depth1, T_w_c1,
        // T_w_c3);
        Matrix<double, 3, 1> p_c1_3 = T_c3_w * T_c1_w.inverse() * p_c1;
        double u_3 = intr.fx * (p_c1_3[0] / p_c1_3[2]) - intr.cx;
        double v_3 = intr.fy * (p_c1_3[1] / p_c1_3[2]) - intr.cy;
        if (u_3 >= 0 && u_3 < height && v_3 >= 0 && v_3 < width) {
          // current_point.push_back(p_c3);
          ceres::CostFunction* cost_function =
              new ceres::AutoDiffCostFunction<newPhotoConsistencyCostFunctor, 1,
                                              7, 7>(
                  // new PhotoConsistencyCostFunctor(gray1, intr, u, v,
                  // gray_3_array, p_c1, var1_3));
                  new newPhotoConsistencyCostFunctor(gray1, intr, u, v,
                                                     gray_3_array, p_c1));
          ceres::LossFunction* loss;
          loss = new ceres::HuberLoss(0.5);
          problem.AddResidualBlock(cost_function, loss, T_c1_w.data(),
                                   T_c3_w.data());
        }
        // Frame 1 and Frame 4
        // double var1_4 = compute_var(gray1, gray4, depth1, T_w_c1,
        // T_w_c4);
        Matrix<double, 3, 1> p_c1_4 = T_c4_w * T_c1_w.inverse() * p_c1;
        double u_4 = intr.fx * (p_c1_4[0] / p_c1_4[2]) - intr.cx;
        double v_4 = intr.fy * (p_c1_4[1] / p_c1_4[2]) - intr.cy;
        if (u_4 >= 0 && u_4 < height && v_4 >= 0 && v_4 < width) {
          // current_point.push_back(p_c4);
          ceres::CostFunction* cost_function =
              new ceres::AutoDiffCostFunction<newPhotoConsistencyCostFunctor, 1,
                                              7, 7>(
                  // new PhotoConsistencyCostFunctor(gray1, intr, u, v,
                  // gray_4_array, p_c1, var1_4));
                  new newPhotoConsistencyCostFunctor(gray1, intr, u, v,
                                                     gray_4_array, p_c1));
          ceres::LossFunction* loss;
          loss = new ceres::HuberLoss(0.5);
          problem.AddResidualBlock(cost_function, loss, T_c1_w.data(),
                                   T_c4_w.data());
        }
      }

      // Frame2

      Matrix<double, 3, 1> p_c2;
      if (depth2.at<uint16_t>(u, v) != 0) {
        p_c2(2) = (depth2.at<uint16_t>(u, v) / intr.factor);
        p_c2(0) = (u + intr.cx) * p_c2(2) / intr.fx;
        p_c2(1) = (v + intr.cy) * p_c2(2) / intr.fy;
        // Frame 2 and Frame 3
        // double var2_3 = compute_var(gray2, gray3, depth2, T_w_c2,T_w_c3);
        Matrix<double, 3, 1> p_c2_3 = T_c3_w * T_c2_w.inverse() * p_c2;
        double u_3 = intr.fx * (p_c2_3[0] / p_c2_3[2]) - intr.cx;
        double v_3 = intr.fy * (p_c2_3[1] / p_c2_3[2]) - intr.cy;
        if (u_3 >= 0 && u_3 < height && v_3 >= 0 && v_3 < width) {
          // current_point.push_back(p_c2);
          ceres::CostFunction* cost_function =
              new ceres::AutoDiffCostFunction<newPhotoConsistencyCostFunctor, 1,
                                              7, 7>(
                  // new PhotoConsistencyCostFunctor(gray2, intr, u, v,
                  // gray_3_array, p_c2, var2_3));
                  new newPhotoConsistencyCostFunctor(gray2, intr, u, v,
                                                     gray_3_array, p_c2));
          ceres::LossFunction* loss;
          loss = new ceres::HuberLoss(0.5);
          problem.AddResidualBlock(cost_function, loss, T_c2_w.data(),
                                   T_c3_w.data());
        }

        // Frame 2 and Frame 4
        // double var2_4 = compute_var(gray2, gray4, depth2, T_w_c2,T_w_c4);
        Matrix<double, 3, 1> p_c2_4 = T_c4_w * T_c2_w.inverse() * p_c2;
        double u_4 = intr.fx * (p_c2_4[0] / p_c2_4[2]) - intr.cx;
        double v_4 = intr.fy * (p_c2_4[1] / p_c2_4[2]) - intr.cy;
        if (u_4 >= 0 && u_4 < height && v_4 >= 0 && v_4 < width) {
          // current_point.push_back(p_c2);
          ceres::CostFunction* cost_function =
              new ceres::AutoDiffCostFunction<newPhotoConsistencyCostFunctor, 1,
                                              7, 7>(
                  // new PhotoConsistencyCostFunctor(gray2, intr, u, v,
                  // gray_4_array, p_c2, var2_4));
                  new newPhotoConsistencyCostFunctor(gray2, intr, u, v,
                                                     gray_4_array, p_c2));
          ceres::LossFunction* loss;
          loss = new ceres::HuberLoss(0.5);
          problem.AddResidualBlock(cost_function, loss, T_c2_w.data(),
                                   T_c4_w.data());
        }
      }

      // Frame3
      Matrix<double, 3, 1> p_c3;
      if (depth3.at<uint16_t>(u, v) != 0) {
        p_c3(2) = (depth3.at<uint16_t>(u, v) / intr.factor);
        p_c3(0) = (u + intr.cx) * p_c3(2) / intr.fx;
        p_c3(1) = (v + intr.cy) * p_c3(2) / intr.fy;
        // Frame 3 and Frame 4
        // double var3_4 = compute_var(gray3, gray4, depth3, T_w_c3,T_w_c4);
        Matrix<double, 3, 1> p_c3_4 = T_c4_w * T_c3_w.inverse() * p_c3;
        double u_4 = intr.fx * (p_c3_4[0] / p_c3_4[2]) - intr.cx;
        double v_4 = intr.fy * (p_c3_4[1] / p_c3_4[2]) - intr.cy;
        if (u_4 >= 0 && u_4 < height && v_4 >= 0 && v_4 < width) {
          // current_point.push_back(p_c2);
          ceres::CostFunction* cost_function =
              new ceres::AutoDiffCostFunction<newPhotoConsistencyCostFunctor, 1,
                                              7, 7>(
                  // new PhotoConsistencyCostFunctor(gray3, intr, u,
                  // v,gray_4_array, p_c3, var3_4));
                  new newPhotoConsistencyCostFunctor(gray3, intr, u, v,
                                                     gray_4_array, p_c3));
          ceres::LossFunction* loss;
          loss = new ceres::HuberLoss(0.5);
          problem.AddResidualBlock(cost_function, loss, T_c3_w.data(),
                                   T_c4_w.data());
        }
      }

      // end
    }
  }
  cout << "after adding resiual block" << endl;

  // solve
  cout << "sloving" << endl;
  ceres::Solver::Options ceres_options;
  ceres_options.max_num_iterations = 50;
  ceres_options.minimizer_type = ceres::TRUST_REGION;
  ceres_options.linear_solver_type = ceres::SPARSE_SCHUR;
  ceres_options.num_threads = std::thread::hardware_concurrency();
  ceres_options.max_solver_time_in_seconds = 60;
  // ceres_options.update_state_every_iteration = true;
  ceres::Solver::Summary summary;
  ceres::Solve(ceres_options, &problem, &summary);
  cout << summary.BriefReport() << endl;
  // cout << summary.FullReport() << endl;
  cout << summary.total_time_in_seconds << endl;
}

void load_data(const std::string& dataset_path) {
  const string match_txt = dataset_path + "/match.txt";
  ifstream matches(match_txt);
  FrameId id = 0;
  while (matches) {
    string line;
    getline(matches, line);
    if (line.size() < 20) continue;
    string timestamp = line.substr(0, line.find("depth") - 1);
    string depth_name = line.substr(line.find("depth"),
                                    line.find('g') - line.find("depth") + 1);
    string rgb_name = line.substr(line.find("rgb"), line.size());
    vector<string> match = {timestamp, dataset_path + '/' + depth_name,
                            dataset_path + '/' + rgb_name};
    timestamp_depth_rgb[id] = match;
    id++;
  }
  std::cerr << "Loaded " << id << " rgb-depth pairs" << std::endl;
  if (id == 0) abort();

  show_frame1.Meta().range[1] = timestamp_depth_rgb.size() / NUM_CAMS - 1;
  show_frame1.Meta().gui_changed = true;
  show_frame2.Meta().range[1] = timestamp_depth_rgb.size() / NUM_CAMS - 1;
  show_frame2.Meta().gui_changed = true;
}

// Update the image views to a given image id
void change_display_to_image(const FrameCamId& fcid) {
  if (fcid.cam_id == 0) {
    // left view
    show_cam1 = 0;
    show_frame1 = fcid.frame_id;
    show_cam1.Meta().gui_changed = true;
    show_frame1.Meta().gui_changed = true;
  } else {
    // right view
    show_cam2 = fcid.cam_id;
    show_frame2 = fcid.frame_id;
    show_cam2.Meta().gui_changed = true;
    show_frame2.Meta().gui_changed = true;
  }
}

void draw_image_overlay(pangolin::View& v, size_t view_id) {}

// Render the 3D viewer scene of cameras and points
/*
void draw_scene() {
  const FrameCamId fcid1(show_frame1, show_cam1);
  const FrameCamId fcid2(show_frame2, show_cam2);

  const u_int8_t color_camera_current[3]{255, 0, 0};         // red
  const u_int8_t color_camera_left[3]{0, 125, 0};            // dark green
  const u_int8_t color_camera_right[3]{0, 0, 125};           // dark blue
  const u_int8_t color_points[3]{0, 0, 0};                   // black
  const u_int8_t color_old_points[3]{170, 170, 170};         // gray
  const u_int8_t color_selected_left[3]{0, 250, 0};          // green
  const u_int8_t color_selected_right[3]{0, 0, 250};         // blue
  const u_int8_t color_selected_both[3]{0, 250, 250};        // teal
  const u_int8_t color_outlier_observation[3]{250, 0, 250};  // purple

  // cout << "in draw scene" << endl;
  //  render cameras
  if (show_cameras3d) {
    render_camera(current_pose.inverse().matrix(), 2.0f, color_camera_current,
                  0.1f);
  }

  // render points
  if (show_points3d && current_point.size() > 0) {
    glPointSize(3.0);
    glBegin(GL_POINTS);
    for (int i = 0; i < (int)current_point.size(); i++) {
      glColor3ubv(color_points);
      pangolin::glVertex(current_pose.inverse() * current_point[i]);
    }
    glEnd();
  }
}*/

void record_pose(Sophus::SE3<double> current_pose) {
  double tx = current_pose.matrix3x4()(0, 3);
  double ty = current_pose.matrix3x4()(1, 3);
  double tz = current_pose.matrix3x4()(2, 3);

  Quaterniond q;
  Matrix3d Rotation_M = current_pose.so3().matrix();
  q = Rotation_M;
  // cout << "q: " << q.x() << q.y() << q.z() << q.w() << endl;

  pose_file << timestamp_depth_rgb.at(current_frame)[0] << " " << tx << " "
            << ty << " " << tz << " " << q.x() << " " << q.y() << " " << q.z()
            << " " << q.w() << endl;
  cout << timestamp_depth_rgb.at(current_frame)[0] << " x:" << tx << " y:" << ty
       << endl;
}

void initialize_world_cord_and_transformation() {
  world_cord << 0, 0, 0, 1;
  // Matrix3d R;
  // R << 0.0798578, 0.6121341, -0.7867113, 0.9967406, -0.0399786,
  // 0.0700705, 0.0114409, -0.7897427, -0.6133315;

  Quaternion<double> q;
  q.x() = 0.886385;
  q.y() = 0.231053;
  q.z() = -0.089359;
  q.w() = -0.391090;

  Matrix3d R = q.normalized().toRotationMatrix();

  // Sophus::SE3<double> T_c1_w(R, Vector3d(1.314809, 0.847662, 1.519455));
  Sophus::SE3<double> T_c1_w(Matrix3d::Identity(), Vector3d(0, 0, 0));
  current_pose = T_c1_w;
  cout << T_c1_w.matrix3x4() << endl;

  // for (size_t i = 0; i < window_size; i++) pose.push_back(T_c1_w);
  // for (size_t i = 0; i < current_frame+window_size; i++)
  // pose.push_back(T_c1_w);
  for (size_t i = 0; i < window_size; i++) {
    pose[current_frame + i] = T_c1_w;
    // pose.at(current_frame + i) = T_c1_w;
  }
  // keypose_map[key_id] = T_c1_w;
}


#pragma once

#include <boost/units/quantity.hpp>

#include <boost/units/systems/si/acceleration.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/force.hpp>

namespace EvolutionaryWalker::Physics
{

using TimeQuantity = boost::units::quantity<boost::units::si::time, double>;
using LengthQuantity = boost::units::quantity<boost::units::si::length, double>;
using VelocityQuantity = boost::units::quantity<boost::units::si::velocity, double>;
using AccelerationQuantity = boost::units::quantity<boost::units::si::acceleration, double>;
using MassQuantity = boost::units::quantity<boost::units::si::mass, double>;
using ForceQuantity = boost::units::quantity<boost::units::si::force, double>;

} // namespace EvolutionaryWalker::Physics


/* +------------------------------------------------------------------------+
|                     Mobile Robot Programming Toolkit (MRPT)            |
|                          https://www.mrpt.org/                         |
|                                                                        |
| Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
| See: https://www.mrpt.org/Authors - All rights reserved.               |
| Released under BSD License. See: https://www.mrpt.org/License          |
+------------------------------------------------------------------------+ */

#include "math-precomp.h"  // Precompiled headers

#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/CVectorDynamic.h>
#include <Eigen/Dense>
#include "MatrixVectorBase_impl.h"

// Template instantiations:
#define DO_MATFIXED_INSTANTIATION_NM(T_, N_, M_) \
	template class mrpt::math::MatrixVectorBase< \
		T_, mrpt::math::CMatrixFixed<T_, N_, M_>>;

#define DO_MATFIXED_INSTANTIATION(T_)      \
	DO_MATFIXED_INSTANTIATION_NM(T_, 2, 2) \
	DO_MATFIXED_INSTANTIATION_NM(T_, 3, 3) \
	DO_MATFIXED_INSTANTIATION_NM(T_, 4, 4) \
	DO_MATFIXED_INSTANTIATION_NM(T_, 6, 6) \
	DO_MATFIXED_INSTANTIATION_NM(T_, 7, 7) \
	DO_MATFIXED_INSTANTIATION_NM(T_, 12, 12)

DO_MATFIXED_INSTANTIATION(float);
DO_MATFIXED_INSTANTIATION(double);


#ifndef GIL_YUV_IMAGE_HPP
#define GIL_YUV_IMAGE_HPP

#include <boost/gil/image.hpp>
#include <boost/gil/extension/toolbox/color_spaces/ycbcr.hpp>

#include <boost/gil/extension/yuv/yuv_planar_pixel_iterator.hpp>

namespace boost {
namespace gil {

template< int J
        , int a
        , int b
        >
struct Scaling_Factors
{
    BOOST_STATIC_ASSERT(( mpl::equal_to< mpl::int_< J >, mpl::int_< 4 > >::type::value ));

    BOOST_STATIC_ASSERT(( mpl::or_< mpl::equal_to< mpl::int_< a >, mpl::int_< 4 > >
                                  , mpl::or_< mpl::equal_to< mpl::int_< a >, mpl::int_< 2 > >
                                            , mpl::equal_to< mpl::int_< a >, mpl::int_< 1 > >
                                            >
                                  >::type::value
                       ));

    BOOST_STATIC_ASSERT(( mpl::or_< mpl::equal_to< mpl::int_< b >, mpl::int_< 4 > >
                                  , mpl::or_< mpl::equal_to< mpl::int_< b >, mpl::int_< 2 > >
                                            , mpl::or_< mpl::equal_to< mpl::int_< b >, mpl::int_< 1 > >
                                                      , mpl::equal_to< mpl::int_< b >, mpl::int_< 0 > >
                                                      >
                                            >
                                  >::type::value
                       ));

    BOOST_STATIC_CONSTANT( int, ss_X = ( mpl::divides< mpl::int_< J >
                                                     , mpl::int_< a >
                                                     >::type::value )
                         );

    BOOST_STATIC_CONSTANT( int, ss_Y = ( mpl::if_< mpl::equal_to< mpl::int_< b >, mpl::int_< 0 > >
                                                 , mpl::int_< 2 >
                                                 , typename mpl::if_< mpl::equal_to< mpl::int_< a >, mpl::int_< b > >
                                                                    , mpl::int_< 1 >
                                                                    , mpl::int_< 4 >
                                                                    >::type
                                                 >::type::value )
                         );
};

///////////////////////////
// get_channel_ptr
///////////////////////////
template< typename Pixel >
struct get_channel_ptr
{
    typedef typename channel_traits< typename channel_type< Pixel >::type >::pointer type;
};

///////////////////////////
// is_supported_yuv_type
///////////////////////////
template< typename Factors > struct is_supported_yuv_type : mpl::false_ {};
template<> struct is_supported_yuv_type< mpl::vector_c< int, 4, 4, 4 > > : mpl::true_ {};
template<> struct is_supported_yuv_type< mpl::vector_c< int, 4, 2, 2 > > : mpl::true_ {};
template<> struct is_supported_yuv_type< mpl::vector_c< int, 4, 2, 0 > > : mpl::true_ {};

///////////////////////////
// get_yuv_x_iterator
///////////////////////////
template< typename ChannelPtr
        , typename ColorSpace
        , typename Factors
        >
struct get_yuv_x_iterator {};

template< typename ChannelPtr
        , typename ColorSpace
        >
struct get_yuv_x_iterator< ChannelPtr
                         , ColorSpace
                         , mpl::vector_c< int, 4, 4, 4 >
                         >
{
    typedef planar_pixel_iterator< ChannelPtr
                                 , ColorSpace
                                 > type;
};

template< typename ChannelPtr
        , typename ColorSpace
        >
struct get_yuv_x_iterator< ChannelPtr
                         , ColorSpace
                         , mpl::vector_c< int, 4, 2, 0 >
                         >
{
    typedef yuv_planar_pixel_iterator< ChannelPtr
                                     , ColorSpace
                                     > type;
};

///////////////////////////
// get_yuv_view
///////////////////////////
template< typename Pixel
        , typename Factors
        , class Enable = void
        >
struct get_yuv_view {};

template< typename Pixel
        , typename Factors
        >
struct get_yuv_view< Pixel
                   , Factors
                   , typename enable_if< is_supported_yuv_type< Factors > >::type
                   >
{
    typedef typename get_channel_ptr< Pixel >::type channel_ptr_t;
    typedef typename color_space_type< Pixel >::type color_space_t;

    typedef typename get_yuv_x_iterator< channel_ptr_t
                                       , color_space_t
                                       , Factors
                                       >::type x_iterator_t;

    typedef typename type_from_x_iterator< x_iterator_t >::view_t type;
};

///////////////////////////
// is_supported_yuv_type
///////////////////////////
struct IMC1
{
    template< typename ChannelPtr
            , int SS_X
            , int SS_Y
            >
    static
    void set_planes( std::size_t width
                   , std::size_t height
                   , std::size_t channel_size
                   , ChannelPtr data
                   , ChannelPtr& y_plane
                   , ChannelPtr& v_plane
                   , ChannelPtr& u_plane
                   )
    {
        y_plane = data;
        v_plane = y_plane + width * height * channel_size;
        u_plane = v_plane + ( width  / SS_X )
                          * ( height / SS_Y )
                          * channel_size;
    }
};

struct IMC2
{
    template< typename ChannelPtr
            , int SS_X
            , int SS_Y
            >
    static
    void set_planes( std::size_t width
                   , std::size_t height
                   , std::size_t channel_size
                   , ChannelPtr data
                   , ChannelPtr& y_plane
                   , ChannelPtr& v_plane
                   , ChannelPtr& u_plane
                   )
    {
        y_plane = data;
        v_plane = y_plane + width * height * channel_size;
        u_plane = v_plane + ( width / SS_X )
                          * channel_size;
    }
};

//////////////////////////////

template< typename Pixel
        , typename Factors
        , typename Format
        >
inline
typename get_yuv_view< Pixel
                     , Factors
                     >::type
yuv_view( std::size_t width
        , std::size_t height
        , typename get_channel_ptr< Pixel >::type data
        , std::ptrdiff_t rowsize_in_bytes
        )
{
    typedef Scaling_Factors< mpl::at_c< Factors, 0 >::type::value
                           , mpl::at_c< Factors, 1 >::type::value
                           , mpl::at_c< Factors, 2 >::type::value
                           > scaling_factors_t;

    typedef typename get_yuv_view< Pixel, Factors >::type view_t;

    typedef typename get_channel_ptr< Pixel >::type channel_ptr_t;

    std::size_t channel_size = 1;

    channel_ptr_t y_base, v_base, u_base;

    Format::template set_planes< channel_ptr_t
                      , scaling_factors_t::ss_X
                      , scaling_factors_t::ss_Y
                      >( width
                       , height
                       , channel_size
                       , data
                       , y_base
                       , v_base
                       , u_base
                       );

    return view_t( width
                 , height
                 , typename view_t::locator( typename view_t::x_iterator( y_base
                                                                        , v_base
                                                                        , u_base
                                                                        )
                                           , rowsize_in_bytes
                                           )
                 );
}

//
//
//
///// \ingroup ImageViewConstructors
///// \brief from raw YUV planar data
//template < typename ChannelPtr
//         , typename ColorSpace
//         >
//inline
//typename type_from_x_iterator< yuv_planar_pixel_iterator< ChannelPtr
//                                                        , ColorSpace
//                                                        >
//                             >::view_t
//planar_yuv_view( std::size_t width
//               , std::size_t height
//               , ChannelPtr y
//               , ChannelPtr v
//               , ChannelPtr u
//               , std::ptrdiff_t rowsize_in_bytes
//               )
//{
//    typedef typename type_from_x_iterator< yuv_planar_pixel_iterator< ChannelPtr
//                                                                    , ColorSpace
//                                                                    >
//                                         >::view_t RView;
//
//    return RView( width
//                , height
//                , typename RView::locator( yuv_planar_pixel_iterator< ChannelPtr
//                                                                    , ColorSpace
//                                                                    >( y, v, u )
//                                         , rowsize_in_bytes
//                                         )
//                );
//}
//
//
//template < typename ChannelPtr
//         , typename ColorSpace
//         >
//inline
//typename type_from_x_iterator< planar_pixel_iterator< ChannelPtr
//                                                    , ColorSpace
//                                                    >
//                             >::view_t
//planar_view( std::size_t width
//           , std::size_t height
//           , ChannelPtr y
//           , ChannelPtr v
//           , ChannelPtr u
//           , std::ptrdiff_t rowsize_in_bytes
//           )
//{
//    typedef typename type_from_x_iterator< planar_pixel_iterator< ChannelPtr
//                                                                , ColorSpace
//                                                                >
//                                         >::view_t RView;
//
//    return RView( width
//                , height
//                , typename RView::locator( planar_pixel_iterator< ChannelPtr
//                                                                , ColorSpace
//                                                                >( y, v, u )
//                                         , rowsize_in_bytes
//                                         )
//                );
//}

} // namespace gil
} // namespace boost

// taken from http://fourcc.org/yuv.php#IMC1

///
/// Y - y plane
/// V - v plane
/// U - u plane
/// P - Padding

///
/// YYYY
/// YYYY
/// YYYY
/// YYYY
/// VVPP
/// VVPP
/// UUPP
/// UUPP
///
///
struct IMC1 {};

///
/// YYYY
/// YYYY
/// YYYY
/// YYYY
/// VVUU
/// VVUU
///
struct IMC2 {};

/// - same as IMC1 but V and U planes are swapped
/// YYYY
/// YYYY
/// YYYY
/// YYYY
/// UUPP
/// UUPP
/// VVPP
/// VVPP
///
///
struct IMC3 {};

/// - same as IMC2 but V and U planes are swapped
/// YYYY
/// YYYY
/// YYYY
/// YYYY
/// UUVV
/// UUVV
///
struct IMC4 {};

#endif // GIL_YUV_IMAGE_HPP


/*
 * Author: Johannes M Dieterich
 */

#ifndef GRID_HPP
#define	GRID_HPP

#include <cmath>
#include <complex>
#include <armadillo>
#include <memory>
#include <stdio.h>
using namespace std;
using namespace arma;

class Grid {

public:

    virtual ~Grid(){};

    virtual cube* getRealGrid() = 0;

    virtual const cube* tryReadRealGrid() const = 0;

    virtual const cube* readRealGrid() = 0;

    virtual void complete(cube* realGrid) = 0;

    virtual double sumOver() = 0;

    virtual double integrate() = 0;

    virtual void minMax(double& min, double& max) = 0;

    virtual double stressNorm() const = 0;

    virtual size_t getGridPointsX() const = 0;

    virtual size_t getGridPointsY() const = 0;

    virtual size_t getGridPointsZ() const = 0;

    virtual unsigned long long getTotalGridPoints() const = 0;

    void convolve(Grid * otherGrid){}

    virtual void sqrtGrid() {

#ifdef LIBKEDF_DEBUG
        cout << "DEBUG: sqrtGrid called in grid base class." << endl;
#endif

        cube* data = this->getRealGrid();
        const size_t nSlices = data->n_slices;
        const size_t nRows = data->n_rows;
        const size_t nCols = data->n_cols;
        #pragma omp parallel for default(none) shared(data)
        for(size_t x = 0; x < nSlices; ++x){
            for(size_t col = 0; col < nCols; ++col){
                for(size_t row = 0; row < nRows; ++row){
                    data->at(row,col,x) = sqrt(data->at(row,col,x));
                }
            }
        }
        this->complete(data);

#ifdef LIBKEDF_DEBUG
        cout << "DEBUG: sqrtGrid finished in grid base class." << endl;
#endif

    }

    virtual void powGrid(const double exponent){

#ifdef LIBKEDF_DEBUG
        cout << "DEBUG: powGrid called in grid base class." << endl;
#endif

        cube* data = this->getRealGrid();
        const size_t nSlices = data->n_slices;
        const size_t nRows = data->n_rows;
        const size_t nCols = data->n_cols;
        #pragma omp parallel for default(none) shared(data)
        for(size_t x = 0; x < nSlices; ++x){
            for(size_t col = 0; col < nCols; ++col){
                for(size_t row = 0; row < nRows; ++row){
                    data->at(row,col,x) = pow(data->at(row,col,x), exponent);
                }
            }
        }
        this->complete(data);

#ifdef LIBKEDF_DEBUG
        cout << "DEBUG: powGrid finished in grid base class." << endl;
#endif
    }

    virtual void updateRealGrid(const double* rawData){

        cube* grid = this->getRealGrid();

        double* gridData = grid->memptr();

        const size_t nSlices = this->getGridPointsZ();
        const size_t nRows = this->getGridPointsY();
        const size_t nCols = this->getGridPointsX();
        const size_t totSize = nSlices*nRows*nCols*sizeof(double);

        memcpy(gridData, rawData, totSize);

        this->complete(grid);
    }

    virtual void getRealGridData(double* rawData){

        const cube* grid = this->readRealGrid();

        const double* gridData = grid->memptr();

        const size_t nSlices = this->getGridPointsZ();
        const size_t nRows = this->getGridPointsY();
        const size_t nCols = this->getGridPointsX();
        const size_t totSize = nSlices*nRows*nCols*sizeof(double);

        memcpy(rawData, gridData, totSize);
    }

    virtual void finalize(){
    }

    constexpr static double NUMERICALACCURACY = 1e-10;

private:
};

#endif	/* GRID_HPP */


// Copyright (c) 2021 fortiss GmbH
//
// Authors: Klemens Esterle and Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MODEL_INPUT_DATA_SOURCE_HEADER
#define MODEL_INPUT_DATA_SOURCE_HEADER

#include "miqp_planner_data.hpp"

#include <sstream>

#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

#include "bark/commons/util/util.hpp"

#include "ilopl/iloopl.h"

namespace miqp {
namespace planner {

// adapted from the customdatasource.cpp example
class ModelInputDataSource : public IloOplDataSourceBaseI {
 public:
  ModelInputDataSource(IloEnv& env, int precision)
      : IloOplDataSourceBaseI(env), precision_(precision){};

  ~ModelInputDataSource() {}

  void read() const;

  void resetParameters(std::shared_ptr<ModelParameters> parameters);

  void resetCplexSolverSettingsOnly(
      std::shared_ptr<ModelParameters> parameters);

  int getParallelMode() { return parameters_->parallelmode; }

 private:
  std::shared_ptr<ModelParameters> parameters_;
  int precision_;

  //! one function for each type as the cplex api calls different methods! (hard
  //! to model with templates)
  void addElement(IloOplDataHandler& handler, const char* name,
                  const int& item) const;
  void addElement(IloOplDataHandler& handler, const char* name,
                  const float& item) const;
  void addElement(IloOplDataHandler& handler, const char* name,
                  const Eigen::VectorXd& item) const;
  void addElement(IloOplDataHandler& handler, const char* name,
                  const Eigen::MatrixXd& item) const;
  void addElement(IloOplDataHandler& handler, const char* name,
                  const Eigen::VectorXi& item) const;
  void addElement(IloOplDataHandler& handler, const char* name,
                  const std::vector<int>& item) const;
  void addElement(IloOplDataHandler& handler, const char* name,
                  const Eigen::MatrixXi& item) const;
  void addElement(IloOplDataHandler& handler, const char* name,
                  const Eigen::Tensor<double, 3>& item) const;
  void addEnvironmentElement(
      IloOplDataHandler& handler, const char* name,
      const std::vector<Eigen::MatrixXd>& environments) const;
  void addObstacleElement(
      IloOplDataHandler& handler, const char* name,
      const std::vector<std::vector<Eigen::MatrixXd> >& obstacles) const;
  void addLineTuple(IloOplDataHandler& handler, const Eigen::MatrixXd& item,
                    const int& key, const int& idx1, const int& idx2) const;
  void addLineSet(IloOplDataHandler& handler,
                  const Eigen::MatrixXd& item) const;
  inline double RoundWithPrecision(const double val,
                                   const int precision) const {
    const double scale = pow(10, precision);
    const double r_val = round(val * scale) / scale;
    return r_val;
  }
};

}  // namespace planner
}  // namespace miqp

#endif  // MODEL_INPUT_DATA_SOURCE_HEADER


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_EPS_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_EPS_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
     @brief eps generic tag

     Represents the eps function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct eps_ : ext::elementwise_<eps_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<eps_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_eps_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::eps_, Site> dispatching_eps_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::eps_, Site>();
   }
   template<class... Args>
   struct impl_eps_;
  }
  /*!
    Except for numbers whose absolute value is smaller than Smallestpositivevalue,
    @c eps(x) returns 2^(exponent(x))*Eps

    @par Semantic:

    @code
    T r = eps(a0);
    @endcode

    is similar to:

    @code

    if T is floating
      r = 2^(exponent(x))*Eps<T>
    else if T is integral
      r = 1;
    @endcode

    @param a0

    @return a value of same type as the input
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::eps_, eps, 1)
} }

#endif


// Copyright 2019 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/histogram/axis/regular.hpp>

using namespace boost::histogram;

int main() {
  // circular regular axis cannot be growing
  (void)axis::regular<double, boost::use_default, boost::use_default,
                      decltype(axis::option::circular | axis::option::growth)>(1, 2, 3);
}