/*
 * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy
 * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy
 * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "bebop_simulator/waypoint_filter.h"

#include <Eigen/Eigen>
#include <chrono>
#include <time.h>

#include <trajectory_msgs/MultiDOFJointTrajectory.h>

using namespace std;

namespace bebop_simulator {

WaypointFilter::WaypointFilter()
         :filter_initialized_(false){

         command_trajectory_private_.setFromYaw(0);
         command_trajectory_toSend_.setFromYaw(0);

         command_trajectory_private_.position_W[0] = 0;
         command_trajectory_private_.position_W[1] = 0;
         command_trajectory_private_.position_W[2] = 0;

         command_trajectory_toSend_.position_W[0] = 0;
         command_trajectory_toSend_.position_W[1] = 0;
         command_trajectory_toSend_.position_W[2] = 0;

         Tsf_ = 0;
         H_ = 0;

}

WaypointFilter::~WaypointFilter() {}

// Set the filter parameters
void WaypointFilter::SetParameters(WaypointFilterParameters *waypointFilter_parameters_){

     Tsf_ = waypointFilter_parameters_->tsf_;
     H_ = waypointFilter_parameters_->h_;

}

// Set the trajectory point
void WaypointFilter::SetTrajectoryPoint(const mav_msgs::EigenTrajectoryPoint& command_trajectory_positionControllerNode){

    command_trajectory_private_ = command_trajectory_positionControllerNode;

}

// Get the trajectory point from the control library
void WaypointFilter::GetTrajectoryPoint(mav_msgs::EigenTrajectoryPoint* command_trajectory_positionController){

    *command_trajectory_positionController = command_trajectory_toSend_;

}

// Filter initialization
void WaypointFilter::Initialize(state_t state_){

   if(!filter_initialized_){

     command_trajectory_toSend_.position_W[0] = state_.position.x;
     command_trajectory_toSend_.position_W[1] = state_.position.y;
     command_trajectory_toSend_.position_W[2] = state_.position.z;

     filter_initialized_ = true;

   }
}

// Trajectory generation
void WaypointFilter::TrajectoryGeneration(){

    command_trajectory_toSend_.position_W[0] = (Tsf_/(Tsf_+H_)) * command_trajectory_toSend_.position_W[0] + (H_/(Tsf_+H_)) * command_trajectory_private_.position_W[0];
    command_trajectory_toSend_.position_W[1] = (Tsf_/(Tsf_+H_)) * command_trajectory_toSend_.position_W[1] + (H_/(Tsf_+H_)) * command_trajectory_private_.position_W[1];
    command_trajectory_toSend_.position_W[2] = (Tsf_/(Tsf_+H_)) * command_trajectory_toSend_.position_W[2] + (H_/(Tsf_+H_)) * command_trajectory_private_.position_W[2];

    double yaw = (Tsf_/(Tsf_+H_)) * command_trajectory_toSend_.getYaw() + (H_/(Tsf_+H_)) * command_trajectory_private_.getYaw();
    command_trajectory_toSend_.setFromYaw(yaw);
}

}


#include <boost/units/systems/angle/gradians.hpp>


#include <iostream>
#include <vector>
#include "jefflib.h"
#include <boost/foreach.hpp>
#include <boost/tokenizer.hpp>

using namespace std;
using namespace boost;

int main()
{
    vector<string> vect;
    if(GetStringInput(vect)){
        cout << "Got data!" << endl;
        cout << endl;
    }
    else {
        cout << "Failed to read input :( " << cout;
        return -1;
    }

    // Create a 1000x1000 piece of cloth
    int cloth[1000][1000] = {0};

    // Accumulate claims
    for (const string line : vect)
    {
        char_separator<char> sep(" @,:x");
        tokenizer< char_separator<char> > tokens(line, sep);
        // num col row width height
        vector<string> s_tmp(tokens.begin(), tokens.end());
        int col_offset = stoi(s_tmp[1]);
        int row_offset = stoi(s_tmp[2]);
        int width = stoi(s_tmp[3]);
        int height = stoi(s_tmp[4]);

        for(int col = col_offset; col < col_offset+width; col++){
            for(int row = row_offset; row<row_offset+height; row++){
                cloth[row][col] += 1;
            }
        }
    }

    // Find claim
    for (const string line : vect)
    {
        char_separator<char> sep(" @,:x");
        tokenizer< char_separator<char> > tokens(line, sep);
        // num col row width height
        vector<string> s_tmp(tokens.begin(), tokens.end());
        string id = s_tmp[0];
        int col_offset = stoi(s_tmp[1]);
        int row_offset = stoi(s_tmp[2]);
        int width = stoi(s_tmp[3]);
        int height = stoi(s_tmp[4]);

        bool perfect = true;
        for(int col = col_offset; col < col_offset+width; col++){
            for(int row = row_offset; row<row_offset+height; row++){
                if(cloth[row][col] != 1){
                    perfect = false;
                }
            }
        }

        if(perfect){
            cout << "Found it! " << id << endl;
            return 0;
        }
    }

    cout << "Count: " << cout << endl;
}


#include <stan/math/prim/scal.hpp>
#include <stan/math/rev/core/var.hpp>
#include <gtest/gtest.h>
#include <boost/typeof/typeof.hpp>
#include <type_traits>

using stan::math::promote_elements;
using stan::math::var;

TEST(MathFunctionsScalPromote_Elements, int2double) {
  int from;
  promote_elements<double, int> p;
  typedef BOOST_TYPEOF(p.promote(from)) result_t;
  bool same = std::is_same<double, result_t>::value;
  EXPECT_TRUE(same);
}

TEST(MathFunctionsScalPromote_Elements, double2double) {
  double from;
  promote_elements<double, double> p;
  typedef BOOST_TYPEOF(p.promote(from)) result_t;
  bool same = std::is_same<double, result_t>::value;
  EXPECT_TRUE(same);
}

TEST(MathFunctionsScalPromote_Elements, double2var) {
  double from;
  promote_elements<var, double> p;
  typedef BOOST_TYPEOF(p.promote(from)) result_t;
  bool same = std::is_same<var, result_t>::value;
  EXPECT_TRUE(same);
}


/**
 *  This file is part of dvo.
 *
 *  Copyright 2012 Christian Kerl <christian.kerl@in.tum.de> (Technical University of Munich)
 *  For more information see <http://vision.in.tum.de/data/software/dvo>.
 *
 *  dvo is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  dvo is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with dvo.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <dvo_core/intrinsic_matrix.h>

#include <boost/unordered_map.hpp>

std::size_t IntrinsicMatrix::Hash::operator ()(IntrinsicMatrix const& value) const
{
    std::size_t seed = 0;

    boost::hash_combine(seed, value.fx());
    boost::hash_combine(seed, value.fy());
    boost::hash_combine(seed, value.ox());
    boost::hash_combine(seed, value.oy());

    return seed;
}

bool IntrinsicMatrix::Equal::operator()(IntrinsicMatrix const& left, IntrinsicMatrix const& right) const
{
    return left.fx() == right.fx() && left.fy() == right.fy() && left.ox() == right.ox() && left.oy() == right.oy();
}

IntrinsicMatrix IntrinsicMatrix::create(float fx, float fy, float ox, float oy)
{
  IntrinsicMatrix result;
  result.data.setZero();
  result.data(0, 0) = fx;
  result.data(1, 1) = fy;
  result.data(2, 2) = 1.0f;
  result.data(0, 2) = ox;
  result.data(1, 2) = oy;

  return result;
}

IntrinsicMatrix::IntrinsicMatrix(const IntrinsicMatrix & other) : data(other.data)
{
}

float IntrinsicMatrix::fx() const
{
  return data(0, 0);
}

float IntrinsicMatrix::fy() const
{
  return data(1, 1);
}

float IntrinsicMatrix::ox() const
{
  return data(0, 2);
}

float IntrinsicMatrix::oy() const
{
  return data(1, 2);
}

void IntrinsicMatrix::invertOffset()
{
  data(0, 2) *= -1;
  data(1, 2) *= -1;
}

void IntrinsicMatrix::scale(float factor)
{
  data *= factor;
}


/* simd_sqrt_test.cc
   Jeremy Barnes, 30 August 2005
   Copyright (c) 2005 Jeremy Barnes.  All rights reserved.
   $Source$

   Test of square root functionality and accuracy using SIMD instructions.
*/

#include "jml/boosting/config_impl.h"
#include "jml/arch/sse.h"
#include <boost/timer.hpp>
#include <vector>
#include <cmath>
#include "jml/arch/tick_counter.h"

using namespace std;

void vec_sqrt_simd(const float * x, float * r, int n)
{
    using namespace Arch::SSE;

    pre_prefetch(x);
    pre_prefetch_write(r);

    v4sf hhalf = load_const(0.5);
    v4sf ttwo  = load_const(2.0);
    v4sf nn, aa, rr, xx, yy;

    while (n >= 4) {
        /* Load our x and y arrays. */
        nn      = loadups(x);
        loop_prefetch(x);
        rr      = VEC_INSN(rsqrtps, (nn));

#if 1
        aa   = VEC_INSN(rcpps,   (rr));  // approx
        xx   = mulps(rr, aa);
        yy   = subps(ttwo, xx);
        aa   = mulps(aa, yy);
#else
        aa      = accurate_recip(rr);
#endif

        rr      = mulps(rr, nn);
        rr      = addps(aa, rr);
        aa      = mulps(hhalf, rr);
        storeups(r, aa);
        loop_prefetch_write(r);
        x += 4;
        r += 4;
        n -= 4;
    }
}

void vec_sqrt_simd2(const float * x, float * r, int n)
{
    using namespace Arch::SSE;

    //pre_prefetch(x);
    //pre_prefetch_write(r);

    v4sf hhalf = load_const(0.5);
    v4sf ttwo  = load_const(2.0);
    v4sf nn0, nn1, aa0, aa1, rr0, rr1, xx0, xx1, yy0, yy1;

    while (n >= 8) {
        /* Load our x and y arrays. */
        nn0     = loadaps(x);
        loop_prefetch(x);
        nn1     = loadaps(x + 4);
        rr0     = VEC_INSN(rsqrtps, (nn0));
        rr1     = VEC_INSN(rsqrtps, (nn1));
        aa0     = VEC_INSN(rcpps,   (rr0));  // approx
        aa1     = VEC_INSN(rcpps,   (rr1));  // approx
        xx0     = mulps(rr0, aa0);
        yy0     = subps(ttwo, xx0);
        xx1     = mulps(rr1, aa1);
        x += 8;
        aa0     = mulps(aa0, yy0);
        yy1     = subps(ttwo, xx1);
        rr0     = mulps(rr0, nn0);
        rr0     = addps(aa0, rr0);
        aa1     = mulps(aa1, yy1);
        n -= 8;
        aa0     = mulps(hhalf, rr0);
        rr1     = mulps(rr1, nn1);
        storeaps(r, aa0);
        rr1     = addps(aa1, rr1);
        loop_prefetch_write(r);
        aa1     = mulps(hhalf, rr1);
        storeaps(r + 4, aa1);
        r += 8;
    }
}

void vec_sqrt_simd3(const float * x, float * r, int n)
{
    using namespace Arch::SSE;

    //pre_prefetch(x);
    //pre_prefetch_write(r);

    v4sf nn0, nn1, rr0, rr1;

    while (n >= 8) {
        /* Load our x and y arrays. */
        nn0     = loadaps(x);
        loop_prefetch(x);
        nn1     = loadaps(x + 4);
        rr0     = VEC_INSN(sqrtps, (nn0));
        rr1     = VEC_INSN(sqrtps, (nn1));
        x += 8;
        n -= 8;
        storeaps(r, rr0);
        loop_prefetch_write(r);
        storeaps(r + 4, rr1);
        r += 8;
    }
}

void vec_sqrt_scalar(const float * p, float * r, int n)
{
    for (unsigned i = 0;  i < n;  ++i)
        *r++ = std::sqrt(*p++);
}

