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

#include "bempp/common/config_trilinos.hpp"

#ifdef WITH_TRILINOS

#include "../check_arrays_are_close.hpp"
#include "../type_template.hpp"
#include "../random_arrays.hpp"

#include "create_regular_grid.hpp"

#include "assembly/assembly_options.hpp"
#include "assembly/discrete_boundary_operator.hpp"
#include "assembly/discrete_boundary_operator_composition.hpp"
#include "assembly/discrete_sparse_boundary_operator.hpp"
#include "assembly/boundary_operator.hpp"
#include "assembly/context.hpp"
#include "assembly/identity_operator.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"
#include "assembly/sparse_cholesky.hpp"

#include "bempp/common/config_ahmed.hpp"

#include "grid/grid.hpp"

#include "space/piecewise_constant_scalar_space.hpp"
#include "space/piecewise_linear_discontinuous_scalar_space.hpp"
#include "space/piecewise_linear_continuous_scalar_space.hpp"

#include "common/armadillo_fwd.hpp"
#include "common/boost_make_shared_fwd.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/version.hpp>
#include <complex>

using namespace Bempp;

// Tests

BOOST_AUTO_TEST_SUITE(SparseCholesky)

BOOST_AUTO_TEST_CASE(sparse_cholesky_works_for_piecewise_linears)
{
    typedef double BFT;
    typedef double RT;

    int nElementsX = 1, nElementsY = 2;
    shared_ptr<Grid> grid = createRegularTriangularGrid(nElementsX, nElementsY);

    shared_ptr<Space<BFT> > space(
        new PiecewiseLinearDiscontinuousScalarSpace<BFT>(grid));

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op = identityOperator<BFT, RT>(
        context, space, space, space);
    shared_ptr<const DiscreteBoundaryOperator<RT> > dop = op.weakForm();
    typedef DiscreteSparseBoundaryOperator<RT> SparseOp;
    shared_ptr<const SparseOp> sdop = SparseOp::castToSparse(dop);
    shared_ptr<const Epetra_CrsMatrix> mat = sdop->epetraMatrix();
    shared_ptr<Epetra_CrsMatrix> L = sparseCholesky(*mat);

    shared_ptr<SparseOp> opL = boost::make_shared<SparseOp>(L, NO_SYMMETRY);
    shared_ptr<SparseOp> opLT = boost::make_shared<SparseOp>(L, NO_SYMMETRY,
                                                             TRANSPOSE);
    typedef DiscreteBoundaryOperatorComposition<RT> OpComposition;
    shared_ptr<DiscreteBoundaryOperatorComposition<RT> > opLLT =
            boost::make_shared<OpComposition>(opL, opLT);

    arma::Mat<double> A = dop->asMatrix();
    arma::Mat<double> LLT = opLLT->asMatrix();

//    arma::Mat<double> matL = opL->asMatrix();
//    std::cout << "A:\n" << A;
//    std::cout << "L:\n" << matL;
//    std::cout << "arma chol:\n" << arma::chol(A).t();
//    std::cout << "L*L.t():\n" << (matL * matL.t());
//    std::cout << "LLT:\n" << LLT;

    BOOST_CHECK(check_arrays_are_close<double>(
                    A, LLT, 100. * std::numeric_limits<double>::epsilon()));
}

BOOST_AUTO_TEST_CASE(sparse_cholesky_works_for_piecewise_constants)
{
    typedef double BFT;
    typedef double RT;

    int nElementsX = 1, nElementsY = 2;
    shared_ptr<Grid> grid = createRegularTriangularGrid(nElementsX, nElementsY);

    shared_ptr<Space<BFT> > space(
        new PiecewiseConstantScalarSpace<BFT>(grid));

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op = identityOperator<BFT, RT>(
        context, space, space, space);
    shared_ptr<const DiscreteBoundaryOperator<RT> > dop = op.weakForm();
    typedef DiscreteSparseBoundaryOperator<RT> SparseOp;
    shared_ptr<const SparseOp> sdop = SparseOp::castToSparse(dop);
    shared_ptr<const Epetra_CrsMatrix> mat = sdop->epetraMatrix();
    shared_ptr<Epetra_CrsMatrix> L = sparseCholesky(*mat);

    shared_ptr<SparseOp> opL = boost::make_shared<SparseOp>(L, NO_SYMMETRY);
    shared_ptr<SparseOp> opLT = boost::make_shared<SparseOp>(L, NO_SYMMETRY,
                                                             TRANSPOSE);
    typedef DiscreteBoundaryOperatorComposition<RT> OpComposition;
    shared_ptr<DiscreteBoundaryOperatorComposition<RT> > opLLT =
            boost::make_shared<OpComposition>(opL, opLT);

    arma::Mat<double> A = dop->asMatrix();
    arma::Mat<double> LLT = opLLT->asMatrix();

    BOOST_CHECK(check_arrays_are_close<double>(
                    A, LLT, 100. * std::numeric_limits<double>::epsilon()));
}