void test1()
{
    size_t SIZE = 1000;
    size_t TRIALS = 100000;
    vector<float> numbers(SIZE);
    for (unsigned i = 0;  i < SIZE;  ++i)
        numbers[i] = rand();

    // test scalar version
    {
        vector<float> x = numbers;

        double before = ticks();

        for (unsigned i = 0;  i < TRIALS;  ++i)
            vec_sqrt_scalar(&x[0], &x[0], SIZE);

        double after = ticks() - ticks_overhead;

        cerr << "scalar: " << (after - before) / (TRIALS * SIZE)
             << " ticks/call" << endl;
    }

    // test vector version
    {
        vector<float> x = numbers;

        double before = ticks();

        for (unsigned i = 0;  i < TRIALS;  ++i)
            vec_sqrt_simd(&x[0], &x[0], SIZE);

        double after = ticks() - ticks_overhead;

        cerr << "vector: " << (after - before) / (TRIALS * SIZE)
             << " ticks/call" << endl;
    }

    // test vector version
    {
        vector<float> x = numbers;

        double before = ticks();

        for (unsigned i = 0;  i < TRIALS;  ++i)
            vec_sqrt_simd2(&x[0], &x[0], SIZE);

        double after = ticks() - ticks_overhead;

        cerr << "vector2: " << (after - before) / (TRIALS * SIZE)
             << " ticks/call" << endl;
    }

    // test vector version
    {
        vector<float> x = numbers;

        double before = ticks();

        for (unsigned i = 0;  i < TRIALS;  ++i)
            vec_sqrt_simd3(&x[0], &x[0], SIZE);

        double after = ticks() - ticks_overhead;

        cerr << "vector3: " << (after - before) / (TRIALS * SIZE)
             << " ticks/call" << endl;
    }
}

int main(int argc, char ** argv)
{
    using namespace Arch::SSE;

#if 0
    double num = 2.09384923048;

    double correct = std::sqrt(num);

    int iter = 6;

    float approx = num / 2;

    for (unsigned i = 0;  i < iter;  ++i) {
        float z = approx * approx - num;
        float new_approx = 0.5 * (approx + num / approx);
        cerr << "iter " << i << " approx = " << approx
             << " new_approx = " << new_approx << " z = " << z << endl;
        approx = new_approx;
        cerr << "after " << i + 1 << " iterations, error = "
             << correct - approx << endl;
    }

    v4sf nn    = load_const(num);
    v4sf hhalf = load_const(0.5);
    v4sf cc    = load_const(correct);
    //v4sf aa    = mulps(hhalf, nn);
    v4sf rr    = VEC_INSN(rsqrtps, (nn));
    v4sf aa    = accurate_recip(rr);//VEC_INSN(rcpps, (rr));
    //v4sf aa    = VEC_INSN(rcpps, (rr));
    //v4sf aa    = VEC_INSN(rcpps,   (VEC_INSN(rsqrtps, (nn))));  // approx

    for (unsigned i = 0;  i < iter;  ++i) {
        cerr << "iter " << i << " approx " << aa
             << " error " << subps(cc, aa) << endl;

        //rr      = accurate_recip(aa);
        rr      = mulps(rr, nn);
        rr      = addps(aa, rr);
        aa      = mulps(hhalf, rr);
        //rr      = VEC_INSN(rcpps, (aa));
        rr      = accurate_recip(aa);//VEC_INSN(rcpps, (aa));

        //v4sf ee = subps(cc, aa);
        //cerr << "iter " << i << " error " << ee << endl;
    }
#endif

    test1();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Performance Test

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

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

#include <boost/geometry.hpp>
#include <boost/geometry/multi/geometries/multi_geometries.hpp>

#include <boost/foreach.hpp>
#include <boost/timer.hpp>
#include <fstream>

namespace bg = boost::geometry;

template
<
    typename GeometryOut,
    typename Geometry
>
double test_growth(Geometry const& geometry, int n, int d, double distance)
{

    typedef typename bg::coordinate_type<Geometry>::type coordinate_type;
    typedef typename bg::point_type<Geometry>::type point_type;

    // extern int point_buffer_count;
    std::ostringstream complete;
    complete
        << "point_growth"
        << "_" << "r"
        << "_" << n
        << "_" << d
         // << "_" << point_buffer_count
        ;

    //std::cout << complete.str() << std::endl;

    std::ostringstream filename;
    filename << "buffer_" << complete.str() << ".svg";

    std::ofstream svg(filename.str().c_str());

#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
    bg::svg_mapper<point_type> mapper(svg, 500, 500);

    {
        bg::model::box<point_type> box;
        bg::envelope(geometry, box);

        bg::buffer(box, box, distance * 1.01);
        mapper.add(box);
    }
#endif

    bg::strategy::buffer::join_round join_strategy(100);
    bg::strategy::buffer::end_flat end_strategy;
    bg::strategy::buffer::point_circle point_strategy;
    bg::strategy::buffer::side_straight side_strategy;

    typedef bg::strategy::buffer::distance_symmetric<coordinate_type> distance_strategy_type;
    distance_strategy_type distance_strategy(distance);

    std::vector<GeometryOut> buffered;

    bg::buffer(geometry, buffered,
                distance_strategy, side_strategy,
                join_strategy, end_strategy, point_strategy);

    typename bg::default_area_result<GeometryOut>::type area = 0;
    BOOST_FOREACH(GeometryOut const& polygon, buffered)
    {
        area += bg::area(polygon);
    }

#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
    // Map input geometry in green
    mapper.map(geometry, "opacity:0.5;fill:rgb(0,128,0);stroke:rgb(0,128,0);stroke-width:10");

    BOOST_FOREACH(GeometryOut const& polygon, buffered)
    {
        mapper.map(polygon, "opacity:0.4;fill:rgb(255,255,128);stroke:rgb(0,0,0);stroke-width:3");
    }
#endif

    return area;
}

template <typename P>
void test_growth(int n, int distance_count)
{
    srand(int(time(NULL)));
    //std::cout << typeid(bg::coordinate_type<P>::type).name() << std::endl;
    boost::timer t;

    namespace buf = bg::strategy::buffer;
    typedef bg::model::polygon<P> polygon;
    typedef bg::model::multi_point<P> multi_point_type;

    multi_point_type multi_point;
    for (int i = 0; i < n; i++)
    {
        P point(rand() % 100, rand() % 100);
        multi_point.push_back(point);
    }

    //std::cout << bg::wkt(multi_point) << std::endl;

    double previous_area = 0;
    double epsilon = 0.1;
    double distance = 15.0;
    for (int d = 0; d < distance_count; d++, distance += epsilon)
    {
        double area = test_growth<polygon>(multi_point, n, d, distance);
        if (area < previous_area)
        {
            std::cout << "Error: " << area << " < " << previous_area << std::endl
                << " n=" << n << " distance=" << distance
                << bg::wkt(multi_point) << std::endl;
        }
        previous_area = area;
    }
    std::cout << "n=" << n << " time=" << t.elapsed() << std::endl;
}

int main(int, char* [])
{
    for (int i = 5; i <= 50; i++)
    {
        test_growth<bg::model::point<double, 2, bg::cs::cartesian> >(i, 20);
    }

    return 0;
}


#ifndef BART_SRC_MATERIAL_TESTS_MATERIAL_MOCK_HPP_
#define BART_SRC_MATERIAL_TESTS_MATERIAL_MOCK_HPP_

#include <unordered_map>
#include <vector>

#include <deal.II/lac/full_matrix.h>

#include "data/material/material_i.hpp"
#include "test_helpers/gmock_wrapper.h"

namespace bart::data::material {

class MaterialMock : public MaterialI {
 public:
  using int_bool_map = std::unordered_map<int, bool>;
  using int_vector_map = std::unordered_map<int, std::vector<double>>;
  using int_matrix_map = std::unordered_map<int, dealii::FullMatrix<double>>;

  MOCK_METHOD(int_bool_map, GetFissileIDMap, (), (const, override));
  MOCK_METHOD(int_vector_map, GetDiffusionCoef, (), (const, override));
  MOCK_METHOD(int_vector_map, GetSigT, (), (const, override));
  MOCK_METHOD(int_vector_map, GetInvSigT, (), (const, override));
  MOCK_METHOD(int_vector_map, GetQ, (), (const, override));
  MOCK_METHOD(int_vector_map, GetQPerSter, (), (const, override));
  MOCK_METHOD(int_vector_map, GetNuSigF, (), (const, override));
  MOCK_METHOD(int_matrix_map, GetSigS, (), (const, override));
  MOCK_METHOD(int_matrix_map, GetSigSPerSter, (), (const, override));
  MOCK_METHOD(int_matrix_map, GetChiNuSigF, (), (const, override));
  MOCK_METHOD(int_matrix_map, GetChiNuSigFPerSter, (), (const, override));
};

} // namespace bart::data::material

#endif //BART_SRC_MATERIAL_TESTS_MOCK_MATERIAL_PROPERTIES_H_


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  Permission to use, copy, modify, distribute and sell this software
//  and its documentation for any purpose is hereby granted without fee,
//  provided that the above copyright notice appear in all copies and
//  that both that copyright notice and this permission notice appear
//  in supporting documentation.  The authors make no representations
//  about the suitability of this software for any purpose.
//  It is provided "as is" without express or implied warranty.
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_OPERATION_SPARSE_
#define _BOOST_UBLAS_OPERATION_SPARSE_

#include <boost/numeric/ublas/traits.hpp>

// These scaled additions were borrowed from MTL unashamedly.
// But Alexei Novakov had a lot of ideas to improve these. Thanks.

namespace boost { namespace numeric { namespace ublas {

    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, TRI,
                 row_major_tag) {
        typedef M matrix_type;
        typedef TRI triangular_restriction;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

        // ISSUE why is there a dense vector here?
        vector<value_type> temporary (e2 ().size2 ());
        temporary.clear ();
#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, row_major> cm (m.size1 (), m.size2 ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_assign> (cm, prod (e1, e2), row_major_tag ());
#endif
        typename expression1_type::const_iterator1 it1 (e1 ().begin1 ());
        typename expression1_type::const_iterator1 it1_end (e1 ().end1 ());
        while (it1 != it1_end) {
            size_type jb (temporary.size ());
            size_type je (0);
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression1_type::const_iterator2 it2 (it1.begin ());
            typename expression1_type::const_iterator2 it2_end (it1.end ());
#else
            typename expression1_type::const_iterator2 it2 (boost::numeric::ublas::begin (it1, iterator1_tag ()));
            typename expression1_type::const_iterator2 it2_end (boost::numeric::ublas::end (it1, iterator1_tag ()));
#endif
            while (it2 != it2_end) {
                // temporary.plus_assign (*it2 * row (e2 (), it2.index2 ()));
                matrix_row<expression2_type> mr (e2 (), it2.index2 ());
                typename matrix_row<expression2_type>::const_iterator itr (mr.begin ());
                typename matrix_row<expression2_type>::const_iterator itr_end (mr.end ());
                while (itr != itr_end) {
                    size_type j (itr.index ());
                    temporary (j) += *it2 * *itr;
                    jb = (std::min) (jb, j);
                    je = (std::max) (je, j);
                    ++ itr;
                }
                ++ it2;
            }
            for (size_type j = jb; j < je + 1; ++ j) {
                if (temporary (j) != value_type/*zero*/()) {
                    // FIXME we'll need to extend the container interface!
                    // m.push_back (it1.index1 (), j, temporary (j));
                    // FIXME What to do with adaptors?
                    // m.insert (it1.index1 (), j, temporary (j));
                    if (triangular_restriction::other (it1.index1 (), j))
                        m (it1.index1 (), j) = temporary (j);
                    temporary (j) = value_type/*zero*/();
                }
            }
            ++ it1;
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, TRI,
                 column_major_tag) {
        typedef M matrix_type;
        typedef TRI triangular_restriction;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

        // ISSUE why is there a dense vector here?
        vector<value_type> temporary (e1 ().size1 ());
        temporary.clear ();
#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, column_major> cm (m.size1 (), m.size2 ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_assign> (cm, prod (e1, e2), column_major_tag ());
#endif
        typename expression2_type::const_iterator2 it2 (e2 ().begin2 ());
        typename expression2_type::const_iterator2 it2_end (e2 ().end2 ());
        while (it2 != it2_end) {
            size_type ib (temporary.size ());
            size_type ie (0);
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression2_type::const_iterator1 it1 (it2.begin ());
            typename expression2_type::const_iterator1 it1_end (it2.end ());
#else
            typename expression2_type::const_iterator1 it1 (boost::numeric::ublas::begin (it2, iterator2_tag ()));
            typename expression2_type::const_iterator1 it1_end (boost::numeric::ublas::end (it2, iterator2_tag ()));
#endif
            while (it1 != it1_end) {
                // column (m, it2.index2 ()).plus_assign (*it1 * column (e1 (), it1.index1 ()));
                matrix_column<expression1_type> mc (e1 (), it1.index1 ());
                typename matrix_column<expression1_type>::const_iterator itc (mc.begin ());
                typename matrix_column<expression1_type>::const_iterator itc_end (mc.end ());
                while (itc != itc_end) {
                    size_type i (itc.index ());
                    temporary (i) += *it1 * *itc;
                    ib = (std::min) (ib, i);
                    ie = (std::max) (ie, i);
                    ++ itc;
                }
                ++ it1;
            }
            for (size_type i = ib; i < ie + 1; ++ i) {
                if (temporary (i) != value_type/*zero*/()) {
                    // FIXME we'll need to extend the container interface!
                    // m.push_back (i, it2.index2 (), temporary (i));
                    // FIXME What to do with adaptors?
                    // m.insert (i, it2.index2 (), temporary (i));
                    if (triangular_restriction::other (i, it2.index2 ()))
                        m (i, it2.index2 ()) = temporary (i);
                    temporary (i) = value_type/*zero*/();
                }
            }
            ++ it2;
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    // Dispatcher
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, TRI, bool init = true) {
        typedef typename M::value_type value_type;
        typedef TRI triangular_restriction;
        typedef typename M::orientation_category orientation_category;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return sparse_prod (e1, e2, m, triangular_restriction (), orientation_category ());
    }
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 TRI) {
        typedef M matrix_type;
        typedef TRI triangular_restriction;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        // FIXME needed for c_matrix?!
        // return sparse_prod (e1, e2, m, triangular_restriction (), false);
        return sparse_prod (e1, e2, m, triangular_restriction (), true);
    }
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, bool init = true) {
        typedef typename M::value_type value_type;
        typedef typename M::orientation_category orientation_category;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return sparse_prod (e1, e2, m, full (), orientation_category ());
    }
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2) {
        typedef M matrix_type;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        // FIXME needed for c_matrix?!
        // return sparse_prod (e1, e2, m, full (), false);
        return sparse_prod (e1, e2, m, full (), true);
    }

}}}

#endif


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

/* Author: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng */

#include <algorithm>
#include <cmath>
#include <Eigen/Geometry>
#include <limits>
#include <moveit/trajectory_processing/ruckig_traj_smoothing.h>
#include <rclcpp/rclcpp.hpp>
#include <vector>

namespace trajectory_processing
{
namespace
{
const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing");
constexpr double DEFAULT_MAX_VELOCITY = 5;       // rad/s
constexpr double DEFAULT_MAX_ACCELERATION = 10;  // rad/s^2
constexpr double DEFAULT_MAX_JERK = 1000;        // rad/s^3
constexpr double MAX_DURATION_EXTENSION_FACTOR = 10.0;
constexpr double DURATION_EXTENSION_FRACTION = 1.1;
}  // namespace

bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
                                     const double max_velocity_scaling_factor,
                                     const double max_acceleration_scaling_factor)
{
  moveit::core::JointModelGroup const* const group = trajectory.getGroup();
  if (!group)
  {
    RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for");
    return false;
  }

  const size_t num_waypoints = trajectory.getWayPointCount();
  if (num_waypoints < 2)
  {
    RCLCPP_WARN(LOGGER,
                "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
    return true;
  }

  // Cache the trajectory in case we need to reset it
  robot_trajectory::RobotTrajectory original_trajectory =
      robot_trajectory::RobotTrajectory(trajectory, true /* deep copy */);

  const size_t num_dof = group->getVariableCount();

  // This lib does not actually work properly when angles wrap around, so we need to unwind the path first
  trajectory.unwind();

  // Instantiate the smoother
  double timestep = trajectory.getAverageSegmentDuration();
  std::unique_ptr<ruckig::Ruckig<ruckig::DynamicDOFs>> ruckig_ptr;
  ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
  ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
  ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ num_dof };

  // Kinematic limits (vel/accel/jerk)
  const std::vector<std::string>& vars = group->getVariableNames();
  const moveit::core::RobotModel& rmodel = group->getParentModel();
  const std::vector<int>& move_group_idx = group->getVariableIndexList();
  for (size_t i = 0; i < num_dof; ++i)
  {
    const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars.at(i));

    // This assumes min/max bounds are symmetric
    if (bounds.velocity_bounded_)
    {
      ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * bounds.max_velocity_;
    }
    else
    {
      RCLCPP_WARN_STREAM_ONCE(LOGGER,
                              "Joint velocity limits are not defined. Using the default "
                                  << DEFAULT_MAX_VELOCITY
                                  << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml.");
      ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * DEFAULT_MAX_VELOCITY;
    }
    if (bounds.acceleration_bounded_)
    {
      ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * bounds.max_acceleration_;
    }
    else
    {
      RCLCPP_WARN_STREAM_ONCE(LOGGER,
                              "Joint acceleration limits are not defined. Using the default "
                                  << DEFAULT_MAX_ACCELERATION
                                  << " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.");
      ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * DEFAULT_MAX_ACCELERATION;
    }
    ruckig_input.max_jerk.at(i) = bounds.jerk_bounded_ ? bounds.max_jerk_ : DEFAULT_MAX_JERK;
    if (bounds.jerk_bounded_)
    {
      ruckig_input.max_jerk.at(i) = bounds.max_jerk_;
    }
    else
    {
      RCLCPP_WARN_STREAM_ONCE(LOGGER, "Joint jerk limits are not defined. Using the default "
                                          << DEFAULT_MAX_JERK
                                          << " rad/s^3. You can define jerk limits in joint_limits.yaml.");
      ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;
    }
  }

  // Initialize the smoother
  initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);

  ruckig::Result ruckig_result;
  double duration_extension_factor = 1;
  bool smoothing_complete = false;
  while ((duration_extension_factor < MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete)
  {
    for (size_t waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx)
    {
      moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1);

      getNextRuckigInput(trajectory.getWayPointPtr(waypoint_idx), next_waypoint, group, ruckig_input);

      // Run Ruckig
      ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);

      if ((waypoint_idx == num_waypoints - 2) && ruckig_result == ruckig::Result::Finished)
      {
        smoothing_complete = true;
        break;
      }

      // Extend the trajectory duration if Ruckig could not reach the waypoint successfully
      if (ruckig_result != ruckig::Result::Finished)
      {
        duration_extension_factor *= DURATION_EXTENSION_FRACTION;
        // Reset the trajectory
        trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */);
        for (size_t time_stretch_idx = 1; time_stretch_idx < num_waypoints; ++time_stretch_idx)
        {
          trajectory.setWayPointDurationFromPrevious(
              time_stretch_idx,
              duration_extension_factor * original_trajectory.getWayPointDurationFromPrevious(time_stretch_idx));
          // re-calculate waypoint velocity and acceleration
          auto target_state = trajectory.getWayPointPtr(time_stretch_idx);
          const auto prev_state = trajectory.getWayPointPtr(time_stretch_idx - 1);
          timestep = trajectory.getAverageSegmentDuration();
          for (size_t joint = 0; joint < num_dof; ++joint)
          {
            target_state->setVariableVelocity(move_group_idx.at(joint),
                                              (1 / duration_extension_factor) *
                                                  target_state->getVariableVelocity(move_group_idx.at(joint)));

            double prev_velocity = prev_state->getVariableVelocity(move_group_idx.at(joint));
            double curr_velocity = target_state->getVariableVelocity(move_group_idx.at(joint));
            target_state->setVariableAcceleration(move_group_idx.at(joint), (curr_velocity - prev_velocity) / timestep);
          }
          target_state->update();
        }
        ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
        initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
        // Begin the while() loop again
        break;
      }
    }
  }

  if (ruckig_result != ruckig::Result::Finished)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result);
    return false;
  }

  return true;
}

void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<0>& ruckig_input,
                                            ruckig::OutputParameter<0>& ruckig_output,
                                            const moveit::core::RobotState& first_waypoint,
                                            const moveit::core::JointModelGroup* joint_group)
{
  const size_t num_dof = joint_group->getVariableCount();
  const std::vector<int>& idx = joint_group->getVariableIndexList();

  std::vector<double> current_positions_vector(num_dof);
  std::vector<double> current_velocities_vector(num_dof);
  std::vector<double> current_accelerations_vector(num_dof);

  for (size_t i = 0; i < num_dof; ++i)
  {
    current_positions_vector.at(i) = first_waypoint.getVariablePosition(idx.at(i));
    current_velocities_vector.at(i) = first_waypoint.getVariableVelocity(idx.at(i));
    current_accelerations_vector.at(i) = first_waypoint.getVariableAcceleration(idx.at(i));
    // Clamp velocities/accelerations in case they exceed the limit due to small numerical errors
    current_velocities_vector.at(i) =
        std::clamp(current_velocities_vector.at(i), -ruckig_input.max_velocity.at(i), ruckig_input.max_velocity.at(i));
    current_accelerations_vector.at(i) = std::clamp(
        current_accelerations_vector.at(i), -ruckig_input.max_acceleration.at(i), ruckig_input.max_acceleration.at(i));
  }
  std::copy_n(current_positions_vector.begin(), num_dof, ruckig_input.current_position.begin());
  std::copy_n(current_velocities_vector.begin(), num_dof, ruckig_input.current_velocity.begin());
  std::copy_n(current_accelerations_vector.begin(), num_dof, ruckig_input.current_acceleration.begin());
  // Initialize output data struct
  ruckig_output.new_position = ruckig_input.current_position;
  ruckig_output.new_velocity = ruckig_input.current_velocity;
  ruckig_output.new_acceleration = ruckig_input.current_acceleration;
}

void RuckigSmoothing::getNextRuckigInput(const moveit::core::RobotStatePtr& current_waypoint,
                                         const moveit::core::RobotStatePtr& next_waypoint,
                                         const moveit::core::JointModelGroup* joint_group,
                                         ruckig::InputParameter<0>& ruckig_input)
{
  const size_t num_dof = joint_group->getVariableCount();
  const std::vector<int>& idx = joint_group->getVariableIndexList();

  for (size_t joint = 0; joint < num_dof; ++joint)
  {
    ruckig_input.current_position.at(joint) = current_waypoint->getVariablePosition(idx.at(joint));
    ruckig_input.current_velocity.at(joint) = current_waypoint->getVariableVelocity(idx.at(joint));
    ruckig_input.current_acceleration.at(joint) = current_waypoint->getVariableAcceleration(idx.at(joint));

    // Target state is the next waypoint
    ruckig_input.target_position.at(joint) = next_waypoint->getVariablePosition(idx.at(joint));
    ruckig_input.target_velocity.at(joint) = next_waypoint->getVariableVelocity(idx.at(joint));
    ruckig_input.target_acceleration.at(joint) = next_waypoint->getVariableAcceleration(idx.at(joint));

    // Clamp velocities/accelerations in case they exceed the limit due to small numerical errors
    ruckig_input.current_velocity.at(joint) =
        std::clamp(ruckig_input.current_velocity.at(joint), -ruckig_input.max_velocity.at(joint),
                   ruckig_input.max_velocity.at(joint));
    ruckig_input.current_acceleration.at(joint) =
        std::clamp(ruckig_input.current_acceleration.at(joint), -ruckig_input.max_acceleration.at(joint),
                   ruckig_input.max_acceleration.at(joint));
    ruckig_input.target_velocity.at(joint) =
        std::clamp(ruckig_input.target_velocity.at(joint), -ruckig_input.max_velocity.at(joint),
                   ruckig_input.max_velocity.at(joint));
    ruckig_input.target_acceleration.at(joint) =
        std::clamp(ruckig_input.target_acceleration.at(joint), -ruckig_input.max_acceleration.at(joint),
                   ruckig_input.max_acceleration.at(joint));
  }
}
}  // namespace trajectory_processing