BOOST_AUTO_TEST_CASE(sparse_cholesky_throws_for_continuous_functions)
{
    typedef double BFT;
    typedef double RT;

    int nElementsX = 1, nElementsY = 2;
    shared_ptr<Grid> grid = createRegularTriangularGrid(nElementsX, nElementsY);

    shared_ptr<Space<BFT> > space(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    BoundaryOperator<BFT, RT> op = identityOperator<BFT, RT>(
        context, space, space, space);
    shared_ptr<const DiscreteBoundaryOperator<RT> > dop = op.weakForm();
    typedef DiscreteSparseBoundaryOperator<RT> SparseOp;
    shared_ptr<const SparseOp> sdop = SparseOp::castToSparse(dop);
    shared_ptr<const Epetra_CrsMatrix> mat = sdop->epetraMatrix();
    BOOST_CHECK_THROW(sparseCholesky(*mat), std::invalid_argument   );
}

BOOST_AUTO_TEST_SUITE_END()

#endif // WITH_TRILINOS


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

  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/ifnotdec.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>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/two.hpp>

STF_CASE_TPL (" ifnotdec signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ifnotdec;

  // return type conformity test
  STF_EXPR_IS( ifnotdec(T(), T()), T);

  // specific values tests
  STF_EQUAL(ifnotdec(bs::Zero<T>(), bs::Mone<T>()), bs::Mtwo<T>());
  STF_EQUAL(ifnotdec(bs::Zero<T>(), bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(ifnotdec(bs::Zero<T>(), bs::Zero<T>()), bs::Mone<T>());
  STF_EQUAL(ifnotdec(bs::One<T>(), bs::Mone<T>()), bs::Mone<T>());
  STF_EQUAL(ifnotdec(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(ifnotdec(bs::One<T>(), bs::Zero<T>()), bs::Zero<T>());
}

STF_CASE_TPL (" ifnotdec unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ifnotdec;

  // return type conformity test
  STF_EXPR_IS( ifnotdec(T(), T()), T);

  // specific values tests
  STF_EQUAL(ifnotdec(bs::Zero<T>(), bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(ifnotdec(bs::Zero<T>(), bs::Two<T>()), bs::One<T>());
  STF_EQUAL(ifnotdec(bs::Zero<T>(), bs::Zero<T>()), bs::Valmax<T>());
  STF_EQUAL(ifnotdec(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(ifnotdec(bs::One<T>(), bs::Zero<T>()), bs::Zero<T>());}

STF_CASE_TPL(" ifnotdec floating", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ifnotdec;

  // return type conformity test
  STF_EXPR_IS( ifnotdec(T(), T()), T);

  // specific values tests
  STF_EQUAL(ifnotdec(bs::Zero<T>(), bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(ifnotdec(bs::Zero<T>(), bs::Two<T>()), bs::One<T>());
  STF_EQUAL(ifnotdec(bs::Zero<T>(), bs::Zero<T>()), bs::Mone<T>());
  STF_EQUAL(ifnotdec(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(ifnotdec(bs::One<T>(), bs::Zero<T>()), bs::Zero<T>());
}


/*
** BENSUPERPC PROJECT, 2020
** RPG
** Source: https://stackoverflow.com/questions/32410186/convert-bool-array-to-int32-unsigned-int-and-double
** binary.hpp
*/

#ifndef BINARY_HPP
#define BINARY_HPP

#include <bitset>
#include <boost/predef.h>
#include <climits>
#include <iostream>
#include <vector>

enum class endianness : bool
{
    big,
    little
};

#if (BOOST_COMP_GNUC && __cplusplus >= 201402L)
constexpr endianness getEndianness()
{
    uint32_t word = 1;
    uint8_t *byte = (uint8_t *)&word;

    if (byte[0])
        return endianness::little;
    else
        return endianness::big;
}

constexpr bool isLittleEndian()
{
    switch (getEndianness()) {
        case endianness::little:
            return true;
        case endianness::big:
            return false;
        default:
            return false;
    }
}
#else
endianness getEndianness();
endianness getEndianness()
{
    uint32_t word = 1;
    uint8_t *byte = (uint8_t *)&word;

    if (byte[0])
        return endianness::little;
    else
        return endianness::big;
}
bool isLittleEndian();
bool isLittleEndian()
{
    switch (getEndianness()) {
        case endianness::little:
            return true;
        case endianness::big:
            return false;
        default:
            return false;
    }
}
#endif
namespace my
{
namespace binary
{
unsigned long long make_bitSet(bool *[], size_t);
int bitArrayToInt32_big(const bool *[], int);
template <typename T> T bitArrayToInt_big(const bool *ar[], size_t ar_size);
template <typename T> T bitArrayToInt_big(const std::vector<bool> &, size_t);

} // namespace binary
} // namespace my
#include "binary_imp.hpp"

#endif


#include "FilterMirrorPlugin.h"

#include <vector>

#include <Eigen/Dense>
#include <StarlabException.h>

#include "MirrorTools.h"

FilterMirrorPlugin::FilterMirrorPlugin()
{

}

void FilterMirrorPlugin::applyFilter(RichParameterSet *richParameterSet)
{
    std::vector<Eigen::Vector3d> face(3, Eigen::Vector3d::Zero());
    SurfaceMeshModel *model = safeCast(document()->selectedModel());
    // 获得参数
    if(richParameterSet == nullptr
            || model == nullptr
            || !isA(model)){
        throw new StarlabException("Exception:");
        return;
    }

    face[0].x() = static_cast<double>(richParameterSet->getFloat("V0_x"));
    face[0].y() = static_cast<double>(richParameterSet->getFloat("V0_y"));
    face[0].z() = static_cast<double>(richParameterSet->getFloat("V0_z"));

    face[1].x() = static_cast<double>(richParameterSet->getFloat("V1_x"));
    face[1].y() = static_cast<double>(richParameterSet->getFloat("V1_y"));
    face[1].z() = static_cast<double>(richParameterSet->getFloat("V1_z"));

    face[2].x() = static_cast<double>(richParameterSet->getFloat("V2_x"));
    face[2].y() = static_cast<double>(richParameterSet->getFloat("V2_y"));
    face[2].z() = static_cast<double>(richParameterSet->getFloat("V2_z"));

    // 坐标系转换
    Eigen::Matrix4d transMat = model->getTransformationMatrix().inverse();
    Eigen::Vector4d tmpPoint;
    for(int i = 0; i < 3; i++) {
        tmpPoint << face[i], 1;
        tmpPoint = transMat * tmpPoint;
        face[i] = tmpPoint.segment(0, 3);
    }

    // 执行镜像操作
    mirror(safeCast(model), face);
}

/**
 * @brief FilterMirrorPlugin::initParameters
 * 初始化参数界面
 * @param richParameterSet
 */
void FilterMirrorPlugin::initParameters(RichParameterSet *richParameterSet)
{
    richParameterSet->addParam(new RichFloat("V0_x", 0.0, "X of Vertex[0]"));
    richParameterSet->addParam(new RichFloat("V0_y", 0.0, "Y of Vertex[0]"));
    richParameterSet->addParam(new RichFloat("V0_z", 0.0, "Z of Vertex[0]"));
    richParameterSet->addParam(new RichFloat("V1_x", 0.0, "X of Vertex[1]"));
    richParameterSet->addParam(new RichFloat("V1_y", 1.0, "Y of Vertex[1]"));
    richParameterSet->addParam(new RichFloat("V1_z", 0.0, "Z of Vertex[1]"));
    richParameterSet->addParam(new RichFloat("V2_x", 0.0, "X of Vertex[2]"));
    richParameterSet->addParam(new RichFloat("V2_y", 0.0, "Y of Vertex[2]"));
    richParameterSet->addParam(new RichFloat("V2_z", 1.0, "Z of Vertex[2]"));
}

bool FilterMirrorPlugin::isApplicable(Starlab::Model *model)
{
    return model != nullptr && isA(model);
}


//  Copyright Daniel James 2005-2006. Use, modification, and distribution are
//  subject to the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

//  Based on Peter Dimov's proposal
//  http://www.open-std.org/JTC1/SC22/WG21/docs/papers/2005/n1756.pdf
//  issue 6.18.

#if !defined(BOOST_FUNCTIONAL_DETAIL_HASH_FLOAT_HEADER)
#define BOOST_FUNCTIONAL_DETAIL_HASH_FLOAT_HEADER

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
# pragma once
#endif

#include <boost/functional/detail/float_functions.hpp>
#include <boost/limits.hpp>
#include <boost/assert.hpp>
#include <errno.h>

// Don't use fpclassify or _fpclass for stlport.
#if !defined(__SGI_STL_PORT) && !defined(_STLPORT_VERSION)
#  if defined(__GLIBCPP__) || defined(__GLIBCXX__)
// GNU libstdc++ 3
#    if (defined(__USE_ISOC99) || defined(_GLIBCXX_USE_C99_MATH)) && \
        !(defined(macintosh) || defined(__APPLE__) || defined(__APPLE_CC__))
#      define BOOST_HASH_USE_FPCLASSIFY
#    endif
#  elif (defined(_YVALS) && !defined(__IBMCPP__)) || defined(_CPPLIB_VER)
// Dinkumware Library, on Visual C++
#    if defined(BOOST_MSVC)
#      define BOOST_HASH_USE_FPCLASS
#    endif
#  endif
#endif

namespace boost
{
    namespace hash_detail
    {
        inline void hash_float_combine(std::size_t& seed, std::size_t value)
        {
            seed ^= value + (seed<<6) + (seed>>2);
        }

        template <class T>
        inline std::size_t float_hash_impl(T v)
        {
            int exp = 0;
            errno = 0;
            v = boost::hash_detail::call_frexp(v, &exp);
            if(errno) return 0;

            std::size_t seed = 0;

            std::size_t const length
                = (std::numeric_limits<T>::digits +
                        std::numeric_limits<int>::digits - 1)
                / std::numeric_limits<int>::digits;

            for(std::size_t i = 0; i < length; ++i)
            {
                v = boost::hash_detail::call_ldexp(v, std::numeric_limits<int>::digits);
                int const part = static_cast<int>(v);
                v -= part;
                hash_float_combine(seed, part);
            }

            hash_float_combine(seed, exp);

            return seed;
        }

        template <class T>
        inline std::size_t float_hash_value(T v)
        {
#if defined(BOOST_HASH_USE_FPCLASSIFY)
            using namespace std;
            switch (fpclassify(v)) {
            case FP_ZERO:
                return 0;
            case FP_INFINITE:
                return (std::size_t)(v > 0 ? -1 : -2);
            case FP_NAN:
                return (std::size_t)(-3);
            case FP_NORMAL:
            case FP_SUBNORMAL:
                return float_hash_impl(v);
            default:
                BOOST_ASSERT(0);
                return 0;
            }
#elif defined(BOOST_HASH_USE_FPCLASS)
            switch(_fpclass(v)) {
            case _FPCLASS_NZ:
            case _FPCLASS_PZ:
                return 0;
            case _FPCLASS_PINF:
                return (std::size_t)(-1);
            case _FPCLASS_NINF:
                return (std::size_t)(-2);
            case _FPCLASS_SNAN:
            case _FPCLASS_QNAN:
                return (std::size_t)(-3);
            case _FPCLASS_NN:
            case _FPCLASS_ND:
                return float_hash_impl(v);
            case _FPCLASS_PD:
            case _FPCLASS_PN:
                return float_hash_impl(v);
            default:
                BOOST_ASSERT(0);
                return 0;
            }
#else
            return float_hash_impl(v);
#endif
        }
    }
}

#endif


#pragma once

#include <algorithm>
#include <ros/ros.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

// ROS
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/MultiArrayDimension.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

// EIGEN
#include <Eigen/Dense>
#include <Eigen/Geometry>

// Submodules
#include <corridor.hpp>
#include <init_traj_planner.hpp>
#include <mission.hpp>
#include <param.hpp>

#include "com_fun.h"
#include "optimization.h"
#include "time.h"
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#include <eigen3/Eigen/Core>

using CppAD::AD;

int qi;

double cost;

// The solver takes all the state variables and actuator
// variables in a singular vector. Thus, we should to establish
// when one variable starts and another ends to make our lifes easier.
size_t x_start;
size_t y_start;
size_t theta_start;
size_t v_start;
size_t omega_start;

std::vector<ros::Publisher> way_point_pub;

std::vector < std::vector< std::array<int, 2> > > relative_pair;
std::vector < std::array<int, 3> > selected_pair_new;
std::vector < std::array<int, 3> > selected_pair_old;
std::vector < std::array<int, 3> > relative_3_pair;
std::vector < std::array<int, 2> > relative_3_2_pair;
std::vector < std::vector<int> > group;
int group_num;
int group_qn;

std::set<int> old_set;

class FG_eval {
     public:

      typedef CPPAD_TESTVECTOR(AD<double>) ADvector;

      void operator()(ADvector& fg, const ADvector& vars) {
        // MPC implementation
        // fg a vector of constraints, x is a vector of constraints.
        // NOTE: You'll probably go back and forth between this function and
        // the Solver function below.
      fg[0] = 0;

      for (int j=0;j<group_qn;j++){

        int nu=group[group_num][j];

        for (int i = 1; i < N; i++) {
          fg[0] += W_X * CppAD::pow(vars[x_start + j*N + i] - plan[nu][i][0], 2);
          fg[0] += W_Y * CppAD::pow(vars[y_start + j*N + i] - plan[nu][i][1], 2);
        }

        fg[0] += W_DV * CppAD::pow(vars[v_start+j*(N-1)], 2);
        fg[0] += W_DOMEGA * CppAD::pow(vars[omega_start+j*(N-1)], 2);

        // Minimize the value gap between sequential actuations.
        for (int i = 0; i < N - 2; i++) {
          fg[0] += W_DV * CppAD::pow(vars[v_start + j*(N-1) + i + 1] - vars[v_start + j*(N-1)+ i], 2);
          fg[0] += W_DOMEGA * CppAD::pow(vars[omega_start + j*(N-1) + i + 1] - vars[omega_start + j*(N-1) + i], 2);
        }

        // Initial constraints
        fg[1 + N * j + x_start] = vars[x_start + N * j];
        fg[1 + N * j + y_start] = vars[y_start + N * j];
        fg[1 + N * j + theta_start] = vars[theta_start + N * j];

        // The rest of the constraints
        for (int i = 0; i < N - 1; i++) {
          // The state at time t+1 .
          AD<double> x1 = vars[x_start + N * j + i + 1];
          AD<double> y1 = vars[y_start + N * j + i + 1];
          AD<double> theta1 = vars[theta_start + N * j + i + 1];

          // The state at time t.
          AD<double> x0 = vars[x_start + N * j + i];
          AD<double> y0 = vars[y_start + N * j + i];
          AD<double> theta0 = vars[theta_start + N * j + i];

          // Only consider the actuation at time t.
          AD<double> v0 = vars[v_start + (N-1) * j + i];
          AD<double> omega0 = vars[omega_start + (N-1) * j + i];

          fg[2 + x_start + N * j + i] = x1 - (x0 + v0 * CppAD::cos(theta0) * DT);
          fg[2 + y_start + N * j + i] = y1 - (y0 + v0 * CppAD::sin(theta0) * DT);
          fg[2 + theta_start + N * j + i] = theta1 - (theta0 + omega0 * DT);

        }
      }

      int cont=N*3*group_qn+1;
      int neww,old,tt;
      for (int i=0;i<selected_pair_new.size();i++)
        {
          tt=selected_pair_new[i].at(0);
          for (int k=0;k<group_qn;k++){
            if(group[group_num][k]==selected_pair_new[i].at(1))
              neww=k;
            else if (group[group_num][k]==selected_pair_new[i].at(2))
              old=k;
          }

          fg[cont]=CppAD::pow(vars[x_start+N*neww+tt]-vars[x_start+N*old+tt],2)
                  +CppAD::pow(vars[y_start+N*neww+tt]-vars[y_start+N*old+tt],2);
          cont++;
        }
      for (int i=0;i<selected_pair_old.size();i++)
        {
          tt=selected_pair_old[i].at(0);
          for (int k=0;k<group_qn;k++){
            if (group[group_num][k]==selected_pair_old[i].at(1)){
              neww=k;
              break;
            }
          }
          fg[cont]=CppAD::pow(vars[x_start+N*neww+tt]-plan[selected_pair_old[i].at(2)][tt][0],2)
                  +CppAD::pow(vars[y_start+N*neww+tt]-plan[selected_pair_old[i].at(2)][tt][1],2);
          cont++;
        }
    }
};

class MPCPlanner {
public:
    std_msgs::Float64MultiArray msgs_traj_info;
    std::vector<std_msgs::Float64MultiArray> msgs_traj_coef;

    MPCPlanner(std::shared_ptr<Corridor> _corridor_obj,
               std::shared_ptr<InitTrajPlanner> _initTrajPlanner_obj,
               SwarmPlanning::Mission _mission,
               SwarmPlanning::Param _param)
            : corridor_obj(std::move(_corridor_obj)),
              initTrajPlanner_obj(std::move(_initTrajPlanner_obj)),
              mission(std::move(_mission)),
              param(std::move(_param))

    {
        M = initTrajPlanner_obj.get()->T.size()-1; // the number of segments

        outdim = 3; // the number of outputs (x,y,z)

        T = initTrajPlanner_obj.get()->T;
        initTraj = initTrajPlanner_obj.get()->initTraj;
        SFC = corridor_obj.get()->SFC;

        int i,j,k;

        double dx;
        double dy;
        double dis;

        for (i=0;i<qn;i++){
            for(j=0;j<M;j++){

                dx=(initTraj[i][j+1].x()-initTraj[i][j].x())/5;
                dy=(initTraj[i][j+1].y()-initTraj[i][j].y())/5;
                dis= 25*(dx*dx+dy*dy);

                for (k=0;k<5;k++){
                    plan[i][5*j+k][0]=initTraj[i][j].x()+dx*k;
                    plan[i][5*j+k][1]=initTraj[i][j].y()+dy*k;

                    if (dis<0.1)
                    {
                      plan[i][5*j+k][3]=0;
                    }
                    if (dis<1.1){
                      plan[i][5*j+k][3]=MAXV*0.7;
                    }
                    else{
                      plan[i][5*j+k][3]=MAXV;
                    }
                }
            }
            plan[i][5*M][0]=initTraj[i][M].x();
            plan[i][5*M][1]=initTraj[i][M].y();
        }

        N = M*5+1;

        for (int qi=0; qi<qn; qi++)
        {
          for (int next=5;next<N;next+=5){
            if ((plan[qi][next][1]==plan[qi][0][1]) &&(plan[qi][next][0]==plan[qi][0][0]))
              continue;
            else{
              plan[qi][0][2]=atan2(plan[qi][next][1]-plan[qi][0][1],plan[qi][next][0]-plan[qi][0][0]);
              break;
            }
        }
      }

      double angle;

      for (i=0;i<qn;i++){
        for(j=0;j<M;j++){

          dx=initTraj[i][j+1].x()-initTraj[i][j].x();
          dy=initTraj[i][j+1].y()-initTraj[i][j].y();
          dis= dx*dx+dy*dy;

          if (j>0){
            if (dis>0.1){
              angle=atan2(dy,dx);
              if (angle-plan[i][5*(j-1)][2]>PI)
                angle=angle-2*PI;
              else if(plan[i][5*(j-1)][2]-angle>PI)
                angle=angle+2*PI;
            }
              else angle =plan[i][5*(j-1)][2];
          }
          else {angle=plan[i][0][2];}

          for (k=0;k<5;k++){
            plan[i][5*j+k][2]=angle;
          }

        }
      }

      for (i=0;i<qn;i++){
        for(j=0;j<M;j++){
          for (k=0;k<5;k++){
              plan[i][5*j+k][4]=(plan[i][5*(j+1)][2]-plan[i][5*j][2])*0.2/DT;
            }
          }
      }

      std::array<int, 2> sa;
      std::vector <std::array<int, 2>> sb;

      std::array<int, 3> sa3;
      std::vector <std::array<int, 3>> sb3;

      vector<std::array<int, 2>>::iterator it1,it2;

      int count3=0;

      double threshold=0;

      if (qn<=8)
        threshold=2;
      else
        threshold=1.2;

      for (k=0;k<N;k++){
        sb.clear();
        sb3.clear();
        for(i=0;i<qn;i++){
          for (j=i+1;j<qn;j++)
          {
            dis= pow(plan[i][k][0]-plan[j][k][0],2.0)+pow(plan[i][k][1]-plan[j][k][1],2.0);
            if (dis<threshold)
              sb.emplace_back(sa={i, j});
          }
        }
        for (int ii=0;ii<sb.size();ii++){
          for (int jj=sb[ii].at(1)+1;jj<qn;jj++){
            it1 = find(sb.begin()+ii, sb.end(), sa={sb[ii].at(0),jj});
            if (it1 != sb.end()){
              it2 = find(sb.begin()+ii, sb.end(), sa={sb[ii].at(1),jj});
              if (it2 !=sb.end()){
                //sb3.emplace_back(sa3={sb[ii].at(0),sb[ii].at(1),jj});
                relative_3_pair.emplace_back(sa3={sb[ii].at(0),sb[ii].at(1),jj});
                count3++;
              }
            }
          }
        }
        relative_pair.emplace_back(sb);
      }

      //std::cout<<"Num_conflict="<<count3<<std::endl;

    }

  bool Solve(int index) {
    bool ok = true;
    typedef CPPAD_TESTVECTOR(double) Dvector;

    // Set the number of model variables (includes both states and inputs).
    // For example: If the state is a 4 element vector, the actuators is a 2
    // element vector and there are 10 timesteps. The number of variables is:
    size_t n_vars = (N * 3 + (N - 1) * 2) * group_qn;
    // Set the number of constraints
    size_t n_constraints = N * 3 * group_qn;
    //size_t all_constraints = n_constraints + qn*(qn-1)*int(N/2)/2;
    size_t all_constraints = n_constraints +selected_pair_new.size()+selected_pair_old.size();

    // Initial value of the independent variables.
    // SHOULD BE 0 besides initial state.
    Dvector vars(n_vars);

    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);
    // Set lower and upper limits for variables.
    for (int i = 0; i < v_start; i++) {
      vars_lowerbound[i] = -BOUND;
      vars_upperbound[i] = BOUND;
    }

    for (int j=0;j<group_qn;j++){
      qi=group[group_num][j];
      int count=0;

      for (int i = 0; i < N; i++) {

        vars[x_start+j*N+i]=plan[qi][i][0];
        vars[y_start+j*N+i]=plan[qi][i][1];
        vars[theta_start+j*N+i]=plan[qi][i][2];

        if (i<N-1)
        {
          vars[v_start+j*(N-1)+i]=plan[qi][i][3];
          vars[omega_start+j*(N-1)+i]=plan[qi][i][4];
        }

        if ( i>1 && i==SFC[qi][count+1].second && count<SFC[qi].size()-1){
          count++;
        }

        if (plan[qi][i][0]-SFC[qi][count].first[0]>2)
          vars_lowerbound[x_start+j*N+i]=plan[qi][i][0]-2;
        else
          vars_lowerbound[x_start+j*N+i]=SFC[qi][count].first[0];
        if (SFC[qi][count].first[3]-plan[qi][i][0]>2)
          vars_upperbound[x_start+j*N+i]=plan[qi][i][0]+2;
        else
          vars_upperbound[x_start+j*N+i]=SFC[qi][count].first[3];
        if (plan[qi][i][1]-SFC[qi][count].first[1]>2)
          vars_lowerbound[y_start+j*N+i]=plan[qi][i][1]-2;
        else
          vars_lowerbound[y_start+j*N+i]=SFC[qi][count].first[1];
        if (SFC[qi][count].first[4]-plan[qi][i][1]>2)
          vars_upperbound[y_start+j*N+i]=plan[qi][i][1]+2;
        else
          vars_upperbound[y_start+j*N+i]=SFC[qi][count].first[4];
      }
    }

    for (int i = v_start; i < omega_start; i++) {
      vars_lowerbound[i] = 0;
      vars_upperbound[i] = MAXV;
      if (param.backward_enable)
        vars_lowerbound[i] = -MAXV;
    }

    for (int i = omega_start; i < n_vars; i++) {
      vars_lowerbound[i] = -MAXOMEGA;
      vars_upperbound[i] = MAXOMEGA;
    }

    // Lower and upper limits for the constraints
    // Should be 0 besides initial state.
    Dvector constraints_lowerbound(all_constraints);
    Dvector constraints_upperbound(all_constraints);
    for (int i = 0; i < n_constraints; i++) {
      constraints_lowerbound[i] = 0;
      constraints_upperbound[i] = 0;
    }
    //0.2 to 0.3
    double inter_collision=2*mission.quad_size[0]+0.2;
    inter_collision=inter_collision*inter_collision;
    for (int i= n_constraints;i<all_constraints;i++)
    {
      constraints_lowerbound[i] = inter_collision;
      constraints_upperbound[i] = BOUND;
    }

    for (int j = 0; j < group_qn; j ++){
      qi=group[group_num][j];
      double startangle;
      for (int next=5;next<N;next+=5){
        if ((plan[qi][next][1]==plan[qi][0][1]) &(plan[qi][next][0]==plan[qi][0][0]))
          continue;
        else{
          startangle=atan2(plan[qi][next][1]-plan[qi][0][1],plan[qi][next][0]-plan[qi][0][0]);
          break;
        }
      }

      constraints_lowerbound[x_start + N * j] = plan[qi][0][0];
      constraints_lowerbound[y_start + N * j] = plan[qi][0][1];
      constraints_upperbound[x_start + N * j] = plan[qi][0][0];
      constraints_upperbound[y_start + N * j] = plan[qi][0][1];

      if(param.initial_angle)
      {
        constraints_lowerbound[theta_start + N * j] = mission.startState[qi][2]*PI/2;
        constraints_upperbound[theta_start + N * j] = mission.startState[qi][2]*PI/2;
      }
      else
      {
        constraints_lowerbound[theta_start + N * j] = startangle;
        constraints_upperbound[theta_start + N * j] = startangle;
      }

    }

    // object that computes objective and constraints
    FG_eval fg_eval;
    // options for IPOPT solver
    std::string options;

    options += "Numeric tol          1e-5\n";
    options += "String linear_solver mumps\n";
    // Uncomment this if you'd like more print information
    options += "Integer print_level  0\n";
    // NOTE: Setting sparse to true allows the solver to take advantage
    // of sparse routines, this makes the computation MUCH FASTER. If you
    // can uncomment 1 of these and see if it makes a difference or not but
    // if you uncomment both the computation time should go up in orders of
    // magnitude.
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
    // Change this as you see fit.

    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval>(
        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
        constraints_upperbound, fg_eval, solution);

    // Check some of the solution values
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
    cost+=solution.obj_value;

    if(!ok){
      ROS_INFO_STREAM("Infeasible Solution: Group_"<<group_num);
      return false;
    }

    for (int j=0;j<group_qn;j++){
      qi=group[group_num][j];
      for (int i=0;i<N;i++){
          plan[qi][i][0]=solution.x[x_start+j*N+i];
          plan[qi][i][1]=solution.x[y_start+j*N+i];
          plan[qi][i][2]=solution.x[theta_start+j*N+i];
          plan[qi][i][3]=solution.x[v_start+j*(N-1)+i];
          plan[qi][i][4]=solution.x[omega_start+j*(N-1)+i];
      }
    }

    return true;
  }

    bool update(bool log){

        geometry_msgs::PoseStamped pp;

        if(param.random_group)
        {
          group.clear();
          for (int i=0;i<4;i++)
          {
            for(int j=i;j<qn;j+=4)
            {
              group.emplace_back(std::vector<int>{j});
              std::cout<<"Group_"<<group.size()<<": "<<j<<std::endl;
            }
          }
        }
        else
        {
          while(relative_3_pair.size()!=0 || relative_3_2_pair.size()!=0)
          {

            std::array<int,3> small_group;
            std::array<int,2> small_group2;
            std::array<int,3> temp;
            std::array<int,2> temp2;

            std::map<std::array<int,3>,int> m;
            std::map<std::array<int,2>,int> m2;

            for(int i=0; i<relative_3_pair.size();i++)
            {
              m[relative_3_pair[i]]++;
              if(m[relative_3_pair[i]]>m[small_group])
              {
                small_group=relative_3_pair[i];
              }
            }

            for(int i=0; i<relative_3_2_pair.size();i++)
            {
              m2[relative_3_2_pair[i]]++;
              if(m2[relative_3_2_pair[i]]>m2[small_group2])
              {
                small_group2=relative_3_2_pair[i];
              }
            }

            //3-case
            if(m[small_group]>=m2[small_group2])
            {
              if(param.log)
              std::cout<<"Group_"<<group.size()<<": "<<small_group.at(0)<<", "<<small_group.at(1)<<", "
              <<small_group.at(2)<<std::endl;

              std::vector<int> t={small_group.at(0),small_group.at(1),small_group.at(2)};

              group.emplace_back(t);

              std::vector < std::array<int, 3> >::iterator it;

              for(it=relative_3_pair.begin();it!=relative_3_pair.end();)
              {
                int erase=0;
                int num=0;
                temp=*it;
                for(int i=0;i<3;i++)
                {
                  for (int j=0;j<3;j++)
                  {
                    if(temp.at(i)==small_group.at(j))
                    {
                      erase++;
                      num=i;
                      j=3;
                    }
                  }
                }
                if (erase==1)
                {
                  if(num==0)
                    relative_3_2_pair.emplace_back(temp2={temp.at(1),temp.at(2)});
                  else if (num==1)
                    relative_3_2_pair.emplace_back(temp2={temp.at(0),temp.at(2)});
                  else
                    relative_3_2_pair.emplace_back(temp2={temp.at(0),temp.at(1)});
                }

                if (erase==0)
                  ++it;
                else
                  it=relative_3_pair.erase(it);
              }

              std::vector < std::array<int, 2> >::iterator iter;

              for(iter=relative_3_2_pair.begin();iter!=relative_3_2_pair.end();)
              {
                temp2=*iter;
                int erase=0;
                for(int i=0;i<2;i++)
                {
                  for (int j=0;j<3;j++)
                  {
                    if(temp2.at(i)==small_group.at(j))
                    {
                      erase=1;
                      i=2; j=3;
                    }
                  }
                }
                if (erase==0)
                  ++iter;
                else
                  iter=relative_3_2_pair.erase(iter);
              }
            }
            //2-case
            else
            {
              if (param.log)
              std::cout<<"Group_"<<group.size()<<": "<<small_group2.at(0)<<", "<<small_group2.at(1)<<std::endl;

              std::vector<int> t={small_group2.at(0),small_group2.at(1)};

              group.emplace_back(t);

              std::vector < std::array<int, 2> >::iterator iter;

              for(iter=relative_3_2_pair.begin();iter!=relative_3_2_pair.end();)
              {
                temp2=*iter;
                int erase=0;
                for(int i=0;i<2;i++)
                {
                  for (int j=0;j<2;j++)
                  {
                    if(temp2.at(i)==small_group2.at(j)){
                      erase=1;
                      i=2; j=2;
                    }
                  }
                }
                if (erase==0)
                  ++iter;
                else
                  iter=relative_3_2_pair.erase(iter);
              }

              std::vector < std::array<int, 3> >::iterator it;

              for(it=relative_3_pair.begin();it!=relative_3_pair.end();)
              {
                int erase=0;
                int num=0;
                temp=*it;
                for(int i=0;i<3;i++)
                {
                  for (int j=0;j<2;j++)
                  {
                    if(temp.at(i)==small_group2.at(j))
                    {
                      erase++;
                      num=i;
                      j=2;
                    }
                  }
                }

                if (erase==1)
                {
                  if(num==0)
                    relative_3_2_pair.emplace_back(temp2={temp.at(1),temp.at(2)});
                  else if (num==1)
                    relative_3_2_pair.emplace_back(temp2={temp.at(0),temp.at(2)});
                  else
                    relative_3_2_pair.emplace_back(temp2={temp.at(0),temp.at(1)});
                }

                if (erase==0)
                  ++it;
                else
                  it=relative_3_pair.erase(it);
              }
            }
          }

          for (int i=0;i<qn;i++)
          {
            bool ingroup=0;
            for (int j=0;j<group.size();j++)
            {
              for(int k=0;k<group[j].size();k++)
              {
                if(i==group[j][k])
                {
                  ingroup=1;
                  break;
                }
              }
              if (ingroup)
                break;
            }

            if(!ingroup)
            {
              group.emplace_back(std::vector<int>{i});
              if(param.log)
              std::cout<<"Group_"<<group.size()<<": "<<i<<std::endl;
            }
          }
        }

        for (int i=0;i<group.size();i++)
        {
            int temp_qn = group[i].size();
            x_start = 0;
            y_start = x_start + temp_qn*N;
            theta_start = y_start + temp_qn*N;
            v_start = theta_start + temp_qn*N;
            omega_start = v_start + temp_qn*(N - 1);

            group_num=i;
            group_qn=temp_qn;

            selected_pair_new.clear();
            selected_pair_old.clear();
            std::set<int> new_set;

            for(int j=0;j<group_qn;j++)
              new_set.insert(group[i][j]);

            int n1,n2;
            for (int j=0;j<relative_pair.size();j++){
              for (int k=0;k<relative_pair[j].size();k++){
                n1=relative_pair[j][k].at(0);
                n2=relative_pair[j][k].at(1);

                if (new_set.count(n1)==1 && old_set.count(n2)==1)
                {
                  selected_pair_old.emplace_back(std::array<int,3>{j,n1,n2});
                }
                else if (new_set.count(n2)==1 && old_set.count(n1)==1)
                {
                  selected_pair_old.emplace_back(std::array<int,3>{j,n2,n1});
                }
                else if (new_set.count(n1)==1 && new_set.count(n2)==1)
                {
                  selected_pair_new.emplace_back(std::array<int,3>{j,n1,n2});
                }
              }
            }

            bool group_ok =Solve(i);

            if ((!group_ok) && (!param.random_group))
            {
              std::vector <int> group_temp=group[i];
              group.erase(group.begin()+i);
              group.insert(group.begin(),group_temp);
              i=-1;
              old_set.clear();
              ROS_INFO_STREAM("Try again");
              continue;
            }

            for(int j=0;j<group_qn;j++)
              old_set.insert(group[i][j]);
        }
        ROS_INFO_STREAM("Optimization Success!");
        ROS_INFO_STREAM("Cost="<<cost);

        for (int qi=0;qi<qn;qi++)
        {
          nav_msgs::Path path;
          path.header.frame_id="map";
          path.header.stamp=ros::Time::now();
          for (int i=0;i<N;i++)
          {
              pp.pose.position.x=plan[qi][i][0];
              pp.pose.position.y=plan[qi][i][1];
              pp.pose.orientation=tf::createQuaternionMsgFromYaw(plan[qi][i][2]);
              path.poses.push_back(pp);

              if (fabs(plan[qi][i][0]-plan[qi][N-1][0])<0.01){
                  if(fabs(plan[qi][i][1]-plan[qi][N-1][1])<0.01){
                      break;
                  }
              }
          }
        way_point_pub[qi].publish(path);
        }

        return true;
    }

private:
    std::shared_ptr<Corridor> corridor_obj;
    std::shared_ptr<InitTrajPlanner> initTrajPlanner_obj;
    SwarmPlanning::Mission mission;
    SwarmPlanning::Param param;

    initTraj_t initTraj;
    std::vector<double> T;
    SFC_t SFC;

    int M, phi, outdim;

    std::vector<Eigen::MatrixXd> coef;

    void createMsg(){
        std::vector<double> traj_info;
        traj_info.emplace_back(N);
        traj_info.emplace_back(qn);
        traj_info.insert(traj_info.end(), T.begin(), T.end());
        msgs_traj_info.data = traj_info;

        msgs_traj_coef.resize(N);
        for(int qi = 0; qi < N; qi++) {
            std_msgs::MultiArrayDimension rows;
            rows.size = M * (qn + 1);
            msgs_traj_coef[qi].layout.dim.emplace_back(rows);

            std_msgs::MultiArrayDimension cols;
            cols.size = outdim;
            msgs_traj_coef[qi].layout.dim.emplace_back(cols);

            std::vector<double> coef_temp(coef[qi].data(), coef[qi].data() + coef[qi].size());
            msgs_traj_coef[qi].data.insert(msgs_traj_coef[qi].data.end(), coef_temp.begin(), coef_temp.end());
        }
    }
};


#include <iostream>
#include <iomanip>
#include <sstream>
#include <string>
#include <ctime>
#include <cmath>
#include <cstdlib>
#include <ccmc/Kameleon.h>
#include <ccmc/FileReader.h>
#include <boost/lexical_cast.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

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

	ccmc::Kameleon kameleon;
	std::string filename;
	std::string variable;
	int iterations = 10;
	float c0,c1,c2;
	if (argc != 6)
	{
		std::cout << "kameleon <filename> variable c0 c1 c2\n";
		std::cout << "\tAdapt3D, OpenGGCM, BATSRUS: x y z\n";
		std::cout << "\tENLIL, MAS: r theta(latitude) phi(longitude)" << std::endl;
		exit(1);
	}
	filename = argv[1];
	variable = argv[2];
	c0 = boost::lexical_cast<float>(argv[3]);
	c1 = boost::lexical_cast<float>(argv[4]);
	c2 = boost::lexical_cast<float>(argv[5]);
	//for (int o = 0; o < 5; o++)
	{
		long status = kameleon.open(filename);
		std::cout << "Opened file: " << filename << " with status: " << status << std::endl;
		std::cout << "FileReader::OK = " << ccmc::FileReader::OK << std::endl;
		if (status == ccmc::FileReader::OK)
		{

			bool successLoading = kameleon.loadVariable(variable);
			ccmc::Interpolator * interpolator = kameleon.createNewInterpolator();
			clock_t start, finish;

			std::cout << "starting interpolations" << std::endl;
			//start = clock();
			float value = 0.f;
			//for (int i = 0; i < 1; i++)
			{
				value = interpolator->interpolate(variable, c0, c1, c2);
				std::cout << "value: " << value << std::endl;
			}
			finish = clock();
			delete interpolator;
			kameleon.close();
		}
	}
	return 0;
}


#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <boost/format.hpp> // for formating strings
#include <sophus/se3.hpp>
#include <pangolin/pangolin.h>

using namespace std;
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// 在pangolin中画图，已写好，无需调整
void showPointCloud(
    const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud);

int main(int argc, char **argv)
{
    vector<cv::Mat> colorImgs, depthImgs;
    TrajectoryType poses;

    ifstream fin("./pose.txt");
    if (!fin)
    {
        cerr << "no pose data" << endl;
        return 1;
    }
    for (int i = 0; i < 5; i++)
    {
        boost::format fmt("./%s/%d.%s");
        colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
        depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1));

        double data[7] = {0};
        for (auto &d : data)
        {
            fin >> d;
        }
        for(auto &d : data)
        {
            cout<<d<<" ";
        }
        cout<<endl;

        Sophus::SE3d pose(Eigen::Quaterniond(data[6],data[3],data[4],data[5]),Eigen::Vector3d(data[0],data[1],data[2]));
        poses.push_back(pose);
    }

    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale=1000.0;
    vector<Vector6d,Eigen::aligned_allocator<Vector6d>> pointcloud;
    pointcloud.reserve(1000000);

    for(int i=0;i<5;i++)
    {
        cout<<"image processing "<<i+1<<endl;
        cv::Mat color=colorImgs[i];
        cv::Mat depth=depthImgs[i];
        Sophus::SE3d T=poses[i];
        for(int v=0;v<color.rows;v++)
        {
            for(int u=0;u<color.cols;u++)
            {
                unsigned int d=depth.ptr<unsigned short>(v)[u];//short 2byte
                if(d==0) continue;
                Eigen::Vector3d point;
                point[2]=double(d)/depthScale;
                point[0]=(u-cx)*point[2]/fx;
                point[1]=(v-cy)*point[2]/fy;
                Eigen::Vector3d pointWorld=T*point;//T^w_c*X_c
                Vector6d p;
                p.head<3>()=pointWorld;
                p[5]=color.data[v*color.step+u*color.channels()];//blue
                p[4]=color.data[v*color.step+u*color.channels()+1];//green
                p[3]=color.data[v*color.step+u*color.channels()+2];//red
                pointcloud.push_back(p);

            }
        }

    }
    cout<<"done"<<pointcloud.size()<<endl;
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud)
{

    if (pointcloud.empty())
    {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));

    pangolin::View &d_cam = pangolin::CreateDisplay()
                                .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
                                .SetHandler(new pangolin::Handler3D(s_cam));

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

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p : pointcloud)
        {
            glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000); // sleep 5 ms
    }
    return;
}


#include "VEE.h"

#include <boost/shared_ptr.hpp>

using namespace std;
using namespace boost;

ConstantValue doUSad8(vector<ExpressionPtr> args) {
    ConstantValue           v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    getConstArgs(args, constArgs);

    if( constArgs.size() == 2 ) {
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];
        //okay, this is CRAZY
        //get 8 values
        uint8_t a11, a12, a13, a14, a21, a22, a23, a24;

        a11 = a1.U32 & 0xff;
        a12 = (a1.U32 >> 8) & 0xff;
        a13 = (a1.U32 >> 16) & 0xff;
        a14 = (a1.U32 >> 24) & 0xff;

        a21 = a2.U32 & 0xff;
        a22 = (a2.U32 >> 8) & 0xff;
        a23 = (a2.U32 >> 16) & 0xff;
        a24 = (a2.U32 >> 24) & 0xff;

        //compute their differences
        int d1, d2, d3, d4;

        d1 = a11 - a21;
        d2 = a12 - a22;
        d3 = a13 - a23;
        d4 = a14 - a24;

        //take their absolute values
        d1 = abs(d1);
        d2 = abs(d2);
        d3 = abs(d3);
        d4 = abs(d4);

        //sum the values
        uint32_t    res = d1 + d2 + d3 + d4;

        //return the result
        v.valueIsKnown = true;
        v.width = 32;
        v.valueType = ConstantValue::T_I32;
        v.U32 = res;
    }

    return v;
}

ConstantValue doAdd(vector<ExpressionPtr>  args) {
    ConstantValue           v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    getConstArgs(args, constArgs);

    if( constArgs.size() == 2 ) {
        //add is a binop, and all args are known
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];

        assert(a1.width == a2.width);
        unsigned short opW = a1.width;
        switch(opW) {
            case 8:
                v.valueType = ConstantValue::T_I8;
                v.valueIsKnown = true;
                v.width = 8;
                v.U8 = a1.U8 + a2.U8;
                break;
            case 16:
                v.valueType = ConstantValue::T_I16;
                v.valueIsKnown = true;
                v.width = 16;
                v.U16 = a1.U16 + a2.U16;
                break;
            case 32:
                v.valueType = ConstantValue::T_I32;
                v.valueIsKnown = true;
                v.width = 32;
                v.U32 = a1.U32 + a2.U32;
                break;
            case 64:
                v.valueType = ConstantValue::T_I64;
                v.valueIsKnown = true;
                v.width = 64;
                v.U64 = a1.U64 + a2.U64;
                break;
        }
    }

    return v;
}

ConstantValue doSub(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        //sub is a binop
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];

        assert(a1.width == a2.width);
        unsigned short opW = a1.width;
        switch(opW) {
            case 8:
                v.valueType = ConstantValue::T_I8;
                v.valueIsKnown = true;
                v.width = 8;
                v.U8 = a1.U8 - a2.U8;
                break;
            case 16:
                v.valueType = ConstantValue::T_I16;
                v.valueIsKnown = true;
                v.width = 16;
                v.U16 = a1.U16 - a2.U16;
                break;
            case 32:
                v.valueType = ConstantValue::T_I32;
                v.valueIsKnown = true;
                v.width = 32;
                v.U32 = a1.U32 - a2.U32;
                break;
            case 64:
                v.valueType = ConstantValue::T_I64;
                v.valueIsKnown = true;
                v.width = 64;
                v.U64 = a1.U64 - a2.U64;
                break;
        }
    }

    return v;
}

ConstantValue doMul(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue   v1 = constArgs[0];
        ConstantValue   v2 = constArgs[1];

        assert(v1.width == v2.width);
        uint16_t    opW = v1.width;
        switch(opW) {
            case 8:
                {
                    uint8_t a1 = v1.U8;
                    uint8_t a2 = v2.U8;
                    v.width = 8;
                    v.valueType = ConstantValue::T_I8;
                    v.U8 = a1*a2;
                }
                break;

            case 16:
                {
                    uint16_t    a1 = v1.U16;
                    uint16_t    a2 = v2.U16;
                    v.width = 16;
                    v.valueType = ConstantValue::T_I16;
                    v.U16 = a1*a2;

                }
                break;

            case 32:
                {
                    uint32_t    a1 = v1.U32;
                    uint32_t    a2 = v2.U32;
                    v.width = 32;
                    v.valueType = ConstantValue::T_I32;
                    v.U32 = a1*a2;

                }
                break;

            case 64:
                {
                    uint64_t    a1 = v1.U64;
                    uint64_t    a2 = v2.U64;
                    v.width = 64;
                    v.valueType = ConstantValue::T_I64;
                    v.U64 = a1*a2;

                }
                break;
        }
    }

    return v;
}

ConstantValue doMulU(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        //mulU is a binop
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];

        assert(a1.width == a2.width);
        unsigned short opW = a1.width;
        switch(opW) {
            case 8:
                {
                    v.valueIsKnown = true;
                    v.width = 16;
                    v.valueType = ConstantValue::T_I16;
                    uint8_t i1 = a1.U8;
                    uint8_t i2 = a2.U8;
                    v.U16 = i1*i2;
                }
                break;
            case 16:
                {
                    v.valueIsKnown = true;
                    v.width = 32;
                    v.valueType = ConstantValue::T_I32;
                    uint16_t i1 = a1.U16;
                    uint16_t i2 = a2.U16;
                    v.U32 = i1*i2;
                }
                break;
            case 32:
                {
                    v.valueIsKnown = true;
                    v.width = 64;
                    v.valueType = ConstantValue::T_I64;
                    uint32_t i1 = a1.U32;
                    uint32_t i2 = a2.U32;
                    v.U64 = i1*i2;
                }
                break;
            case 64:
                //something went wrong, we can't do this
                throw StepErr("mulU of unusual size");
                break;
        }
    }

    return v;
}