// Copyright 2016 Robert Maier, Technical University Munich
#include "dvo.hpp"

#include <iostream>
#include <sstream>
#include <string>

#include <Eigen/Cholesky>
#include <sophus/se3.hpp>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

DVO::DVO() :
    numPyramidLevels_(2),
    useWeights_(true),
    numIterations_(500),
    algo_(LevenbergMarquardt)
    //algo_(GaussNewton)
{
}

DVO::~DVO()
{
    for (int i = 0; i < numPyramidLevels_; ++i)
    {
        delete[] J_[i];
        delete[] residuals_[i];
        delete[] weights_[i];
    }
}

void DVO::init(int w, int h, const Eigen::Matrix3f &K)
{
    // pyramid level size
    int wDown = w;
    int hDown = h;
    int n = wDown*hDown;
    sizePyramid_.push_back(cv::Size(wDown, hDown));

    // gradients
    cv::Mat gradX = cv::Mat::zeros(h, w, CV_32FC1);
    gradX_.push_back(gradX);
    cv::Mat gradY = cv::Mat::zeros(h, w, CV_32FC1);
    gradY_.push_back(gradY);

    // Jacobian
    float* J = new float[n*6];
    J_.push_back(J);
    // residuals
    float* residuals = new float[n];
    residuals_.push_back(residuals);
    // per-residual weights
    float* weights = new float[n];
    weights_.push_back(weights);

    // camera matrix
    kPyramid_.push_back(K);

    for (int i = 1; i < numPyramidLevels_; ++i)
    {
        // pyramid level size
        wDown = wDown / 2;
        hDown = hDown / 2;
        int n = wDown*hDown;
        sizePyramid_.push_back(cv::Size(wDown, hDown));

        // gradients
        cv::Mat gradXdown = cv::Mat::zeros(hDown, wDown, CV_32FC1);
        gradX_.push_back(gradXdown);
        cv::Mat gradYdown = cv::Mat::zeros(hDown, wDown, CV_32FC1);
        gradY_.push_back(gradYdown);

        // Jacobian
        float* J = new float[n*6];
        J_.push_back(J);
        // residuals
        float* residuals = new float[n];
        residuals_.push_back(residuals);
        // per-residual weights
        float* weights = new float[n];
        weights_.push_back(weights);

        // downsample camera matrix
        Eigen::Matrix3f kDown = kPyramid_[i-1];
        kDown(0, 2) += 0.5f;
        kDown(1, 2) += 0.5f;
        kDown.topLeftCorner(2, 3) = kDown.topLeftCorner(2, 3) * 0.5f;
        kDown(0, 2) -= 0.5f;
        kDown(1, 2) -= 0.5f;
        kPyramid_.push_back(kDown);
        //std::cout << "Camera matrix (level " << i << "): " << kDown << std::endl;
    }
}

void DVO::convertSE3ToTf(const Vec6f &xi, Eigen::Matrix3f &rot, Eigen::Vector3f &t)
{
    // rotation
    Sophus::SE3f se3 = Sophus::SE3f::exp(xi);
    Eigen::Matrix4f mat = se3.matrix();
    rot = mat.topLeftCorner(3, 3);
    t = mat.topRightCorner(3, 1);
}

void DVO::convertSE3ToTf(const Vec6f &xi, Eigen::Matrix4f &pose)
{
    Sophus::SE3f se3 = Sophus::SE3f::exp(xi);
    pose = se3.matrix();
}

void DVO::convertTfToSE3(const Eigen::Matrix3f &rot, const Eigen::Vector3f &t, Vec6f &xi)
{
    Sophus::SE3f se3(rot, t);
    xi = Sophus::SE3f::log(se3);
}

void DVO::convertTfToSE3(const Eigen::Matrix4f &pose, Vec6f &xi)
{
    Eigen::Matrix3f rot = pose.topLeftCorner(3, 3);
    Eigen::Vector3f t = pose.topRightCorner(3, 1);
    convertTfToSE3(rot, t, xi);
}

cv::Mat DVO::downsampleGray(const cv::Mat &gray)
{
    const float* ptrIn = (const float*)gray.data;
    int w = gray.cols;
    int h = gray.rows;
    int wDown = w/2;
    int hDown = h/2;

    cv::Mat grayDown = cv::Mat::zeros(hDown, wDown, gray.type());
    float* ptrOut = (float*)grayDown.data;
    for (size_t y = 0; y < hDown; ++y)
    {
        for (size_t x = 0; x < wDown; ++x)
        {
            float sum = 0.0f;
            sum += ptrIn[2*y * w + 2*x] * 0.25f;
            sum += ptrIn[2*y * w + 2*x+1] * 0.25f;
            sum += ptrIn[(2*y+1) * w + 2*x] * 0.25f;
            sum += ptrIn[(2*y+1) * w + 2*x+1] * 0.25f;
            ptrOut[y*wDown + x] = sum;
        }
    }

    return grayDown;
}

cv::Mat DVO::downsampleDepth(const cv::Mat &depth)
{
    const float* ptrIn = (const float*)depth.data;
    int w = depth.cols;
    int h = depth.rows;
    int wDown = w/2;
    int hDown = h/2;

    // downscaling by averaging the inverse depth
    cv::Mat depthDown = cv::Mat::zeros(hDown, wDown, depth.type());
    float* ptrOut = (float*)depthDown.data;
    for (size_t y = 0; y < hDown; ++y)
    {
        for (size_t x = 0; x < wDown; ++x)
        {
            float d0 = ptrIn[2*y * w + 2*x];
            float d1 = ptrIn[2*y * w + 2*x+1];
            float d2 = ptrIn[(2*y+1) * w + 2*x];
            float d3 = ptrIn[(2*y+1) * w + 2*x+1];

            int cnt = 0;
            float sum = 0.0f;
            if (d0 != 0.0f)
            {
                sum += 1.0f / d0;
                ++cnt;
            }
            if (d1 != 0.0f)
            {
                sum += 1.0f / d1;
                ++cnt;
            }
            if (d2 != 0.0f)
            {
                sum += 1.0f / d2;
                ++cnt;
            }
            if (d3 != 0.0f)
            {
                sum += 1.0f / d3;
                ++cnt;
            }

            if (cnt > 0)
            {
                float dInv = sum / float(cnt);
                if (dInv != 0.0f)
                    ptrOut[y*wDown + x] = 1.0f / dInv;
            }
        }
    }

    return depthDown;
}

void DVO::computeGradient(const cv::Mat &gray, cv::Mat &gradient, int direction)
{
    int dirX = 1;
    int dirY = 0;
    if (direction == 1)
    {
        dirX = 0;
        dirY = 1;
    }

    // compute gradient manually using finite differences
    int w = gray.cols;
    int h = gray.rows;
    const float* ptrIn = (const float*)gray.data;
    gradient.setTo(0);
    float* ptrOut = (float*)gradient.data;

    int yStart = dirY;
    int yEnd = h - dirY;
    int xStart = dirX;
    int xEnd = w - dirX;
    for (size_t y = yStart; y < yEnd; ++y)
    {
        for (size_t x = xStart; x < xEnd; ++x)
        {
            float v0;
            float v1;
            if (direction == 1)
            {
                // y-direction
                v0 = ptrIn[(y-1)*w + x];
                v1 = ptrIn[(y+1)*w + x];
            }
            else
            {
                // x-direction
                v0 = ptrIn[y*w + (x-1)];
                v1 = ptrIn[y*w + (x+1)];
            }
            ptrOut[y*w + x] = 0.5f * (v1 - v0);
        }
    }
}

float DVO::interpolate(const float* ptrImgIntensity, float x, float y, int w, int h)
{
    float valCur = std::numeric_limits<float>::quiet_NaN();

#if 0
    // direct lookup, no interpolation
    int x0 = static_cast<int>(x + 0.5f);
    int y0 = static_cast<int>(y + 0.5f);
    if (x0 >= 0 && x0 < w && y0 >= 0 && y0 < h)
        valCur = ptrImgIntensity[y0*w + x0];
#else
    //bilinear interpolation
    int x0 = static_cast<int>(x);
    int y0 = static_cast<int>(y);
    int x1 = x0 + 1;
    int y1 = y0 + 1;

    float x1_weight = x - static_cast<float>(x0);
    float y1_weight = y - static_cast<float>(y0);
    float x0_weight = 1.0f - x1_weight;
    float y0_weight = 1.0f - y1_weight;

    if (x0 < 0 || x0 >= w)
        x0_weight = 0.0f;
    if (x1 < 0 || x1 >= w)
        x1_weight = 0.0f;
    if (y0 < 0 || y0 >= h)
        y0_weight = 0.0f;
    if (y1 < 0 || y1 >= h)
        y1_weight = 0.0f;
    float w00 = x0_weight * y0_weight;
    float w10 = x1_weight * y0_weight;
    float w01 = x0_weight * y1_weight;
    float w11 = x1_weight * y1_weight;

    float sumWeights = w00 + w10 + w01 + w11;
    float sum = 0.0f;
    if (w00 > 0.0f)
        sum += ptrImgIntensity[y0*w + x0] * w00;
    if (w01 > 0.0f)
        sum += ptrImgIntensity[y1*w + x0] * w01;
    if (w10 > 0.0f)
        sum += ptrImgIntensity[y0*w + x1] * w10;
    if (w11 > 0.0f)
        sum += ptrImgIntensity[y1*w + x1] * w11;

    if (sumWeights > 0.0f)
        valCur = sum / sumWeights;
#endif

    return valCur;
}

float DVO::calculateError(const float* residuals, int n)
{
    float error = 0.0f;
    int numValid = 0;
    for (int i = 0; i < n; ++i)
    {
        if (residuals[i] != 0.0f)
        {
            error += residuals[i] * residuals[i];
            ++numValid;
        }
    }
    if (numValid > 0)
        error = error / static_cast<float>(numValid);
    return error;
}

void DVO::calculateErrorImage(const float* residuals, int w, int h, cv::Mat &errorImage)
{
    cv::Mat imgResiduals = cv::Mat::zeros(h, w, CV_32FC1);
    float* ptrResiduals = (float*)imgResiduals.data;

    // fill residuals image
    for (size_t y = 0; y < h; ++y)
    {
        for (size_t x = 0; x < w; ++x)
        {
            size_t off = y*w + x;
            if (residuals[off] != 0.0f)
                ptrResiduals[off] = residuals[off];
        }
    }

    imgResiduals.convertTo(errorImage, CV_8SC1, 127.0);
}