ConstantValue doMulS(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        //mulS is a binop
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];

        assert(a1.width == a2.width);
        unsigned short opW = a1.width;
        switch(opW) {
            case 8:
                {
                    v.width = 16;
                    v.valueType = ConstantValue::T_I16;
                    v.valueIsKnown = true;
                    int8_t i1 = a1.U8;
                    int8_t i2 = a2.U8;
                    v.U16 = i1*i2;
                }
                break;
            case 16:
                {
                    v.width = 32;
                    v.valueType = ConstantValue::T_I32;
                    v.valueIsKnown = true;
                    int16_t i1 = a1.U16;
                    int16_t i2 = a2.U16;
                    v.U32 = i1*i2;
                }
                break;
            case 32:
                {
                    v.width = 64;
                    v.valueType = ConstantValue::T_I64;
                    v.valueIsKnown = true;
                    int32_t i1 = a1.U32;
                    int32_t i2 = a2.U32;
                    v.U64 = i1*i2;
                }
                break;
            case 64:
                //can't do this either
                throw StepErr("mulS of unusual size");
                break;
        }
    }

    return v;
}

ConstantValue doOr(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];

        assert(a1.width == a2.width);
        unsigned short opW = a1.width;
        switch(opW) {
            case 8:
                v.valueIsKnown = true;
                v.width = 8;
                v.valueType = ConstantValue::T_I8;
                v.U8 = a1.U8 | a2.U8;
                break;
            case 16:
                v.valueIsKnown = true;
                v.width = 16;
                v.valueType = ConstantValue::T_I16;
                v.U16 = a1.U16 | a2.U16;
                break;
            case 32:
                v.valueIsKnown = true;
                v.width = 32;
                v.valueType = ConstantValue::T_I32;
                v.U32 = a1.U32 | a2.U32;
                break;
            case 64:
                v.valueIsKnown = true;
                v.width = 64;
                v.valueType = ConstantValue::T_I64;
                v.U64 = a1.U64 | a2.U64;
                break;
        }
    }

    return v;
}

ConstantValue doAnd(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];

        assert(a1.width == a2.width);
        unsigned short opW = a1.width;
        switch(opW) {
            case 8:
                v.valueIsKnown = true;
                v.width = 8;
                v.valueType = ConstantValue::T_I8;
                v.U8 = a1.U8 & a2.U8;
                break;
            case 16:
                v.valueIsKnown = true;
                v.width = 16;
                v.valueType = ConstantValue::T_I16;
                v.U16 = a1.U16 & a2.U16;
                break;
            case 32:
                v.valueIsKnown = true;
                v.width = 32;
                v.valueType = ConstantValue::T_I32;
                v.U32 = a1.U32 & a2.U32;
                break;
            case 64:
                v.valueIsKnown = true;
                v.width = 64;
                v.valueType = ConstantValue::T_I64;
                v.U64 = a1.U64 & a2.U64;
                break;
        }
    }

    return v;
}

ConstantValue doXor(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];

        assert(a1.width == a2.width);
        unsigned short opW = a1.width;
        switch(opW) {
            case 8:
                v.valueIsKnown = true;
                v.width = 8;
                v.valueType = ConstantValue::T_I8;
                v.U8 = a1.U8 ^ a2.U8;
                break;
            case 16:
                v.valueIsKnown = true;
                v.width = 16;
                v.valueType = ConstantValue::T_I16;
                v.U16 = a1.U16 ^ a2.U16;
                break;
            case 32:
                v.valueIsKnown = true;
                v.width = 32;
                v.valueType = ConstantValue::T_I32;
                v.U32 = a1.U32 ^ a2.U32;
                break;
            case 64:
                v.valueIsKnown = true;
                v.width = 64;
                v.valueType = ConstantValue::T_I64;
                v.U64 = a1.U64 ^ a2.U64;
                break;
        }
    }

    return v;
}