void DVO::calculateError(const cv::Mat &grayRef, const cv::Mat &depthRef,
                         const cv::Mat &grayCur, const cv::Mat &depthCur,
                         const Eigen::VectorXf &xi, const Eigen::Matrix3f &K,
                         float* residuals)
{
    // create residual image
    int w = grayRef.cols;
    int h = grayRef.rows;

    // camera intrinsics
    float fx = K(0, 0);
    float fy = K(1, 1);
    float cx = K(0, 2);
    float cy = K(1, 2);
    float fxInv = 1.0f / fx;
    float fyInv = 1.0f / fy;

    // convert SE3 to rotation matrix and translation vector
    Eigen::Matrix3f rotMat;
    Eigen::Vector3f t;
    convertSE3ToTf(xi, rotMat, t);

    const float* ptrGrayRef = (const float*)grayRef.data;
    const float* ptrDepthRef = (const float*)depthRef.data;
    const float* ptrGrayCur = (const float*)grayCur.data;
    const float* ptrDepthCur = (const float*)depthCur.data;

    for (size_t y = 0; y < h; ++y)
    {
        for (size_t x = 0; x < w; ++x)
        {
            size_t off = y*w + x;
            float residual = 0.0f;

            // project 2d point back into 3d using its depth
            float dRef = ptrDepthRef[y*w + x];
            if (dRef > 0.0)
            {
                float x0 = (static_cast<float>(x) - cx) * fxInv;
                float y0 = (static_cast<float>(y) - cy) * fyInv;
                float scale = 1.0f;
                //scale = std::sqrt(x0*x0 + y0*y0 + 1.0f);
                dRef = dRef * scale;
                x0 = x0 * dRef;
                y0 = y0 * dRef;

                // transform reference 3d point into current frame
                // reference 3d point
                Eigen::Vector3f pt3Ref(x0, y0, dRef);
                Eigen::Vector3f pt3Cur = rotMat * pt3Ref + t;
                if (pt3Cur[2] > 0.0f)
                {
                    // project 3d point to 2d
                    Eigen::Vector3f pt2CurH = K * pt3Cur;
                    float ptZinv = 1.0f / pt2CurH[2];
                    float px = pt2CurH[0] * ptZinv;
                    float py = pt2CurH[1] * ptZinv;

                    // interpolate residual
                    float valCur = interpolate(ptrGrayCur, px, py, w, h);
                    if (!std::isnan(valCur))
                    {
                        float valRef = ptrGrayRef[off];
                        float valDiff = valRef - valCur;
                        residual = valDiff;
                    }
                }
            }
            residuals[off] = residual;
        }
    }
}

void DVO::calculateMeanStdDev(const float* residuals, float &mean, float &stdDev, int n)
{
    float meanVal = 0.0f;
    for (int i = 0; i < n; ++i)
        meanVal += residuals[i];
    mean = meanVal / static_cast<float>(n);

    float variance = 0.0f;
    for (int i = 0; i < n; ++i)
        variance += (residuals[i] - mean) * (residuals[i] - mean);
    stdDev = std::sqrt(variance);
}

void DVO::computeWeights(const float* residuals, float* weights, int n)
{
#if 0
    // no weighting
    for (int i = 0; i < n; ++i)
        weights[i] = 1.0f;
#if 0
    // squared residuals
    for (int i = 0; i < n; ++i)
        residuals[i] = residuals[i] * residuals[i];
    return;
#endif
#endif

    // compute mean and standard deviation
    float mean, stdDev;
    calculateMeanStdDev(residuals, mean, stdDev, n);

    // compute robust Huber weights

    float k = 1.345f * stdDev;
    //float k = 0.15f * stdDev;
    for (int i = 0; i < n; ++i)
    {
        float w;
        if (std::abs(residuals[i]) <= k)
            w = 1.0f;
        else
            w = k / std::abs(residuals[i]);
        weights[i] = w;
    }
    /*
    #if 0
    // compute Tukey weights
    float k = 4.6851f * stdDev;
    for (int i = 0; i<n; i++)
    {
        float w;
        if (std::abs(residuals[i]) <= k)
            w = std::pow((1.0f-residuals[i]*residuals[i] / (k*k)),2);
        else
            w = 0.0f;
        weights[i] = w;
    }

    // compute Cauchy weights
    float k = 1.35f * stdDev;
    for (int i = 0; i<n; i++)
    {
        float w;
        w = 1.0f/(1+(residuals[i]*residuals[i] / (k*k)));
        weights[i] = w;
    }
    */

}

void DVO::applyWeights(const float* weights, float* residuals, int n)
{
    for (size_t i = 0; i < n; ++i)
    {
        // weight residual
        residuals[i] = residuals[i] * weights[i];
    }
}

void DVO::deriveNumeric(const cv::Mat &grayRef, const cv::Mat &depthRef,
                                  const cv::Mat &grayCur, const cv::Mat &depthCur,
                                  const Eigen::VectorXf &xi, const Eigen::Matrix3f &K,
                                  float* residuals, float* J)
{
    float epsilon = 1e-6;
    float scale = 1.0f / epsilon;

    int w = grayRef.cols;
    int h = grayRef.rows;
    int n = w*h;

    // calculate per-pixel residuals
    calculateError(grayRef, depthRef, grayCur, depthCur, xi, K, residuals);

    // create and fill Jacobian column by column
    float* residualsInc = new float[n];
    for (int j = 0; j < 6; ++j)
    {
        Eigen::VectorXf unitVec = Eigen::VectorXf::Zero(6);
        unitVec[j] = epsilon;

        // left-multiplicative increment on SE3
        Eigen::VectorXf xiEps = Sophus::SE3f::log(Sophus::SE3f::exp(unitVec) * Sophus::SE3f::exp(xi));

        calculateError(grayRef, depthRef, grayCur, depthCur, xiEps, K, residualsInc);
        for (int i = 0; i < n; ++i)
            J[i*6 + j] = (residualsInc[i] - residuals[i]) * scale;
    }
    delete[] residualsInc;
}

void DVO::compute_JtR(const float* J, const float* residuals, Vec6f &b, int validRows)
{
    int n = 6;
    int m = validRows;

    // compute b = Jt*r
    for (int j = 0; j < n; ++j)
    {
        float val = 0.0f;
        for (int i = 0; i < m; ++i)
            val += J[i*6 + j] * residuals[i];
        b[j] = val;
    }
}

void DVO::compute_JtJ(const float* J, Mat6f &A, const float* weights, int validRows, bool useWeights)
{
    int n = 6;
    int m = validRows;

    // compute A = Jt*J
    for (int k = 0; k < n; ++k)
    {
        for (int j = 0; j < n; ++j)
        {
            float val = 0.0f;
            for (int i = 0; i < m; ++i)
            {
                float valSqr = J[i*6 + j] * J[i*6 + k];
                if (useWeights)
                    valSqr *= weights[i];
                val += valSqr;
            }
            A(k, j) = val;
        }
    }
}

void DVO::deriveAnalytic(const cv::Mat &grayRef, const cv::Mat &depthRef,
                   const cv::Mat &grayCur, const cv::Mat &depthCur,
                   const cv::Mat &gradX, const cv::Mat &gradY,
                   const Eigen::VectorXf &xi, const Eigen::Matrix3f &K,
                   float* residuals, float* J)
{
    // reference input images
    int w = grayRef.cols;
    int h = grayRef.rows;
    int n = w*h;
    const float* ptrDepthRef = (const float*)depthRef.data;

    // camera intrinsics
    float fx = K(0, 0);
    float fy = K(1, 1);
    float cx = K(0, 2);
    float cy = K(1, 2);
    float fxInv = 1.0f / fx;
    float fyInv = 1.0f / fy;

    // convert SE3 to rotation matrix and translation vector
    Eigen::Matrix3f rotMat;
    Eigen::Vector3f t;
    convertSE3ToTf(xi, rotMat, t);

    // calculate per-pixel residuals
    calculateError(grayRef, depthRef, grayCur, depthCur, xi, K, residuals);

    // reference gradient images
    const float* ptrGradX = (const float*)gradX.data;
    const float* ptrGradY = (const float*)gradY.data;

    // create and fill Jacobian row by row
    float residualRowJ[6];
    for (size_t y = 0; y < h; ++y)
    {
        for (size_t x = 0; x < w; ++x)
        {
            size_t off = y*w + x;

            // project 2d point back into 3d using its depth
            float dRef = ptrDepthRef[y*w + x];
            if (dRef > 0.0f)
            {
                float x0 = (static_cast<float>(x) - cx) * fxInv;
                float y0 = (static_cast<float>(y) - cy) * fyInv;
                float scale = 1.0f;
                //scale = std::sqrt(x0*x0 + y0*y0 + 1.0);
                dRef = dRef * scale;
                x0 = x0 * dRef;
                y0 = y0 * dRef;

                // transform reference 3d point into current frame
                // reference 3d point
                Eigen::Vector3f pt3Ref(x0, y0, dRef);
                Eigen::Vector3f pt3 = rotMat * pt3Ref + t;
                if (pt3[2] > 0.0f)
                {
                    // project 3d point to 2d
                    Eigen::Vector3f pt2CurH = K * pt3;
                    float ptZinv = 1.0f / pt2CurH[2];
                    float px = pt2CurH[0] * ptZinv;
                    float py = pt2CurH[1] * ptZinv;

                    // compute interpolated image gradient
                    float dX = interpolate(ptrGradX, px, py, w, h);
                    float dY = interpolate(ptrGradY, px, py, w, h);
                    if (!std::isnan(dX) && !std::isnan(dY))
                    {
                        dX = fx * dX;
                        dY = fy * dY;
                        float pt3Zinv = 1.0f / pt3[2];

                        // shorter computation
                        residualRowJ[0] = dX * pt3Zinv;
                        residualRowJ[1] = dY * pt3Zinv;
                        residualRowJ[2] = - (dX * pt3[0] + dY * pt3[1]) * pt3Zinv * pt3Zinv;
                        residualRowJ[3] = - (dX * pt3[0] * pt3[1]) * pt3Zinv * pt3Zinv - dY * (1 + (pt3[1] * pt3Zinv) * (pt3[1] * pt3Zinv));
                        residualRowJ[4] = + dX * (1.0 + (pt3[0] * pt3Zinv) * (pt3[0] * pt3Zinv)) + (dY * pt3[0] * pt3[1]) * pt3Zinv * pt3Zinv;
                        residualRowJ[5] = (- dX * pt3[1] + dY * pt3[0]) * pt3Zinv;
                    }
                }
            }

            // set 1x6 Jacobian row for current residual
            // invert Jacobian according to kerl2012msc.pdf (necessary?)
            for (int j = 0; j < 6; ++j)
                J[off*6 + j] = - residualRowJ[j];
        }
    }
}

void DVO::buildPyramid(const cv::Mat &depth, const cv::Mat &gray, std::vector<cv::Mat> &depthPyramid, std::vector<cv::Mat> &grayPyramid)
{
    grayPyramid.push_back(gray);
    depthPyramid.push_back(depth);

    for (int i = 1; i < numPyramidLevels_; ++i)
    {
        // downsample grayscale image
        cv::Mat grayDown = downsampleGray(grayPyramid[i-1]);
        grayPyramid.push_back(grayDown);

        // downsample depth image
        cv::Mat depthDown = downsampleDepth(depthPyramid[i-1]);
        depthPyramid.push_back(depthDown);
    }
}

void DVO::align(const cv::Mat &depthRef, const cv::Mat &grayRef, const cv::Mat &depthCur, const cv::Mat &grayCur, Eigen::Matrix4f &pose)
{
    // downsampling
    std::vector<cv::Mat> grayRefPyramid;
    std::vector<cv::Mat> depthRefPyramid;
    buildPyramid(depthRef, grayRef, depthRefPyramid, grayRefPyramid);

    std::vector<cv::Mat> grayCurPyramid;
    std::vector<cv::Mat> depthCurPyramid;
    buildPyramid(depthCur, grayCur, depthCurPyramid, grayCurPyramid);

    align(depthRefPyramid, grayRefPyramid, depthCurPyramid, grayCurPyramid, pose);
}