ConstantValue doShl(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        //Shl is a binop
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];
        uint64_t        shiftWidth;

        v.width = getArgLen(args[0]);
        v.valueType = getITyFromWidth(v.width);

        switch(a2.width) {
            case 1:
                shiftWidth = a2.U1;
                break;
            case 8:
                shiftWidth = a2.U8;
                break;
            case 16:
                shiftWidth = a2.U16;
                break;
            case 32:
                shiftWidth = a2.U32;
                break;
            case 64:
                shiftWidth = a2.U64;
                break;
            default:
                throw StepErr("Invalid State");
        }

        switch(a1.width) {
            case 8:
                {
                    uint8_t i1 = a1.U8;
                    v.valueIsKnown = true;
                    v.valueType = ConstantValue::T_I8;
                    v.width = 8;
                    v.U8 = i1 << shiftWidth;
                }
                break;
            case 16:
                {
                    uint16_t i1 = a1.U16;
                    v.valueIsKnown = true;
                    v.U16 = i1 << shiftWidth;
                }
                break;
            case 32:
                {
                    uint32_t i1 = a1.U32;
                    v.valueIsKnown = true;
                    v.U32 = i1 << shiftWidth;
                }
                break;
            case 64:
                {
                    uint64_t i1 = a1.U64;
                    v.valueIsKnown = true;
                    v.U64 = i1 << shiftWidth;
                }
                break;
        }
    }

    return v;
}

ConstantValue doShr(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = getArgLen(args[0]);
    v.valueType = getITyFromWidth(v.width);

    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        //Shr is a binop
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];
        uint64_t        shiftWidth;

        switch(a2.width) {
            case 1:
                shiftWidth = a2.U1;
                break;
            case 8:
                shiftWidth = a2.U8;
                break;
            case 16:
                shiftWidth = a2.U16;
                break;
            case 32:
                shiftWidth = a2.U32;
                break;
            case 64:
                shiftWidth = a2.U64;
                break;
            default:
                throw StepErr("Invalid State");
        }

        switch(a1.width) {
            case 8:
                {
                    uint8_t i1 = a1.U8;
                    v.valueIsKnown = true;
                    v.valueType = ConstantValue::T_I8;
                    v.width = 8;
                    v.U8 = i1 >> shiftWidth;
                }
                break;
            case 16:
                {
                    uint16_t i1 = a1.U16;
                    v.valueIsKnown = true;
                    v.U16 = i1 >> shiftWidth;
                }
                break;
            case 32:
                {
                    uint32_t i1 = a1.U32;
                    v.valueIsKnown = true;
                    v.U32 = i1 >> shiftWidth;
                }
                break;
            case 64:
                {
                    uint64_t i1 = a1.U64;
                    v.valueIsKnown = true;
                    v.U64 = i1 >> shiftWidth;
                }
                break;
        }
    }

    return v;

}

ConstantValue doSar(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = getArgLen(args[0]);
    v.valueType = getITyFromWidth(v.width);

    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        //binop
        ConstantValue   v1 = constArgs[0];
        ConstantValue   v2 = constArgs[1];

        v.valueIsKnown = true;

        switch(v1.width) {
            case 8:
                v.U8 = v1.U8;
                for( uint64_t i = 0; i < v2.U8; i++ ) {
                    v.U8 *= 2;
                }
                break;
            case 16:
                v.U16 = v1.U16;
                for( uint64_t i = 0; i < v2.U16; i++ ) {
                    v.U16 *= 2;
                }
                break;
            case 32:
                v.U32 = v1.U32;
                for( uint64_t i = 0; i < v2.U32; i++ ) {
                    v.U32 *= 2;
                }
                break;
            case 64:
                v.U64 = v1.U64;
                for( uint64_t i = 0; i < v2.U64; i++ ) {
                    v.U64 *= 2;
                }
                break;
        }
    }

    return v;
}

ConstantValue doCmpEQ(vector<ExpressionPtr>   args) {
     ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.width = 1;
    v.valueType = ConstantValue::T_I1;
    v.valueIsKnown = false;
    getConstArgs(args, constArgs);

    if( constArgs.size() == 2 ) {
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];

        v.valueIsKnown = true;
        assert(a1.width == a2.width);
        unsigned short opW = a1.width;

        switch(opW) {
            case 1:
                if( a1.U1 == a2.U1 ) {
                    v.U1 = 1;
                } else {
                    v.U1 = 0;
                }
                break;
            case 8:
                if( a1.U8 == a2.U8 ) {
                    v.U1 = 1;
                } else {
                    v.U1 = 0;
                }
                break;
            case 16:
                if( a1.U16 == a2.U16 ) {
                    v.U1 = 1;
                } else {
                    v.U1 = 0;
                }
                break;
            case 32:
                if( a1.U32 == a2.U32 ) {
                    v.U1 = 1;
                } else {
                    v.U1 = 0;
                }
                break;
            case 64:
                if( a1.U64 == a2.U64 ) {
                    v.U1 = 1;
                } else {
                    v.U1 = 0;
                }
                break;
        }
    }

    return v;
}

ConstantValue doCmpNE(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.width = 1;
    v.valueType = ConstantValue::T_I1;
    v.valueIsKnown = false;
    getConstArgs(args, constArgs);

    if( constArgs.size() == 2 ) {
        ConstantValue   a1 = constArgs[0];
        ConstantValue   a2 = constArgs[1];

        v.valueIsKnown = true;

        assert(a1.width == a2.width);
        unsigned short opW = a1.width;

        switch(opW) {
            case 1:
                if( a1.U1 == a2.U1 ) {
                    v.U1 = 0;
                } else {
                    v.U1 = 1;
                }
                break;
            case 8:
                if( a1.U8 == a2.U8 ) {
                    v.U1 = 0;
                } else {
                    v.U1 = 1;
                }
                break;
            case 16:
                if( a1.U16 == a2.U16 ) {
                    v.U1 = 0;
                } else {
                    v.U1 = 1;
                }
                break;
            case 32:
                if( a1.U32 == a2.U32 ) {
                    v.U1 = 0;
                } else {
                    v.U1 = 1;
                }
                break;
            case 64:
                if( a1.U64 == a2.U64 ) {
                    v.U1 = 0;
                } else {
                    v.U1 = 1;
                }
                break;
        }
    }

    return v;
}

ConstantValue doNot(vector<ExpressionPtr>   args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    assert(args.size() > 0);
    v.width = getArgLen(args[0]);
    v.valueType = getITyFromWidth(v.width);

    getConstArgs(args, constArgs);
    if( constArgs.size() == 1 ) {
        //UNOP
        ConstantValue   v1 = constArgs[0];
        switch( v1.width ) {
            case 1:
                {
                    uint8_t r = v1.U1;
                    r = ~r;
                    r = r & 1;
                    v.U1 = r;
                }
                break;
            case 8:
                v.U8 = ~v1.U8;
                break;
            case 16:
                v.U16 = ~v1.U16;
                break;
            case 32:
                v.U32 = ~v1.U32;
                break;
            case 64:
                v.U64 = ~v1.U64;
                break;
        }
    }

    return v;
}

ConstantValue doC64To8(std::vector<ExpressionPtr>   args) {
    ConstantValue   v;

    v.valueIsKnown = false;

    throw StepErr("C64To8");

    return v;
}

ConstantValue doC32To8(std::vector<ExpressionPtr>   args) {
    vector<ConstantValue> constArgs;
    ConstantValue   v;

    v.valueIsKnown = false;
    v.width = 8;
    v.valueType = ConstantValue::T_I8;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 1 ) {
        ConstantValue v1 = constArgs[0];
        v.valueIsKnown = true;

        assert( v1.width == 32 );

        v.U8 = (v1.U32 & 0xFF);
    }

    return v;
}

ConstantValue doC64To16(std::vector<ExpressionPtr>  args) {
    ConstantValue   v;

    v.valueIsKnown = false;

    throw StepErr("C64To16");

    return v;
}

ConstantValue doC64LOto32(std::vector<ExpressionPtr>  args) {
    ConstantValue           v;
    vector<ConstantValue>   constVec;

    v.valueType = ConstantValue::T_I32;
    v.width = 32;
    v.valueIsKnown = false;

    getConstArgs(args, constVec);
    if( constVec.size() == 1 ) {
        //C64LOto32 is a unop
        ConstantValue   a1 = constVec[0];

        assert(a1.width == 64);

        //we have enough information to set v
        v.valueIsKnown = true;
        uint64_t k = a1.U64;
        uint64_t l = k & ~0xFFFFFFFF00000000;
        v.U32 = (uint32_t) l;
    }

    return v;
}
ConstantValue doC64HIto32(std::vector<ExpressionPtr>  args) {
    ConstantValue   v;
    vector<ConstantValue>   constVec;

    v.valueType = ConstantValue::T_I32;
    v.width = 32;
    v.valueIsKnown = false;

    getConstArgs(args, constVec);
    if( constVec.size() == 1 ) {
        //C64HIto32 is a unop
        ConstantValue   a1 = constVec[0];

        assert(a1.width == 64);

        //we have enough information to set v
        v.valueIsKnown = true;
        uint64_t k = a1.U64;
        uint64_t l = k & ~0x00000000FFFFFFFF;
        v.U32 = (uint32_t) l;
    }

    return v;
}

ConstantValue doC8UTo16(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 16;
    v.valueType = ConstantValue::T_I16;
    getConstArgs(args, constArgs);

    if( constArgs.size() == 1 ) {
        ConstantValue v1 = constArgs[0];

        v.valueIsKnown = true;
        v.U16 = v1.U8;
    }

    return v;
}

ConstantValue doC8UTo64(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 64;
    v.valueType = ConstantValue::T_I64;
    getConstArgs(args, constArgs);

    //throw StepErr("C8Uto64");
    if( constArgs.size() == 1 ) {
        ConstantValue   v1 = constArgs[0];

        v.valueIsKnown = true;
        v.U64 = v1.U8;
    }

    return v;
}

ConstantValue doC8STo32(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.width = 32;
    v.valueType = ConstantValue::T_I32;
    v.valueIsKnown = false;
    getConstArgs(args, constArgs);

    if( constArgs.size() == 1 ) {
        ConstantValue a1 = constArgs[0];

        assert(a1.width == 8);
        v.valueIsKnown = true;
        int32_t *t = (int32_t *)&v.U32;
        *t = ((int8_t)a1.U8);
    }

    return v;
}

ConstantValue doC8UTo32(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.width = 32;
    v.valueType = ConstantValue::T_I32;
    v.valueIsKnown = false;
    getConstArgs(args, constArgs);

    if( constArgs.size() == 1 ) {
        ConstantValue a1 = constArgs[0];

        assert(a1.width == 8);
        v.valueIsKnown = true;
        v.U32 = a1.U8;
    }

    return v;
}

ConstantValue doC32Uto64(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 64;
    v.valueType = ConstantValue::T_I64;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 1 ) {
        ConstantValue v1 = constArgs[0];

        v.valueIsKnown = true;

        v.U64 = v1.U32;
    }

    return v;
}

ConstantValue doC16UTo64(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 64;
    v.valueType = ConstantValue::T_I64;
    getConstArgs(args, constArgs);

    throw StepErr("C16Uto64");
    if( constArgs.size() == 1 ) {
        ConstantValue   v1 = constArgs[0];

        v.valueIsKnown = true;
    }

    return v;
}

ConstantValue doC32To1(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.width = 1;
    v.valueType = ConstantValue::T_I1;
    v.valueIsKnown = false;
    getConstArgs(args, constArgs);

    if( constArgs.size() == 1 ) {
        ConstantValue   a1 = constArgs[0];
        //C32To1 is a unop
        assert(a1.width == 32);
        assert(a1.valueType == ConstantValue::T_I32);
        v.valueIsKnown = true;
        v.U1 = a1.U32 & ~0xfffffffe;
    }

    return v;
}