void DVO::align(const std::vector<cv::Mat> &depthRefPyramid, const std::vector<cv::Mat> &grayRefPyramid,
                const std::vector<cv::Mat> &depthCurPyramid, const std::vector<cv::Mat> &grayCurPyramid,
                Eigen::Matrix4f &pose)
{
    Vec6f xi;
    convertTfToSE3(pose, xi);

    Vec6f lastXi = Vec6f::Zero();

    int maxLevel = numPyramidLevels_-1;
    int minLevel = 1;
    float initGradDescStepSize = 1e-3f;
    float gradDescStepSize = initGradDescStepSize;

    Mat6f A;
    Mat6f diagMatA = Mat6f::Identity();
    Vec6f delta;

    for (int lvl = maxLevel; lvl >= minLevel; --lvl)
    {
        float lambda = 0.1f;

        int w = sizePyramid_[lvl].width;
        int h = sizePyramid_[lvl].height;
        int n = w*h;

        cv::Mat grayRef = grayRefPyramid[lvl];
        cv::Mat depthRef = depthRefPyramid[lvl];
        cv::Mat grayCur = grayCurPyramid[lvl];
        cv::Mat depthCur = depthCurPyramid[lvl];
        Eigen::Matrix3f kLevel = kPyramid_[lvl];
        //std::cout << "level " << level << " (size " << depthRef.cols << "x" << depthRef.rows << ")" << std::endl;

        // compute gradient images
        computeGradient(grayCur, gradX_[lvl], 0);
        computeGradient(grayCur, gradY_[lvl], 1);

        float errorLast = std::numeric_limits<float>::max();
        for (int itr = 0; itr < numIterations_; ++itr)
        {
            // compute residuals and Jacobian
#if 0
            deriveNumeric(grayRef, depthRef, grayCur, depthCur, xi, kLevel, residuals_[lvl], J_[lvl]);
#else
            deriveAnalytic(grayRef, depthRef, grayCur, depthCur, gradX_[lvl], gradY_[lvl], xi, kLevel, residuals_[lvl], J_[lvl]);
#endif

#if 0
            // compute and show error image
            cv::Mat errorImage;
            calculateErrorImage(residuals_[level], grayRef.cols, grayRef.rows, errorImage);
            std::stringstream ss;
            ss << dataFolder << "residuals_" << level << "_";
            ss << std::setw(2) << std::setfill('0') << itr << ".png";
            cv::imwrite(ss.str(), errorImage);
            cv::imshow("error", errorImage);
            cv::waitKey(100);
#endif

            // calculate error
            float error = calculateError(residuals_[lvl], n);

            if (useWeights_)
            {
                // compute robust weights
                computeWeights(residuals_[lvl], weights_[lvl], n);
                // apply robust weights
                applyWeights(weights_[lvl], residuals_[lvl], n);
            }

            // compute update
            Vec6f b;
            compute_JtR(J_[lvl], residuals_[lvl], b, n);

            if (algo_ == GradientDescent)
            {
                // Gradient Descent
                delta = -gradDescStepSize * b * (1.0f / b.norm());
            }
            else if (algo_ == GaussNewton)
            {
                // Gauss-Newton algorithm
                compute_JtJ(J_[lvl], A, weights_[lvl], n, useWeights_);
                // solve using Cholesky LDLT decomposition
                delta = -(A.ldlt().solve(b));
            }
            else if (algo_ == LevenbergMarquardt)
            {
                // Levenberg-Marquardt algorithm
                compute_JtJ(J_[lvl], A, weights_[lvl], n, useWeights_);
                diagMatA.diagonal() = lambda * A.diagonal();
                delta = -((A + diagMatA).ldlt().solve(b));
            }

            // apply update: left-multiplicative increment on SE3
            lastXi = xi;
            xi = Sophus::SE3f::log(Sophus::SE3f::exp(delta) * Sophus::SE3f::exp(xi));
#if 0
            std::cout << "delta = " << delta.transpose() << " size = " << delta.rows() << " x " << delta.cols() << std::endl;
            std::cout << "xi = " << xi.transpose() << std::endl;
#endif

            // compute error again
            error = calculateError(residuals_[lvl], n);

            if (algo_ == LevenbergMarquardt)
            {
                if (error >= errorLast)
                {
                    lambda = lambda * 5.0f;
                    xi = lastXi;

                    if (lambda > 5.0f)
                        break;
                }
                else
                {
                    lambda = lambda / 1.5f;
                }
            }
            else if (algo_ == GaussNewton)
            {
                // break if no improvement (0.99 or 0.995)
                if (error / errorLast > 0.995f)
                    break;
            }
            else if (algo_ == GradientDescent)
            {
                if (error >= errorLast)
                {
                    gradDescStepSize = gradDescStepSize * 0.5f;
                    if (gradDescStepSize <= initGradDescStepSize * 0.01f)
                        gradDescStepSize = initGradDescStepSize * 0.01f;
                    xi = lastXi;
                }
                else
                {
                    gradDescStepSize = gradDescStepSize * 2.0f;
                    if (gradDescStepSize >= initGradDescStepSize * 100.0f)
                        gradDescStepSize = initGradDescStepSize * 100.0f;

                    // break if no improvement (0.99 or 0.995)
                    if (error / errorLast > 0.995f)
                        break;
                }
            }

            errorLast = error;
        }
    }

    // store to output pose
    convertSE3ToTf(xi, pose);
}


#define BOOST_TEST_DYN_LINK

#include <iostream>
#include <boost/test/unit_test.hpp>
#include <fstream>
#include <string>
#include "vm_context.hpp"
#include "std.hpp"
#include "std/integral.cpp"

using namespace std;
using namespace cuttle::vm;

struct context_fixture {
    context_t context;

    void setup() {
        populate(context);
    }
};