ConstantValue doC64To1(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.width = 1;
    v.valueType = ConstantValue::T_I1;
    v.valueIsKnown = false;
    getConstArgs(args, constArgs);

    if( constArgs.size() == 1 ) {
        ConstantValue   a1 = constArgs[0];
        //C64To1 is a unop
        assert(a1.width == 64);
        assert(a1.valueType == ConstantValue::T_I64);
        v.valueIsKnown = true;
        v.U1 = a1.U64 & ~0xfffffffffffffffe;
    }

    return v;
}

ConstantValue doC1UTo8(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;
    v.width = 8;
    v.valueType = ConstantValue::T_I8;
    v.valueIsKnown = false;
    getConstArgs(args, constArgs);

    if( constArgs.size() == 1 ) {
        ConstantValue a1 = constArgs[0];
        v.valueIsKnown = true;
        v.U8 = a1.U1;
    }

    return v;
}

ConstantValue doC1UTo32(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;
    v.width = 32;
    v.valueType = ConstantValue::T_I32;
    v.valueIsKnown = false;
    getConstArgs(args, constArgs);

    if( constArgs.size() == 1 ) {
        ConstantValue a1 = constArgs[0];
        v.valueIsKnown = true;
        v.U32 = a1.U1;
    }

    return v;
}

ConstantValue doC32STo64(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 64;
    v.valueType = ConstantValue::T_I64;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 1 ) {
        ConstantValue   v1 = constArgs[0];

        v.valueIsKnown = true;

        int64_t *t = (int64_t *)&v.U64;
        *t = ((int64_t)v1.U32);
    }

    return v;
}

ConstantValue doC16STo32(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 32;
    v.valueType = ConstantValue::T_I32;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 1 ) {
        ConstantValue   v1 = constArgs[0];

        v.valueIsKnown = true;

        int32_t *t = (int32_t *)&v.U32;
        *t = ((int32_t)v1.U16);
    }

    return v;
}

ConstantValue doC8STo16(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 16;
    v.valueType = ConstantValue::T_I16;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 1 ) {
        ConstantValue v1 = constArgs[0];

        v.valueIsKnown = true;

        int16_t *t = (int16_t *)&v.U16;
        *t = ((int16_t)v1.U8);
    }

    return v;
}

ConstantValue doDivModU64to32(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 64;
    v.valueType = ConstantValue::T_I64;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue v1 = constArgs[0];
        ConstantValue v2 = constArgs[1];

        v.valueIsKnown = true;
        assert(v1.width == 64);
        assert(v2.width == 32);
        uint64_t a1 = (uint64_t)v1.U64;
        uint32_t a2 = (uint32_t)v2.U32;
        if( a2 == 0 ) {
            throw StepClientErr("Div by 0");
        }
        uint32_t divRes = a1/a2;
        uint32_t modRes = a1%a2;
        uint64_t final = modRes;
        final = final << 32;
        final += divRes;

        v.U64 = final;
    }

    return v;
}

ConstantValue doDivModS64to32(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 64;
    v.valueType = ConstantValue::T_I64;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue v1 = constArgs[0];
        ConstantValue v2 = constArgs[1];

        v.valueIsKnown = true;
        assert(v1.width == 64);
        assert(v2.width == 32);
        int64_t a1 = (int64_t)v1.U64;
        int32_t a2 = (int32_t)v2.U32;
        if( a2 == 0 ) {
            throw StepClientErr("Div by 0");
        }
        int32_t divRes = a1/a2;
        int32_t modRes = a1%a2;
        uint64_t final = modRes;
        final = final << 32;
        final += divRes;

        v.U64 = final;
    }

    return v;
}

ConstantValue doC32HITo16(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 16;
    v.valueType = ConstantValue::T_I16;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 1 ) {
        ConstantValue   v1 = constArgs[0];

        assert(v1.width == 32);

        v.valueIsKnown = true;

        uint32_t t = v1.U32;
        t = t >> 16;
        v.U16 = t;
    }

    return v;
}

ConstantValue doC32LOTo16(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 16;
    v.valueType = ConstantValue::T_I16;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 1 ) {
        ConstantValue   v1 = constArgs[0];

        assert(v1.width == 32);
        uint32_t    a1 = v1.U32;
        uint16_t    vt = a1 & 0xFFFF;

        v.U16 = vt;
    }

    return v;
}

ConstantValue doC16HITo8(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 8;
    v.valueType = ConstantValue::T_I8;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 1 ) {
        ConstantValue   v1 = constArgs[0];

        assert(v1.width == 16);
        uint16_t    t = v1.U16;
        t = t >> 8;
        v.valueIsKnown = true;
        v.U16 = t;
    }

    return v;
}

ConstantValue doC16LOTo8(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 8;
    v.valueType = ConstantValue::T_I8;

    getConstArgs(args, constArgs);
    if( constArgs.size() == 1 ) {
        ConstantValue   v1 = constArgs[0];

        assert(v1.width == 8);
        uint16_t a1 = v1.U16;

        v.U8 = a1 & 0xFF;
    }

    return v;
}

ConstantValue doCmpLTS(std::vector<ExpressionPtr> args) {
    ConstantValue   v;
    vector<ConstantValue> constArgs;

    v.valueIsKnown = false;
    v.width = 1;
    v.valueType = ConstantValue::T_I1;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue   v1 = constArgs[0];
        ConstantValue   v2 = constArgs[1];

        v.valueIsKnown = true;

        assert(v1.width == v2.width);
        unsigned short opW = v1.width;
        switch(opW) {
            case 32:
                {
                    int32_t    a1 = ((int32_t)v1.U32);
                    int32_t    a2 = ((int32_t)v2.U32);

                    if( a1 < a2 ) {
                        v.U1 = 1;
                    } else {
                        v.U1 = 0;
                    }
                }
                break;
            case 64:
                {
                    int64_t    a1 = ((int64_t)v1.U64);
                    int64_t    a2 = ((int64_t)v2.U64);

                    if( a1 < a2 ) {
                        v.U1 = 1;
                    } else {
                        v.U1 = 0;
                    }
                }
                break;
        }

    }

    return v;
}

ConstantValue doCmpLTU(std::vector<ExpressionPtr> args) {
    ConstantValue   v;
    vector<ConstantValue> constArgs;

    v.valueIsKnown = false;
    v.width = 1;
    v.valueType = ConstantValue::T_I1;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue   v1 = constArgs[0];
        ConstantValue   v2 = constArgs[1];

        v.valueIsKnown = true;

        assert(v1.width == v2.width);
        unsigned short opW = v1.width;
        switch(opW) {
            case 32:
                {
                    uint32_t    a1 = v1.U32;
                    uint32_t    a2 = v2.U32;

                    if( a1 < a2 ) {
                        v.U1 = 1;
                    } else {
                        v.U1 = 0;
                    }
                }
                break;
            case 64:
                {
                    uint64_t    a1 = v1.U64;
                    uint64_t    a2 = v2.U64;

                    if( a1 < a2 ) {
                        v.U1 = 1;
                    } else {
                        v.U1 = 0;
                    }
                }
                break;
        }
    }

    return v;

}

ConstantValue doCmpLES(std::vector<ExpressionPtr> args) {
    ConstantValue   v;
    vector<ConstantValue> constArgs;

    v.valueIsKnown = false;
    v.width = 1;
    v.valueType = ConstantValue::T_I1;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue   v1 = constArgs[0];
        ConstantValue   v2 = constArgs[1];

        v.valueIsKnown = true;

        assert(v1.width == v2.width);
        unsigned short opW = v1.width;
        switch(opW) {
            case 32:
                {
                    int32_t    a1 = ((int32_t)v1.U32);
                    int32_t    a2 = ((int32_t)v2.U32);

                    if( a1 <= a2 ) {
                        v.U1 = 1;
                    } else {
                        v.U1 = 0;
                    }
                }
                break;
            case 64:
                {
                    int64_t    a1 = ((int64_t)v1.U64);
                    int64_t    a2 = ((int64_t)v2.U64);

                    if( a1 <= a2 ) {
                        v.U1 = 1;
                    } else {
                        v.U1 = 0;
                    }
                }
                break;
        }

    }

    return v;

}

ConstantValue doCmpLEU(std::vector<ExpressionPtr> args) {
    ConstantValue   v;
    vector<ConstantValue> constArgs;

    v.valueIsKnown = false;
    v.width = 1;
    v.valueType = ConstantValue::T_I1;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue   v1 = constArgs[0];
        ConstantValue   v2 = constArgs[1];

        v.valueIsKnown = true;

        assert(v1.width == v2.width);
        unsigned short opW = v1.width;
        switch(opW) {
            case 32:
                {
                    uint32_t    a1 = v1.U32;
                    uint32_t    a2 = v2.U32;

                    if( a1 <= a2 ) {
                        v.U1 = 1;
                    } else {
                        v.U1 = 0;
                    }
                }
                break;
            case 64:
                {
                    uint64_t    a1 = v1.U64;
                    uint64_t    a2 = v2.U64;

                    if( a1 <= a2 ) {
                        v.U1 = 1;
                    } else {
                        v.U1 = 0;
                    }
                }
                break;
        }

    }

    return v;

}

ConstantValue doC16HLTo32(std::vector<ExpressionPtr> args) {
    ConstantValue   v;
    vector<ConstantValue> constArgs;

    v.valueIsKnown = false;
    v.width = 32;
    v.valueType = ConstantValue::T_I32;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue   v1 = constArgs[0];
        ConstantValue   v2 = constArgs[1];
        v.valueIsKnown = true;

        assert(v1.width == 16);
        assert(v2.width == 16);

        uint16_t    a1 = v1.U16;
        uint16_t    a2 = v2.U16;
        uint32_t    res = a1;

        res = res << 16;
        res += a2;

        v.U32 = res;
    }

    return v;
}

ConstantValue doC16UTo32(std::vector<ExpressionPtr> args) {
    ConstantValue   v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 32;
    v.valueType = ConstantValue::T_I32;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 1 ) {
        ConstantValue   v1 = constArgs[0];

        v.valueIsKnown = true;
        v.U32 = 0;
        v.U32 = v1.U16;
    }

    return v;
}

ConstantValue doC32HLTo64(std::vector<ExpressionPtr> args) {
    ConstantValue v;
    vector<ConstantValue>   constArgs;

    v.valueIsKnown = false;
    v.width = 64;
    v.valueType = ConstantValue::T_I64;
    getConstArgs(args, constArgs);
    if( constArgs.size() == 2 ) {
        ConstantValue   v1 = constArgs[0];
        ConstantValue   v2 = constArgs[1];

        v.valueIsKnown = true;

        uint64_t    res = v1.U32;
        res = res << 32;
        res += v2.U32;
        v.U64 = res;
    }

    return v;
}


/***************************************************************************
 *  @file       matrix.hpp
 *  @author     Alan.W
 *  @date       28  July 2014
 *  @remark     Implementation for CLRS, using C++ templates.
 ***************************************************************************/
#ifndef MATRIX_HPP
#define MATRIX_HPP

#include <boost/numeric/ublas/matrix.hpp>
#include <assert.h>