BOOST_FIXTURE_TEST_SUITE(integral_plus_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val1 = { { type_id::integral },{ context.gc.add(new integral_t{ 1 }) } };
		value_t val2 = { {type_id::integral}, {context.gc.add(new integral_t{ 2 })} };
		value_t expect = { {type_id::integral}, {context.gc.add(new integral_t{ 3 })} };
		value_t ret;

		integral_plus_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case2) {
		value_t val1 = { { type_id::integral },{ context.gc.add(new integral_t{ 100 }) } };
		value_t val2 = { { type_id::integral },{ context.gc.add(new integral_t{ 3 }) } };
		value_t expect = { { type_id::integral },{ context.gc.add(new integral_t{ 103 }) } };
		value_t ret;

		integral_plus_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(integral_minus_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val1 = { { type_id::integral },{ context.gc.add(new integral_t{ 1 }) } };
		value_t val2 = { { type_id::integral },{ context.gc.add(new integral_t{ 2 }) } };
		value_t expect = { { type_id::integral },{ context.gc.add(new integral_t{ -1 }) } };
		value_t ret;

		integral_minus_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case2) {
		value_t val1 = { { type_id::integral },{ context.gc.add(new integral_t{ 100 }) } };
		value_t val2 = { { type_id::integral },{ context.gc.add(new integral_t{ 3 }) } };
		value_t expect = { { type_id::integral },{ context.gc.add(new integral_t{ 97 }) } };
		value_t ret;

		integral_minus_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(integral_multiply_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val1 = { { type_id::integral },{ context.gc.add(new integral_t{ 1 }) } };
		value_t val2 = { { type_id::integral },{ context.gc.add(new integral_t{ 2 }) } };
		value_t expect = { { type_id::integral },{ context.gc.add(new integral_t{ 2 }) } };
		value_t ret;

		integral_multiply_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case2) {
		value_t val1 = { { type_id::integral },{ context.gc.add(new integral_t{ 100 }) } };
		value_t val2 = { { type_id::integral },{ context.gc.add(new integral_t{ 0 }) } };
		value_t expect = { { type_id::integral },{ context.gc.add(new integral_t{ 0 }) } };
		value_t ret;

		integral_multiply_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(integral_divide_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val1 = { { type_id::integral },{ context.gc.add(new integral_t{ 1 }) } };
		value_t val2 = { { type_id::integral },{ context.gc.add(new integral_t{ 2 }) } };
		value_t expect = { { type_id::integral },{ context.gc.add(new integral_t{ 0 }) } };
		value_t ret;

		integral_divide_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case2) {
		value_t val1 = { { type_id::integral },{ context.gc.add(new integral_t{ 29 }) } };
		value_t val2 = { { type_id::integral },{ context.gc.add(new integral_t{ 5 }) } };
		value_t expect = { { type_id::integral },{ context.gc.add(new integral_t{ 5 }) } };
		value_t ret;

		integral_divide_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(string_to_integral_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val = { { type_id::string } };
		val.data.string = context.gc.add_r(new std::string{ "31" });
		value_t expect = { { type_id::integral },{ context.gc.add(new integral_t{ 31 }) } };
		value_t ret;

		string_to_integral_func(context, { val }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case2) {
		value_t val = { { type_id::string } };
		val.data.string = context.gc.add_r(new std::string{ "3" });
		value_t expect = { { type_id::integral },{ context.gc.add(new integral_t{ 3 }) } };
		value_t ret;

		string_to_integral_func(context, { val }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case3) {
		value_t val = { { type_id::string } };
		val.data.string = context.gc.add_r(new std::string{ "ads" });
		value_t ret;

		BOOST_CHECK_THROW(string_to_integral_func(context, { val }, ret), std::invalid_argument);
	}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(real_to_integral_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val = { { type_id::integral } };
		val.data.real = context.gc.add_r(new real_t{ 3.21 });
		value_t expect = { { type_id::integral },{ context.gc.add(new integral_t{ 3 }) } };
		value_t ret;

		real_to_integral_func(context, { val }, ret);

		BOOST_CHECK(ret == expect);
	}

BOOST_AUTO_TEST_SUITE_END()


#ifndef __solver_steps_h__
#define __solver_steps_h__

#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include "weighted_hqp/HCod.hpp"
#include "weighted_hqp/InitSet.hpp"
#include <algorithm>    // std::set_difference, std::sort

namespace hcod{
    class Step_length{
        public:
            Step_length();
            ~Step_length(){};

        private:
            void check_bound(const double & Ax1, const Eigen::VectorXd &b, const int& typ, double& violval, int& violtype, const double & THR = 1e-8);

        public:
            void setProblem(const Eigen::VectorXd &x0, const Eigen::VectorXd &x1, const std::vector<h_structure> &h, const Eigen::MatrixXd & Y){
                h_ = h;
                Y_ = Y;
                x0_ = x0;
                x1_ = x1;

                nh_ = Y.cols();
                p_ = h.size();
            }
            void compute();
            Eigen::VectorXi getcst(){
                return cst_;
            }
            double gettau(){
                return tau_;
            }
            bool isviolation(){
                return viol_;
            }

        private:
           std::vector<h_structure> h_;
           Eigen::MatrixXd Y_;
           Eigen::Vector3i cst_;
           Eigen::VectorXd x0_, x1_;
           double tau_;
           bool viol_;
           long int nh_, p_;
           double THR_;

    };

}

#endif


/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#include <cap/mp_values.h>
#include <cap/energy_storage_device.h>
#include <deal.II/base/types.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/base/quadrature_lib.h>
#include <iostream>

namespace cap
{

// reads database for finite element model and write database for equivalent
// circuit model
void compute_equivalent_circuit(
    boost::property_tree::ptree const &input_database,
    boost::property_tree::ptree &output_database)
{
  // TODO: of course we could clear the database or just overwrite but for
  // now let's just throw an exception if it is not empty
  if (!output_database.empty())
    throw std::runtime_error("output_database was not empty...");

  auto to_meters = [](double const &cm)
  {
    return 0.01 * cm;
  };
  auto to_square_meters = [](double const &cm2)
  {
    return 0.0001 * cm2;
  };

  double const cross_sectional_area =
      to_square_meters(input_database.get<double>("geometry.geometric_area"));
  // clang-format off
  double const electrode_width = to_meters(input_database.get<double>("geometry.anode_electrode_thickness"));
  double const separator_width = to_meters(input_database.get<double>("geometry.separator_thickness"      ));
  double const collector_width = to_meters(input_database.get<double>("geometry.anode_collector_thickness"));
  // clang-format on

  // getting the material parameters values
  std::shared_ptr<boost::property_tree::ptree> material_properties_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database.get_child("material_properties"));
  MPValuesParameters<2> mp_values_params(material_properties_database);
  std::shared_ptr<boost::property_tree::ptree> geometry_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database.get_child("geometry"));
  // build dummy cell iterator and set its material id. Because we use a dummy
  // triangulation, we can use MPI_COMM_WORLD.
  std::shared_ptr<dealii::distributed::Triangulation<2>> triangulation(
      new dealii::distributed::Triangulation<2>(MPI_COMM_WORLD));
  dealii::GridGenerator::hyper_cube(*triangulation);
  dealii::FE_Q<2> fe(1);
  dealii::DoFHandler<2> dof_handler(*triangulation);
  dof_handler.distribute_dofs(fe);
  mp_values_params.geometry = std::make_shared<Geometry<2>>(
      triangulation,
      std::make_shared<std::unordered_map<
          std::string, std::set<dealii::types::material_id>>>(
          std::initializer_list<std::pair<
              std::string const, std::set<dealii::types::material_id>>>{
              {"anode", std::set<dealii::types::material_id>{1}},
              {"separator", std::set<dealii::types::material_id>{2}},
              {"cathode", std::set<dealii::types::material_id>{3}},
              {"collector", std::set<dealii::types::material_id>{4, 5}}}),
      std::make_shared<std::unordered_map<
          std::string, std::set<dealii::types::material_id>>>(
          std::initializer_list<std::pair<
              std::string const, std::set<dealii::types::boundary_id>>>{
              {"anode", std::set<dealii::types::boundary_id>{1}},
              {"cathode", std::set<dealii::types::boundary_id>{2}}}));

  std::shared_ptr<MPValues<2>> mp_values =
      std::make_shared<SuperCapacitorMPValues<2>>(mp_values_params);
  dealii::DoFHandler<2>::active_cell_iterator cell = dof_handler.begin_active();
  dealii::FEValues<2> fe_values(fe, dealii::QGauss<2>(1),
                                dealii::update_default);
  fe_values.reinit(cell);
  // electrode
  cell->set_material_id(1); // <- matches the anode material_id in the
                            //    initializer list
  std::vector<double> electrode_solid_electrical_conductivity_values(1);
  std::vector<double> electrode_liquid_electrical_conductivity_values(1);
  mp_values->get_values("solid_electrical_conductivity", fe_values,
                        electrode_solid_electrical_conductivity_values);
  mp_values->get_values("liquid_electrical_conductivity", fe_values,
                        electrode_liquid_electrical_conductivity_values);
  double const electrode_resistivity =
      (1.0 / electrode_solid_electrical_conductivity_values[0] +
       1.0 / electrode_liquid_electrical_conductivity_values[0] +
       1.0 / (electrode_solid_electrical_conductivity_values[0] +
              electrode_liquid_electrical_conductivity_values[0])) /
      3.0;
  double const electrode_resistance =
      electrode_resistivity * electrode_width / cross_sectional_area;
  std::vector<double> electrode_specific_capacitance_values(1);
  mp_values->get_values("specific_capacitance", fe_values,
                        electrode_specific_capacitance_values);
  double const electrode_capacitance =
      electrode_specific_capacitance_values[0] * electrode_width *
      cross_sectional_area;
  std::vector<double> electrode_exchange_current_density_values(1);
  mp_values->get_values("faradaic_reaction_coefficient", fe_values,
                        electrode_exchange_current_density_values);
  double const electrode_leakage_resistance =
      1.0 / (electrode_exchange_current_density_values[0] * electrode_width *
             cross_sectional_area);
  std::cout << "ELECTRODE\n";
  std::cout << "    specific_capacitance="
            << electrode_specific_capacitance_values[0] << "\n";
  std::cout << "    solid_electrical_conductivity="
            << electrode_solid_electrical_conductivity_values[0] << "\n";
  std::cout << "    liquid_electrical_conductivity="
            << electrode_liquid_electrical_conductivity_values[0] << "\n";
  std::cout << "    exchange_current_density="
            << electrode_exchange_current_density_values[0] << "\n";
  std::cout << "    width=" << electrode_width << "\n";
  std::cout << "    cross_sectional_area=" << cross_sectional_area << "\n";
  // separator
  cell->set_material_id(2); // <- matches the separator material_id in the
                            //    initializer list
  std::vector<double> separator_liquid_electrical_conductivity_values(1);
  mp_values->get_values("liquid_electrical_conductivity", fe_values,
                        separator_liquid_electrical_conductivity_values);
  double const separator_resistivity =
      1.0 / separator_liquid_electrical_conductivity_values[0];
  double const separator_resistance =
      separator_resistivity * separator_width / cross_sectional_area;
  std::cout << "SEPARATOR\n";
  std::cout << "    liquid_electrical_conductivity="
            << separator_liquid_electrical_conductivity_values[0] << "\n";
  std::cout << "    width=" << separator_width << "\n";
  std::cout << "    cross_sectional_area=" << cross_sectional_area << "\n";
  // collector
  cell->set_material_id(4); // <- matches the collector  material_id in the
                            //    initializer list
  std::vector<double> collector_solid_electrical_conductivity_values(1);
  mp_values->get_values("solid_electrical_conductivity", fe_values,
                        collector_solid_electrical_conductivity_values);
  double const collector_resistivity =
      1.0 / collector_solid_electrical_conductivity_values[0];
  double const collector_resistance =
      collector_resistivity * collector_width / cross_sectional_area;
  std::cout << "COLLECTOR\n";
  std::cout << "    solid_electrical_conductivity="
            << collector_solid_electrical_conductivity_values[0] << "\n";
  std::cout << "    width=" << collector_width << "\n";
  std::cout << "    cross_sectional_area=" << cross_sectional_area << "\n";

  std::cout << "electrode_capacitance=" << electrode_capacitance << "\n";
  std::cout << "electrode_resistance=" << electrode_resistance << "\n";
  std::cout << "electrode_leakage_resistance=" << electrode_leakage_resistance
            << "\n";
  std::cout << "separator_resistance=" << separator_resistance << "\n";
  std::cout << "collector_resistance=" << collector_resistance << "\n";

  // compute the effective resistance and capacitance
  double const sandwich_capacitance = electrode_capacitance / 2.0;
  double const sandwich_resistance = 2.0 * electrode_resistance +
                                     separator_resistance +
                                     2.0 * collector_resistance;
  double const sandwich_leakage_resistance = 2.0 * electrode_leakage_resistance;
  std::cout << "sandwich_capacitance=" << sandwich_capacitance << "\n";
  std::cout << "sandwich_resistance=" << sandwich_resistance << "\n";
  std::cout << "sandwich_leakage_resistance=" << sandwich_leakage_resistance
            << "\n";

  output_database.put("capacitance", sandwich_capacitance);
  output_database.put("series_resistance", sandwich_resistance);
  output_database.put("parallel_resistance", sandwich_leakage_resistance);
  if (std::isfinite(sandwich_leakage_resistance))
    output_database.put("type", "ParallelRC");
  else
    output_database.put("type", "SeriesRC");
}

class EquivalentCircuitBuilder : public EnergyStorageDeviceBuilder
{
public:
  EquivalentCircuitBuilder()
  {
    register_energy_storage_device("EquivalentCircuit", this);
  }
  std::unique_ptr<EnergyStorageDevice>
  build(boost::property_tree::ptree const &ptree,
        boost::mpi::communicator const &comm) override
  {
    boost::property_tree::ptree other;
    compute_equivalent_circuit(ptree, other);
    return EnergyStorageDevice::build(other, comm);
  }
} global_EquivalentCircuitBuilder;

} // end namespace cap


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <mmtbx/bulk_solvent/mosaic.h>
#include <mmtbx/f_model/f_model.h>
#include <mmtbx/bulk_solvent/bulk_solvent.h>

#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/list.hpp>

namespace mmtbx { namespace mosaic {
namespace {

  void init_module()
  {
    using namespace boost::python;
    using boost::python::arg;
    typedef return_value_policy<return_by_value> rbv;

  class_<alg2_tg<> >(
      "alg2_tg")
      .def(init<
         boost::python::list   const&,
         af::const_ref<double> const&>(
             (arg("F"),
              arg("i_obs"))))
      .def("target",   &alg2_tg<>::target)
      .def("gradient", &alg2_tg<>::gradient)
      .def("update",   &alg2_tg<>::update)
   ;

   def("alg4",
      (af::shared<double>(*)
        (boost::python::list                  const& F,
         af::const_ref<double>                const& f_obs,
         af::const_ref<std::complex<double> > const& phase_source,
         int                                  const& max_cycles,
         double                               const& auto_converge_eps
         )) alg4);
   ;

  }

} // namespace <anonymous>
}} // namespace mmtbx::mosaic

BOOST_PYTHON_MODULE(mmtbx_mosaic_ext)
{
  mmtbx::mosaic::init_module();
}


/*
 * Sampling.cpp
 *
 *  Created on: Mar 25, 2012
 *      Author: david
 */

//------------------------------------------------------------------------------
#include <color.hpp>
#include "Sampling.hpp"
#include "../Superpixels.hpp"
#include <pds/Tools.hpp>
#include <density/ScalePyramid.hpp>
#include <Slimage/Paint.hpp>
#include <functional>
#include <boost/random.hpp>
#include <boost/math/constants/constants.hpp>
#include <cmath>

#define CREATE_DEBUG_IMAGES

#ifdef CREATE_DEBUG_IMAGES
	#include <fstream>
	#include <boost/format.hpp>
#endif

//------------------------------------------------------------------------------
namespace dasp {
//------------------------------------------------------------------------------

#ifdef CREATE_DEBUG_IMAGES

template<unsigned int Q>
Eigen::MatrixXf CombineMipmaps(const std::vector<Eigen::MatrixXf>& mm)
{
	Eigen::MatrixXf r = Eigen::MatrixXf::Zero(Q*3*mm[0].rows()/2, Q*mm[0].cols());
	r.block(0, 0, Q*mm[0].rows(), Q*mm[0].cols()) = density::ScaleUp(mm[0], Q);
	unsigned int y = 0;
	for(unsigned int i=1; i<mm.size(); ++i) {
		r.block(Q*mm[0].rows(), y, Q*mm[i].rows(), Q*mm[i].cols()) = density::ScaleUp(mm[i], Q);
		y += Q*mm[i].cols();
	}
	return r;
}

void DebugShowMatrix(const Eigen::MatrixXf& mat, const std::string& tag)
{
	const float range = 5000.0f / static_cast<float>((640*480)/25);
	sDebugImages[tag] = slimage::Ptr(
			common::MatrixToImage(mat,
	 			std::bind(&common::IntensityColor, std::placeholders::_1, 0.0f, range)));
}

void DebugWriteMatrix(const Eigen::MatrixXf& mat, const std::string& tag)
{
	std::ofstream ofs(tag);
	for(int i=0; i<mat.rows(); i++) {
		for(int j=0; j<mat.cols(); j++) {
			ofs << mat(i,j);
			if(j+1 != mat.cols()) {
				ofs << "\t";
			}
		}
		if(i+1 != mat.rows()) {
			ofs << "\n";
		}
	}
}

template<unsigned int Q>
void DebugMipmap(const std::vector<Eigen::MatrixXf>& mipmaps, const std::string& tag)
{
	// boost::format fmt(tag + "_%2d");
	// for(std::size_t i=0; i<mipmaps.size(); ++i) {
	// 	const float range = 3000.0f / static_cast<float>(mipmaps[i].rows() * mipmaps[i].cols());
	// 	Eigen::MatrixXf scl = density::ScaleUp(mipmaps[i], ((Q==2) ? 1 : Q)*(1<<i));
	// 	sDebugImages[(fmt % i).str()] = slimage::Ptr(
	// 		common::MatrixToImage(scl,
	// 			std::bind(&common::IntensityColor, std::placeholders::_1, 0.0f, range)));
	// }
	DebugShowMatrix(CombineMipmaps<Q>(mipmaps), tag);
}

template<unsigned int Q>
void DebugMipmapDelta(const std::vector<Eigen::MatrixXf>& mipmaps, const std::string& tag)
{
	// boost::format fmt(tag + "_%2d");
	// for(std::size_t i=0; i<mipmaps.size(); ++i) {
	// 	const float range = 3000.0f / static_cast<float>(mipmaps[i].rows() * mipmaps[i].cols());
	// 	Eigen::MatrixXf scl = density::ScaleUp(mipmaps[i], ((Q==2) ? 1 : Q)*(1<<i));
	// 	sDebugImages[(fmt % i).str()] = slimage::Ptr(
	// 		common::MatrixToImage(scl,
	// 			std::bind(&common::PlusMinusColor, std::placeholders::_1, range)));
	// }
	const float range = 2500.0f / static_cast<float>((640*480)/25);
	sDebugImages[tag] = slimage::Ptr(
	 		common::MatrixToImage(CombineMipmaps<Q>(mipmaps),
	 			std::bind(&common::PlusMinusColor, std::placeholders::_1, range)));
}

#endif

Eigen::MatrixXf ComputeDepthDensity(const ImagePoints& points, const Parameters& opt)
{
	constexpr float NZ_MIN = 0.174f; // = std::sin(80 deg)

	Eigen::MatrixXf density(points.width(), points.height());
	float* p_density = density.data();
	for(unsigned int i=0; i<points.size(); i++) {
		const Point& p = points[i];
		/** Estimated number of super pixels at this point
		 * We assume circular superpixels. So the area A of a superpixel at
		 * point location is R*R*pi and the superpixel density is 1/A.
		 * If the depth information is invalid, the density is 0.
		 */
		float cnt = 0.0f;
		if(p.is_valid) {
			cnt = 1.0f / (M_PI * p.cluster_radius_px * p.cluster_radius_px);
			// Additionally the local gradient has to be considered.
			if(opt.gradient_adaptive_density) {
				cnt /= std::max(NZ_MIN, p.computeCircularity());
			}
		}
		p_density[i] = cnt;
	}
	return density;
}

Eigen::MatrixXf ComputeSaliency(const ImagePoints& points, const Parameters& opt)
{
	const int rows = points.rows();
	const int cols = points.cols();
	const float BR2_INV = 1.0f / (opt.base_radius * opt.base_radius);
	Eigen::MatrixXf saliency_col(rows, cols);
//	Eigen::MatrixXf saliency_norm(rows, cols);
	float* p_saliency_col = saliency_col.data();
//	float* p_saliency_norm = saliency_norm.data();
	for(int y=0; y<cols; ++y) {
		for(int x=0; x<rows; ++x, ++p_saliency_col/*,++p_saliency_norm*/) {
			const Point& p = points(x,y);
			if(!p.is_valid) {
				*p_saliency_col = 0.0f;
//				*p_saliency_norm = 0.0f;
				continue;
			}
			const int r = static_cast<int>(p.cluster_radius_px + 0.5f);
			const int x0 = std::max(x - r, 0);
			const int x1 = std::min(x + r, rows - 1);
			const int y0 = std::max(y - r, 0);
			const int y1 = std::min(y + r, cols - 1);
			// compute mean
			const Eigen::Vector3f mean_col = p.color;
			const Eigen::Vector3f mean_pos = p.position;
//			const Eigen::Vector3f mean_normal = p.normal;
			// compute compression error
			float err_col = 0.0f;
			float err_norm = 0.0f;
			float w_total = 0.0f;
			for(int i=y0; i<=y1; i++) {
				for(int j=x0; j<=x1; j++) {
					const Point& q = points(j,i);
					if(!q.is_valid)
						continue;
					float w = 1.0f / (1.0f + (q.position - mean_pos).squaredNorm() * BR2_INV);
					w_total += w;
					err_col += w * (q.color - mean_col).squaredNorm();
//					err_norm += w * (1.0f - q.normal.dot(mean_normal));
				}
			}
			// write
			*p_saliency_col = std::sqrt(err_col / w_total);
//			*p_saliency_norm = err_norm / w_total;
		}
	}
	// normalize
	{
		const float mean = saliency_col.mean();
		const float min = saliency_col.minCoeff();
		const float max = saliency_col.maxCoeff();
//		std::cout << "color: mean=" << mean << ", min=" << min << ", max=" << max << std::endl;
		saliency_col = (saliency_col.array() - mean)/std::max(mean-min, max-mean);
	}
	// {
	// 	const float mean = saliency_norm.mean();
	// 	const float min = saliency_norm.minCoeff();
	// 	const float max = saliency_norm.maxCoeff();
//	// 	std::cout << "normal: mean=" << mean << ", min=" << min << ", max=" << max << std::endl;
	// 	saliency_norm = (saliency_norm.array() - mean)/std::max(mean-min, max-mean);
	// }
	return saliency_col;// + 0.25f * saliency_norm;
}

void AdaptClusterRadiusBySaliency(ImagePoints& points, const Eigen::MatrixXf& saliency, const Parameters& opt)
{
	// FIXME what is base?
	const float base = 0.5f;
	auto it_p = points.begin();
	auto it_p_end = points.end();
	auto it_s = saliency.data();
	for(; it_p!=it_p_end; ++it_p, ++it_s) {
		it_p->cluster_radius_px *= std::pow(base, -*it_s);
	}
}

//------------------------------------------------------------------------------

void SetRandomNumberSeed(unsigned int x)
{
	pds::impl::RndSeed(x);
}

//------------------------------------------------------------------------------
}
//------------------------------------------------------------------------------


#include "TCPClient.h"
#include "myCrypto.cpp"
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <string>
#include <unistd.h>
#include <signal.h>
#include <openssl/rsa.h>
#include <openssl/err.h>
#include <openssl/pem.h>
#include <cassert>
#include <cstring>
#include <bits/stdc++.h>
#include <openssl/sha.h>
#include <sstream>
#include <fstream>

#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/algorithm/string/trim.hpp>

#include <iostream>
#include <functional>
#include <bits/stdc++.h>
#include <openssl/sha.h>

#include <openssl/rsa.h>
#include <openssl/err.h>
#include <openssl/pem.h>

#include <iostream>
#include <string>
#include <cstring>
#include <cassert>

//#include "TCPClient.h"
using namespace std;

using namespace boost::archive::iterators;

TCPClient tcp;

void sig_exit(int s)
{
	tcp.exit();
	exit(0);
}

string readFileIntoString(char * filename){
	ifstream ifile(filename);
	// read file content to buf
	ostringstream buf;
	char ch;
	while(buf&&ifile.get(ch))
		buf.put(ch);
	return buf.str();
}

int main(int argc, char *argv[])
{
	string name, tel, address, ip, port;
	if(argc != 11) {
		cerr << "Usage: ./client ip port message" << endl;
		return 0;
	}
	int opt;
	const char *optstring = "i:p:n:t:a:";
	while ((opt = getopt(argc,argv,optstring)) != -1){
		if(opt == 105){
			ip = optarg;
		}
		else if(opt == 112){
			port = optarg;
		}
		else if(opt == 110){
			name = optarg;
		}
		else if(opt == 116){
                        tel = (optarg);
                }
		else if(opt == 97){
                        address = (optarg);
                }

	}

	std::cout << "name:" << name << endl <<  "tel:" << tel << endl << "address" << address << endl;
	signal(SIGINT, sig_exit);

	tcp.setup(ip,atoi(port.c_str()));

	//send request and receive cer pubkey1.pem is the Server B's pubkey, pubkey2.pem is the Third's pubkey
	tcp.Send("pubkey1.pem");
	char file_name[] = "pubkey1.pem";
	char buffer[4096];
	FILE *fp = fopen(file_name,"w");
	if(NULL == fp){
		std::cout << "Can't open this file to write\n";
		exit(1);
	}
	bzero(buffer, 4096);
	std:: cout << tcp.receiveFile(4096,file_name) << std::endl;
	tcp.exit();
	//send request to receive the pubkey2.pem
	TCPClient tcp;
	tcp.setup(ip,atoi(port.c_str()));
	tcp.Send("pubkey2.pem");
        char file_name2[] = "pubkey2.pem";
        char buffer2[4096];
        FILE *fp2 = fopen(file_name2,"w");
        if(NULL == fp2){
                std::cout << "Can't open this file to write\n";
                exit(1);
        }
        bzero(buffer2, 4096);
        std:: cout << tcp.receiveFile(4096,file_name2) << std::endl;
	tcp.exit();
	//send encode messages
	TCPClient tcp1;
	tcp1.setup(ip,atoi(port.c_str()));
	string M1 = name + '\n' + address;
	string encodeM1 = EncodeRSAKeyFile("pubkey.pem",M1);
	cout << "Encrypted M1(RSA use pubkey1): " << encodeM1 << endl;
	//string DecodeM1 = DecodeRSAKeyFile("prikey.pem",encodeM1);
	//std::cout << "jiemahou:::====" << DecodeM1 << endl << "star:";
	//unsigned char sixM1[1024];
//	for(int i=0;i<encodeM1.length();i++){
//		printf("%02x",encodeM1[i]);
//	}
	string base64_encM1;
	Base64Encode(encodeM1,&base64_encM1);
	std::cout << "Encrypted M1 Base64 encode:" << base64_encM1 << endl;
	/*
	test the base64 decode
	string testM1;
	Base64Decode(base64_encM1,&testM1);
	std::cout << "Debase64**********" << testM1 << endl;
	string outM;
	Base64Encode(testM1, &outM);
	std::cout << "Second base64 encode:" << outM  <<endl;
	cout << "======================================================" << endl;
	string testM2 = decode64(base64_encM1);
	cout << "11111111base64:" << testM2 <<endl;
	Base64Encode(testM2,&outM);
	cout << "222222222222encodebaes64:" << outM << endl;
	*/
	//string testM1 = decode64(base64_encM1);
	//std::cout <<"final answer: decode******" << DecodeRSAKeyFile("prikey.pem",testM1);
	//std::cout << "test M1 :=======================" << testM1 << endl;
	string M2 = tel;
	cout << "Hm1 ";
	string Hm1 = SHA1_hash(M1);
	cout << "Hm2 ";
	string Hm2 = SHA1_hash(M2);
	cout << "Hm12 ";
	string Hm12 = SHA1_hash(Hm1 + ":" + Hm2);
	//std::cout << "\nHm12 SHA1 hash : \n" << Hm12 << endl;
	//string Hm2 = SHA1_hash(tel);
	//encrypt string encodeM1 = EncodeRSAKeyFile("pubkey1.pem", "1"+M1);
	auto sign_data = SignRSAKeyFile("prikey.pem",Hm12);
	VerifyRSAKeyFile("pubkey.pem",Hm12,sign_data);
	string base64_str; //hash Hm12 string
	Base64Encode(sign_data,&base64_str);
	string pubstring = readFileIntoString("pubkey.pem");
	std::cout << "local pubkey transformed to string ======:" << endl <<pubstring << endl;
	string message = base64_encM1 + ":" +  Hm2  + ":" + base64_str + ":" + pubstring;
	cout << "Send to B:";
	tcp1.Send(message);
	std::cout <<"receive decode:" <<  tcp1.receive() ;
	cout << endl;
	//tcp1.exit();
        //send encode messages
        TCPClient tcp2;
        tcp2.setup(ip,atoi(port.c_str())+1);
	string encodeM2 = EncodeRSAKeyFile("pubkey2.pem",M2);
	cout << "Encrypted M2(RSA use pubkey2):" << encodeM1 << endl;
	string base64_encM2;
	Base64Encode(encodeM2,&base64_encM2);
	cout << "Encrypted M2 Base64 encode:" << base64_encM2 << endl;
	string message2 = base64_encM2 + ":" + Hm1 + ":" + base64_str + ":" + pubstring;
	cout << "Send to C:";
	tcp2.Send(message2);
	std::cout << "receive messages:" << tcp2.receive();
	cout << endl;

	return 0;
}


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/xray/structure_factors_direct.h>
#include <boost/python/class.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>

namespace cctbx { namespace xray { namespace structure_factors {
namespace boost_python {

namespace {

  struct direct_wrappers
  {
    typedef direct<> w_t;
    typedef w_t::scatterer_type scatterer_type;
    typedef w_t::float_type float_type;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t>("structure_factors_direct", no_init)
        .def(init<uctbx::unit_cell const&,
                  sgtbx::space_group const&,
                  af::const_ref<miller::index<> > const&,
                  af::const_ref<scatterer_type> const&,
                  scattering_type_registry const&>())
        .def(init<math::cos_sin_table<float_type> const&,
                  uctbx::unit_cell const&,
                  sgtbx::space_group const&,
                  af::const_ref<miller::index<> > const&,
                  af::const_ref<scatterer_type> const&,
                  scattering_type_registry const&>())
        .def("f_calc", &w_t::f_calc, ccr())
      ;
    }
  };

} // namespace <anoymous>

}} // namespace structure_factors::boost_python

namespace boost_python {

  void wrap_structure_factors_direct()
  {
    structure_factors::boost_python::direct_wrappers::wrap();
  }

}}} // namespace cctbx::xray::boost_python