namespace ch15
{

//! alias for cha15
template<typename T>
using Matrix = boost::numeric::ublas::matrix<T>;

/**
 * @brief print a matrix like a matrix
 */
template<typename T>
void print(const ch15::Matrix<T>& mat)
{
    using SizeType  =   typename ch15::Matrix<T>::size_type;

    for(SizeType row = 0; row != mat.size1(); ++row)
    {
        for(SizeType col = 0; col != mat.size2(); ++col)
            std::cout << mat(row, col) << " ";
        std::cout << "\n\n";
    }
}

/**
 * @brief copy_row
 * @param mat
 * @param rng
 * @param row
 *
 * @date  13 aug 2014
 *
 * copy a row from a std container into a boost matrix.
 * implemented for longest_mono_increasing.hpp
 */
template<typename Range>
void inline
copy_row(ch15::Matrix<typename Range::value_type>& mat,
                const Range& rng,
                typename ch15::Matrix<typename Range::value_type>::size_type
                        row)
{
    assert(rng.size()   ==  mat.size2());
    using SizeType  =   typename Range::size_type;

    for(SizeType count = 0; count != rng.size(); ++count)
        mat(row, count) =   rng[count];
}

}//namespace ch15

//! @brief  extending boost matrix
namespace boost { namespace numeric { namespace ublas {

/**
 * @brief forwarding declarations
 */
template<typename Range>
void
operator <<= (matrix<typename Range::value_type>& mat, const Range& rng);
template<typename Range>
void
operator >>= ( const Range& rng, matrix<typename Range::value_type>& mat);
template<typename T>
matrix<T>
operator*(const matrix<T>& lhs, const matrix<T>& rhs);

/**
 * @brief operator <<=
 * @param lhs   matrix
 * @param rhs   a stl container that stores data
 *
 * copy data from rhs to matrix, leaving data in matrix untouched when data from
 * the stl container is not enough.
 */
template<typename Range>
inline void
operator <<= (matrix<typename Range::value_type>& mat, const Range& rng)
{
    using SizeType  =   typename matrix<typename Range::value_type>::size_type;

    //! check if equal
    SizeType mat_size   =   mat.size1() *   mat.size2();
    assert(mat_size == rng.size());

    //! copy
    for(SizeType index = 0; index != mat_size; ++index)
        mat.data()[index]   =   rng[index];
}

/**
 * @brief operator >>=
 * @param lhs   a stl container that stores data
 * @param rhs   matrix
 */
template<typename Range>
inline void
operator >>= ( const Range& rng, matrix<typename Range::value_type>& mat)
{
    mat <<= rng;
}

/**
 * @brief operator *
 * @param lhs
 * @param rhs
 *
 * @page 371
 * @pseudocode MATRIX-MULTIPLY
 */
template<typename T>
inline matrix<T>
operator*(const matrix<T>& lhs, const matrix<T>& rhs)
{
    assert(lhs.size2() == rhs.size1());

    matrix<T> ret(lhs.size1(), rhs.size2());
    for(unsigned row = 0; row != lhs.size1(); ++row)
        for(unsigned col = 0; col != rhs.size2(); ++ col)
        {
            ret(row,col) = 0;
            for(unsigned count = 0 ; count != lhs.size2(); ++count)
                ret(row,col) += lhs(row,count) * rhs(count, col);
        }

    return ret;
}

}}}//namespace boost::numeric::ublas

#endif // MATRIX_HPP

//! test for matrix assignment overloading
//! i.e.    <<=
//#include <iostream>
//#include <boost/numeric/ublas/matrix.hpp>
//#include <boost/numeric/ublas/io.hpp>
//#include "color.hpp"
//#include "matrix.hpp"
//#include "matrix_chain_order.hpp"
//#include "matrix_chain_mutiply.hpp"

//int main()
//{
//    using namespace boost::numeric::ublas;

//    std::vector<int> v = {1,2,3,4,5,6,7,8,9};
//    matrix<int> mat(3,3);

//    mat <<= v;
//    std::cout << mat << std::endl;

//    std::cout << color::red("\nend\n");
//    return 0;
//}

//! @test   ch15::copy_row
//!
//#include <iostream>
//#include <boost/numeric/ublas/io.hpp>
//#include "color.hpp"
//#include "matrix.hpp"

//int main()
//{
//    ch15::Matrix<int> mat(3,3,0);

//    ch15::copy_row(mat, std::vector<int>({1,2,3}), 0);
//    ch15::print(mat);

//    std::cout << color::red("\nend\n");
//    return 0;
//}
//! @output:
//!

//1 2 3

//0 0 0

//0 0 0

//end


/*
 *  Copyright (C) 2018 Naomasa Matsubayashi
 *  Licensed under MIT license, see file LICENSE in this source tree.
 */
#ifndef UWG_RESPONDER_HPP
#define UWG_RESPONDER_HPP

#include <iterator>
#include <boost/spirit/include/karma.hpp>
#include <boost/spirit/include/qi.hpp>
#include <uwg/hash.hpp>
#include <uwg/mac.hpp>
#include <uwg/kdf.hpp>
#include <uwg/dh.hpp>
#include <uwg/aead.hpp>
#include <uwg/timestamp.hpp>
#include <uwg/initial_key.hpp>
#include <uwg/clear_key.hpp>
#include <uwg/types.hpp>

namespace uwg {
  template< typename Out, typename CookieType, typename In >
  void responder_hello( key_state &key, Out &out, peer_states &peers, const wg_key_type &self_static_private, const wg_key_type &self_static_public, const wg_key_type &remote_static_public, uint32_t self_spi, const CookieType &cookie, const In &in ) {
    if( in.size() != wg_kx1_len ) throw invalid_packet();
    if( in[ 0 ] != 0x01 ) throw invalid_packet();
    auto encrypted_static = make_svv( in.data(), wg_kx1_encrypted_static_offset, wg_encrypted_static_len );
    auto encrypted_timestamp = make_svv( in.data(), wg_kx1_encrypted_tai64n_offset, wg_encrypted_tai64n_len );
    auto remote_ephemeral_public = make_svv( in.data(), wg_kx1_ephemeral_offset, wg_ephemeral_len );
    wg_key_type kdf_key;
    wg_key_type aead_key;
    wg_key_type chain_key;
    wg_key_type hash_key;
    wg_key_type incoming_remote_static_public;
    wg_key_type self_ephemeral_private;
    wg_key_type q( wg_key_len, 0u );
    wg_key_type t;
    wg_key_type empty;
    wg_key_type mac_key;
    wg_tai64n_type remote_timestamp;
    wg_key_type calculated_kx1_mac1;
    namespace qi = boost::spirit::qi;
    uint32_t remote_spi;
    qi::parse( std::next( in.begin(), wg_kx1_self_spi_offset ), std::next( in.begin(), wg_kx1_self_spi_offset + wg_spi_len ), qi::little_dword, remote_spi );
    auto kx1_mac1_message = make_svv( in.data(), 0, wg_kx1_mac1_message_len );
    auto incoming_kx1_mac1 = make_svv( in.data(), wg_kx1_mac1_offset, wg_mac_len );
    constexpr static char label_mac1[] = "mac1----";
    hash().update( label_mac1, label_mac1 + strlen( label_mac1 ) ).update( self_static_public ).get( mac_key );
    mac( mac_key ).update( kx1_mac1_message ).get( calculated_kx1_mac1 );
    if( !std::equal( calculated_kx1_mac1.begin(), calculated_kx1_mac1.end(), incoming_kx1_mac1.begin(), incoming_kx1_mac1.end() ) )
      throw invalid_packet();
    get_initial_chain_key( chain_key );
    get_initial_hash_key( hash_key, chain_key );
    kdf( chain_key ).update( remote_ephemeral_public ).get( chain_key );
    hash().update( hash_key, self_static_public ).get( hash_key );
    hash().update( hash_key, remote_ephemeral_public ).get( hash_key );
    dh( kdf_key, self_static_private, remote_ephemeral_public );
    kdf( chain_key ).update( kdf_key ).get( chain_key, aead_key );
    out.resize( wg_kx2_len, 0u );
    if( !aead_dec( incoming_remote_static_public, aead_key, 0ull, encrypted_static, hash_key ) )
      throw invalid_packet();
    hash().update( hash_key, encrypted_static ).get( hash_key );
    dh( kdf_key, self_static_private, remote_static_public );
    kdf( chain_key ).update( kdf_key ).get( chain_key, aead_key );
    if( !aead_dec( remote_timestamp, aead_key, 0ull, encrypted_timestamp, hash_key ) )
      throw invalid_packet();
    if( remote_timestamp.size() != wg_tai64n_len  )
      throw invalid_packet();
    const auto peer = peers.find( incoming_remote_static_public );
    if( peer != peers.end() ) {
      if( memcmp( peer->second.timestamp.data(), remote_timestamp.data(), wg_tai64n_len ) >= 0 )
        throw invalid_packet();
      peer->second.timestamp = remote_timestamp;
    }
    else peers.insert( std::make_pair( incoming_remote_static_public, peer_state( remote_timestamp ) ) );
    hash().update( hash_key, encrypted_timestamp ).get( hash_key );

    out.resize( wg_kx2_len, 0u );
    auto self_ephemeral_public = make_svv( out.data(), wg_kx2_ephemeral_offset, wg_ephemeral_len );
    auto encrypted_empty = make_svv( out.data(), wg_kx2_encrypted_empty_offset, wg_encrypted_empty_len );
    dh_generate( self_ephemeral_private, self_ephemeral_public );
    kdf( chain_key ).update( self_ephemeral_public ).get( chain_key );
    hash().update( hash_key, self_ephemeral_public ).get( hash_key );
    dh( kdf_key, self_ephemeral_private, remote_ephemeral_public );
    kdf( chain_key ).update( kdf_key ).get( chain_key );
    dh( kdf_key, self_ephemeral_private, remote_static_public );
    kdf( chain_key ).update( kdf_key ).get( chain_key );
    kdf( chain_key ).update( q ).get( chain_key, t, aead_key );
    hash().update( hash_key, t ).get( hash_key );
    aead_enc( encrypted_empty, aead_key, 0ull, empty, hash_key );
    hash().update( hash_key, encrypted_empty ).get( hash_key );
    out[ 0 ] = 0x02;
    namespace karma = boost::spirit::karma;
    karma::generate( std::next( out.begin(), wg_kx2_self_spi_offset ), karma::little_dword, self_spi );
    karma::generate( std::next( out.begin(), wg_kx2_remote_spi_offset ), karma::little_dword, remote_spi );
    auto kx2_mac1_message = make_svv( out.data(), 0, wg_kx2_mac1_message_len );
    auto kx2_mac1 = make_svv( out.data(), wg_kx2_mac1_offset, wg_mac_len );
    hash().update( label_mac1, label_mac1 + strlen( label_mac1 ) ).update( remote_static_public ).get( mac_key );
    mac( mac_key ).update( kx2_mac1_message ).get( kx2_mac1 );
    if( !cookie.empty() ) {
      auto mac2_message = make_svv( out.data(), 0, wg_kx2_mac2_message_len );
      auto mac2 = make_svv( out.data(), wg_kx2_mac2_offset, wg_mac_len );
      mac( cookie ).update( mac2_message ).get( mac2 );
    }
    kdf( chain_key ).update( empty ).get( key.receive_key, key.send_key );
    kdf_key.resize( wg_key_len );
    clear_key( kdf_key );
    clear_key( aead_key );
    clear_key( mac_key );
    clear_key( q );
    clear_key( t );
    clear_key( self_ephemeral_private );
    clear_key( chain_key );
    clear_key( hash_key );
  }
}
#endif