//  Copyright (c) 2018-2019 Cem Bassoy
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer and Google in producing this work
//  which started as a Google Summer of Code project.
//

#include <iostream>
#include <algorithm>
#include <complex>
#include <boost/numeric/ublas/tensor.hpp>
#include <boost/numeric/ublas/tensor/multi_index.hpp>

#include <boost/test/unit_test.hpp>

#include "utility.hpp"

BOOST_AUTO_TEST_SUITE ( test_multi_index )

using test_types = zip<int,float,std::complex<float>>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

BOOST_AUTO_TEST_CASE ( test_index_classes )
{
    namespace i = boost::numeric::ublas::index;

    BOOST_CHECK_EQUAL (  i::_a.value  ,  1  ) ;
    BOOST_CHECK_EQUAL (  i::_b.value  ,  2  ) ;
    BOOST_CHECK_EQUAL (  i::_c.value  ,  3  ) ;
    BOOST_CHECK_EQUAL (  i::_d.value  ,  4  ) ;
    BOOST_CHECK_EQUAL (  i::_e.value  ,  5  ) ;
    BOOST_CHECK_EQUAL (  i::_f.value  ,  6  ) ;
    BOOST_CHECK_EQUAL (  i::_g.value  ,  7  ) ;
    BOOST_CHECK_EQUAL (  i::_h.value  ,  8  ) ;
    BOOST_CHECK_EQUAL (  i::_i.value  ,  9  ) ;
    BOOST_CHECK_EQUAL (  i::_j.value  , 10  ) ;
    BOOST_CHECK_EQUAL (  i::_k.value  , 11  ) ;
    BOOST_CHECK_EQUAL (  i::_l.value  , 12  ) ;
    BOOST_CHECK_EQUAL (  i::_m.value  , 13  ) ;
    BOOST_CHECK_EQUAL (  i::_n.value  , 14  ) ;
    BOOST_CHECK_EQUAL (  i::_o.value  , 15  ) ;
    BOOST_CHECK_EQUAL (  i::_p.value  , 16  ) ;
    BOOST_CHECK_EQUAL (  i::_q.value  , 17  ) ;
    BOOST_CHECK_EQUAL (  i::_r.value  , 18  ) ;
    BOOST_CHECK_EQUAL (  i::_s.value  , 19  ) ;
    BOOST_CHECK_EQUAL (  i::_t.value  , 20  ) ;
    BOOST_CHECK_EQUAL (  i::_u.value  , 21  ) ;
    BOOST_CHECK_EQUAL (  i::_v.value  , 22  ) ;
    BOOST_CHECK_EQUAL (  i::_w.value  , 23  ) ;
    BOOST_CHECK_EQUAL (  i::_x.value  , 24  ) ;
    BOOST_CHECK_EQUAL (  i::_y.value  , 25  ) ;
    BOOST_CHECK_EQUAL (  i::_z.value  , 26  ) ;

}

BOOST_AUTO_TEST_CASE ( test_multi_index_class_construction )
{
    namespace ublas = boost::numeric::ublas;
    namespace i = ublas::index;

    {
      ublas::multi_index<2> ind(i::_a, i::_b);

        BOOST_CHECK_EQUAL ( get<0>( ind ), 1 ) ;
        BOOST_CHECK_EQUAL ( get<1>( ind ), 2 ) ;
    }

    {
        ublas::multi_index<2> ind(i::_d,i::_c);

        BOOST_CHECK_EQUAL ( ind[0] , 4 ) ;
        BOOST_CHECK_EQUAL ( ind[1] , 3 ) ;
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE( test_tensor_multi_index_class_generation, value,  test_types )
{
    namespace ublas = boost::numeric::ublas;
    namespace i = ublas::index;
    using value_type   = typename value::first_type;
    using layout_type  = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    auto t = std::make_tuple (
          i::_a, // 0
          i::_b, // 1
          i::_c, // 2
          i::_d, // 3
          i::_e  // 4
          );

    {
        auto a = tensor_type(ublas::extents<>{2,3}, value_type{2});
        auto a_ind = a( std::get<0>(t), std::get<2>(t)  );

        BOOST_CHECK_EQUAL ( std::addressof( a_ind.first ), std::addressof( a ) ) ;

        BOOST_CHECK_EQUAL (std::get<0>(a_ind.second)(), i::_a() ) ;
        BOOST_CHECK_EQUAL (std::get<1>(a_ind.second)(), i::_c() ) ;
    }

    {
        auto a = tensor_type(ublas::extents<>{2,3}, value_type{2});
        auto a_ind = a( std::get<2>(t), std::get<0>(t)  );

        BOOST_CHECK_EQUAL ( std::addressof( a_ind.first ), std::addressof( a ) ) ;

        BOOST_CHECK_EQUAL (std::get<0>(a_ind.second)(), i::_c() ) ;
        BOOST_CHECK_EQUAL (std::get<1>(a_ind.second)(), i::_a() ) ;
    }

    {
        auto a = tensor_type(ublas::extents<>{2,3}, value_type{2});
        auto a_ind = a( std::get<2>(t), std::get<3>(t)  );

        BOOST_CHECK_EQUAL (std::addressof(  a_ind.first ), std::addressof( a ) ) ;

        BOOST_CHECK_EQUAL (std::get<0>(a_ind.second)(), i::_c() ) ;
        BOOST_CHECK_EQUAL (std::get<1>(a_ind.second)(), i::_d() ) ;
    }

    {
        auto a = tensor_type(ublas::extents<>{2,3,4}, value_type{2});
        auto a_ind = a( std::get<2>(t), std::get<3>(t), std::get<0>(t)  );

        BOOST_CHECK_EQUAL (std::addressof(  a_ind.first ), std::addressof( a ) ) ;

        BOOST_CHECK_EQUAL (std::get<0>(a_ind.second)(), i::_c() ) ;
        BOOST_CHECK_EQUAL (std::get<1>(a_ind.second)(), i::_d() ) ;
        BOOST_CHECK_EQUAL (std::get<2>(a_ind.second)(), i::_a() ) ;
    }

}

BOOST_AUTO_TEST_SUITE_END()


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

//[ guide_histogram_in_api

#include <boost/histogram.hpp>
#include <cassert>

// function accepts any histogram and returns a copy
template <class Axes, class Storage>
boost::histogram::histogram<Axes, Storage> any_histogram(
    boost::histogram::histogram<Axes, Storage>& h) {
  return h;
}

// function only accepts histograms with fixed axis types and returns a copy
template <class Storage, class... Axes>
boost::histogram::histogram<std::tuple<Axes...>, Storage> only_static_histogram(
    boost::histogram::histogram<std::tuple<Axes...>, Storage>& h) {
  return h;
}

// function only accepts histograms with dynamic axis types and returns a copy
template <class Storage, class... Axes>
boost::histogram::histogram<std::vector<boost::histogram::axis::variant<Axes...>>,
                            Storage>
only_dynamic_histogram(
    boost::histogram::histogram<std::vector<boost::histogram::axis::variant<Axes...>>,
                                Storage>& h) {
  return h;
}

int main() {
  using namespace boost::histogram;

  auto histogram_with_static_axes = make_histogram(axis::regular<>(10, 0, 1));

  using axis_variant = axis::variant<axis::regular<>, axis::integer<>>;
  std::vector<axis_variant> axes;
  axes.emplace_back(axis::regular<>(5, 0, 1));
  axes.emplace_back(axis::integer<>(0, 1));
  auto histogram_with_dynamic_axes = make_histogram(axes);

  assert(any_histogram(histogram_with_static_axes) == histogram_with_static_axes);
  assert(any_histogram(histogram_with_dynamic_axes) == histogram_with_dynamic_axes);
  assert(only_static_histogram(histogram_with_static_axes) == histogram_with_static_axes);
  assert(only_dynamic_histogram(histogram_with_dynamic_axes) ==
         histogram_with_dynamic_axes);

  // does not compile: only_static_histogram(histogram_with_dynamic_axes)
  // does not compile: only_dynamic_histogram(histogram_with_static_axes)
}

//]


/**************************************************************
 *
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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.
 *
 *************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_basegfx.hxx"
// autogenerated file with codegen.pl

#include "preextstl.h"
#include "gtest/gtest.h"
#include "postextstl.h"

#include <basegfx/tools/keystoplerp.hxx>
#include <basegfx/numeric/ftools.hxx>

#include <boost/tuple/tuple.hpp>

using namespace ::basegfx;
using namespace ::boost::tuples;

namespace basegfxtools
{

class KeyStopLerpTest : public ::testing::Test
{
protected:
    tools::KeyStopLerp maKeyStops;

    static std::vector<double> getTestVector()
    {
        std::vector<double> aStops(3);
        aStops[0] = 0.1;
        aStops[1] = 0.5;
        aStops[2] = 0.9;
        return aStops;
    }

public:
    KeyStopLerpTest() :
        maKeyStops(getTestVector())
    {}

    virtual void SetUp()
    {}

    virtual void TearDown()
    {}
};

TEST_F(KeyStopLerpTest, test)
{
    double fAlpha;
    std::ptrdiff_t nIndex;

    tie(nIndex,fAlpha) = maKeyStops.lerp(-1.0);
    ASSERT_TRUE(nIndex==0 && fAlpha==0.0) << "-1.0";

    tie(nIndex,fAlpha) = maKeyStops.lerp(0.1);
    ASSERT_TRUE(nIndex==0 && fAlpha==0.0) << "0.1";

    tie(nIndex,fAlpha) = maKeyStops.lerp(0.3);
    ASSERT_TRUE(nIndex==0 && fTools::equal(fAlpha,0.5)) << "0.3";

    tie(nIndex,fAlpha) = maKeyStops.lerp(0.5);
    ASSERT_TRUE(nIndex==0 && fTools::equal(fAlpha,1.0)) << "0.5";

    tie(nIndex,fAlpha) = maKeyStops.lerp(0.51);
    ASSERT_TRUE(nIndex==1 && fTools::equal(fAlpha,0.025)) << "0.51";

    tie(nIndex,fAlpha) = maKeyStops.lerp(0.9);
    ASSERT_TRUE(nIndex==1 && fTools::equal(fAlpha,1.0)) << "0.51";

    tie(nIndex,fAlpha) = maKeyStops.lerp(1.0);
    ASSERT_TRUE(nIndex==1 && fAlpha==1.0) << "0.51";
}

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

} // namespace basegfxtools


/*
 * Copyright 2015 David A. Boyuka II
 *
 * 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.
 */

/*
 * quantize.cpp
 *
 *  Created on: May 22, 2014
 *      Author: David A. Boyuka II
 */

extern "C" {
#include <getopt.h>
#ifndef _Bool
#define _Bool bool
#endif
#include <pique/util/myopts.h>
}

#include <typeindex>
#include <iostream>
#include <cstdint>
#include <cstdio>
#include <cstring>
#include <cassert>
#include <set>
#include <map>

#include <boost/algorithm/string.hpp>
#include <boost/optional.hpp>

using namespace std;

struct cmd_args_t {
	char *in_filename_str;
	char *out_filename_str;
	char *datatype;
	char *outdatatype;

	int equibin_nbins;
	int equibin_sampling_ratio;
};

struct cmd_config_t {
	string in_filename;
	string out_filename;
	boost::optional< std::type_index > datatype;
	boost::optional< std::type_index > outdatatype;

	int equibin_nbins;
	int equibin_sampling_ratio;
};

template<typename WordT, typename OutWordT>
static void quantize_equalbinning(const cmd_config_t &conf) {
	const int nbins = conf.equibin_nbins;
	const int sampling_ratio = conf.equibin_sampling_ratio;

	FILE *fin = std::fopen(conf.in_filename.c_str(), "r");
	FILE *fout = std::fopen(conf.out_filename.c_str(), "w");
	assert(fin && fout);

	std::set< WordT > keys;

	const uint64_t BUFFER_SIZE = 1048576;
	std::vector<WordT> buf(BUFFER_SIZE);

	while (!feof(fin)) {
		const size_t nwords = fread(&buf.front(), sizeof(WordT), buf.size(), fin);
		for (size_t i = 0; i < nwords; i++)
			if ((rand() % sampling_ratio) == 0)
				keys.insert(buf[i]);
	}

	fseek(fin, 0, SEEK_SET);

	OutWordT nextbid = 0;
	std::map< WordT, OutWordT > bin_boundary_map; // each entry is the exclusive upper bound of a bin (i.e., entry (key, binid) maps values [prev_key, key) -> binid)
	uint64_t rank = 0;
	for (auto it = keys.begin(); it != keys.end(); it++, rank++) {
		OutWordT bid = (rank + 1) * nbins / keys.size() - 1; // So that bin 0 isn't generated right away
		if (bid == nextbid) {
			if (bin_boundary_map.find(*it) == bin_boundary_map.end()) // Only add this value as a bin boundary if it's not already used as one somewhere else
				bin_boundary_map.insert(std::make_pair(*it, nextbid));
			nextbid++;
		}
	}

	// The above loop is a bit sloppy and may introduce one last bin that is very small
	// If so, delete that boundary so it merges with the previous bin
	if (bin_boundary_map.size() == nbins)
		bin_boundary_map.erase(--bin_boundary_map.end());

	for (auto it = bin_boundary_map.cbegin(); it != bin_boundary_map.cend(); ++it)
		std::cout << it->first << "\t" << it->second << std::endl;

	std::vector<OutWordT> outbuf(BUFFER_SIZE);

	while (!feof(fin)) {
		const size_t nwords = fread(&buf.front(), sizeof(WordT), buf.size(), fin);
		for (size_t i = 0; i < nwords; i++) {
			auto bin_boundary_it = bin_boundary_map.lower_bound(buf[i]); // First boundary > (not >=) this element
			OutWordT binid = bin_boundary_it != bin_boundary_map.end() ? bin_boundary_it->second : nbins - 1; // The iterator will always point to a boundary, unless we are on or after the last boundary, in which case we are in the last bin
			outbuf[i] = binid;
		}
		fwrite(&outbuf.front(), sizeof(OutWordT), nwords, fout);
	}

	fclose(fin);
	fclose(fout);
}

static const std::type_info & datatype_name_to_type(string dtname) {
	if (dtname == "double")
		return typeid(double);
	else if (dtname == "uint64")
		return typeid(uint64_t);
	else if (dtname == "uint16")
		return typeid(uint16_t);
	else
		abort();
	return typeid(nullptr);
}

static void validate_and_fully_parse_args(cmd_config_t &conf, cmd_args_t &args) {
	conf.in_filename = string(args.in_filename_str);
	conf.out_filename = string(args.out_filename_str);
	conf.datatype = datatype_name_to_type(args.datatype);
	conf.outdatatype = datatype_name_to_type(args.outdatatype);

	conf.equibin_nbins = args.equibin_nbins;
	conf.equibin_sampling_ratio = args.equibin_sampling_ratio;
}

static myoption addopt(const char *flagname, int hasarg, OPTION_VALUE_TYPE type, void *output, void *fixedval = NULL) {
	myoption opt;
	opt.flagname = flagname;
	opt.hasarg = hasarg;
	opt.type = type;
	opt.output = output;
	opt.fixedval = fixedval;
	return opt;
}

template<typename WordT>
static void quantize_outword_switch(const cmd_config_t &conf) {
    if (*conf.outdatatype == typeid(double))
    	quantize_equalbinning<WordT, double>(conf);
    else if (*conf.outdatatype == typeid(uint64_t))
    	quantize_equalbinning<WordT, uint64_t>(conf);
    else if (*conf.outdatatype == typeid(uint16_t))
    	quantize_equalbinning<WordT, uint16_t>(conf);
    else {
    	std::cerr << "Invalid out datatype " << conf.outdatatype << std::endl;
    	abort();
    }
}

static void quantize_inword_switch(const cmd_config_t &conf) {
	if (*conf.datatype == typeid(double))
		quantize_outword_switch<double>(conf);
	else if (*conf.datatype == typeid(uint64_t))
		quantize_outword_switch<uint64_t>(conf);
	else if (*conf.datatype == typeid(uint16_t))
		quantize_outword_switch<uint16_t>(conf);
	else {
		std::cerr << "Invalid datatype " << conf.datatype << std::endl;
    	abort();
    }
}

int main(int argc, char **argv) {
    cmd_args_t args;
    cmd_config_t conf;

    // static bool FALSEVAL = false, TRUEVAL = true;

    myoption opts[] = {
        addopt("fin", required_argument, OPTION_TYPE_STRING, &args.in_filename_str),
        addopt("fout", required_argument, OPTION_TYPE_STRING, &args.out_filename_str),
        addopt("dt", required_argument, OPTION_TYPE_STRING, &args.datatype),
        addopt("odt", required_argument, OPTION_TYPE_STRING, &args.outdatatype),

        addopt("nbins", required_argument, OPTION_TYPE_INT, &args.equibin_nbins),
        addopt("sample", required_argument, OPTION_TYPE_INT, &args.equibin_sampling_ratio),

        addopt(NULL, required_argument, OPTION_TYPE_BOOLEAN, NULL),
    };
    parse_args(&argc, &argv, opts);
    validate_and_fully_parse_args(conf, args);

    quantize_inword_switch(conf);
}


#include <armadillo>

int main()
{
  arma::mat dataset;
  dataset.load("dataset.csv");
  for (size_t i = 0; i < dataset.n_rows; ++i)
  {
    arma::rowvec feature_vector =  dataset.row(i);
    std::string name = "class" + std::to_string(feature_vector.at(feature_vector.n_elem - 1)) + ".__" + std::to_string(i) + ".csv";
    feature_vector.shed_col(feature_vector.n_elem - 1);
    feature_vector.save(name, arma::csv_ascii);
  }
}


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/fms.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL(" fms",  STF_SIGNED_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using bs::fms;

  STF_EXPR_IS(fms(T(),T(),T()), T);

  STF_EQUAL(fms(T(2),T(2),T(4)) , T(0));
  STF_EQUAL(fms(T(2),T(2),T(-4)), T(8));
  STF_EQUAL(fms(T(2),T(-2),T(4)), T(-8));
  STF_EQUAL(fms(T(-2),T(2),T(4)), T(-8));
}


#include <mutex>
#include <vector>
#include <queue>
#include <thread>
#include <string>
#include <Eigen/Core>

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <tf_conversions/tf_eigen.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <apa_msgs/VehicleOdomEst2D.h>
#include "utils/common_names.h"
#include "utils/stampTools.hpp"
#include "utils/tfTools.hpp"
#include "utils/stampTools.hpp"

template <typename T>
class OdometryPublisherBase
{
public:
    using OdomT = T;

    OdometryPublisherBase():_last_odom_stamp(0), _last_twist_stamp(0)
    {
        ros::NodeHandle nh;
        ros::NodeHandle private_nh("~");
        _target_frame = private_nh.param<std::string>("correction_target_frame", local_map_frame_id);
        _source_frame = private_nh.param<std::string>("correction_source_frame", odom_frame_id);
        _child_frame= private_nh.param<std::string>("offset_source_frame", base_link_frame_id);
        const std::string odometer_frame = private_nh.param<std::string>("offset_target_frame", base_link_frame_id);
        const std::string sub_odom_topic =  topic_odometer + std::string("/") + odometer_frame;
        const std::string sub_twist_topic = topic_velometer + std::string("/") + odometer_frame;
        const std::string pub_topic_postfix = private_nh.param<std::string>("pub_topic_postfix", _target_frame + std::string("/") + _child_frame);

        _T_self_child = read_static_tf(odometer_frame, _child_frame);

        Eigen::Isometry3d T_child_self;

        tf::transformTFToEigen(_T_self_child.inverse(), T_child_self);

        Eigen::Vector3d t(T_child_self.translation());
        Eigen::Matrix3d tx, R;
        R = T_child_self.linear();
        tx <<   0.0, -t.z(),  t.y(),
              t.z(),    0.0, -t.x(),
             -t.y(),  t.x(),    0.0;

        _adj_child_self << R, Eigen::Matrix3d::Zero(), tx * R, R;

        if (_target_frame != "" &&  !_recvT(_target_frame, _source_frame, ros::Time(0), _T_target_source, 5))
        {
            ROS_FATAL("no Transform found from %s to %s within 5 seconds", _source_frame.c_str(), _target_frame.c_str());
            return;
        }

        _pub_odom_info = nh.advertise<nav_msgs::Odometry>(topic_odometer + std::string("/") + pub_topic_postfix, 4);
        _pub_twist_info = nh.advertise<geometry_msgs::TwistStamped>(topic_velometer + std::string("/") + _child_frame + "_local", 4);
        _sub_odom_info = nh.subscribe<OdomT>(sub_odom_topic, 4, &OdometryPublisherBase::_odom_info_handler, this);
        _sub_twist_info = nh.subscribe<geometry_msgs::TwistStamped>(sub_twist_topic, 4, &OdometryPublisherBase::_pub_twist, this);
    }

    virtual ~OdometryPublisherBase() {}

protected:

    void _pub_odom(const nav_msgs::Odometry::Ptr& odom)
    {
        if (ROS_TIME(odom) <= _last_odom_stamp)
            return;

        _last_odom_stamp = ROS_TIME(odom);

        if (_pub_odom_info.getNumSubscribers() < 1)
            return;

        if (_target_frame != "")
            _recvT(_target_frame, _source_frame, ros::Time(0), _T_target_source);

        tf::Transform T_odom;
        tf::poseMsgToTF(odom->pose.pose, T_odom);
        tf::poseTFToMsg(_T_target_source * T_odom * _T_self_child, odom->pose.pose);

        odom->header.frame_id = _target_frame;
        odom->child_frame_id = _child_frame;

        _pub_odom_info.publish(odom);
    }

    void _pub_twist(const geometry_msgs::TwistStampedConstPtr& twist_msg)
    {
        if(ROS_TIME(twist_msg) <= _last_twist_stamp)
            return;

        _last_twist_stamp = ROS_TIME(twist_msg);

        if(_pub_twist_info.getNumSubscribers() < 1)
            return;

        Eigen::Matrix<double, 6, 1> twist;
        twist << twist_msg->twist.angular.x, twist_msg->twist.angular.y, twist_msg->twist.angular.z,
                 twist_msg->twist.linear.x,  twist_msg->twist.linear.y,  twist_msg->twist.linear.z;
        twist = _adj_child_self * twist;
        geometry_msgs::TwistStamped msg;
        msg.header.stamp = twist_msg->header.stamp;
        msg.header.frame_id = _child_frame;
        msg.twist.angular.x = twist[0];
        msg.twist.angular.y = twist[1];
        msg.twist.angular.z = twist[2];
        msg.twist.linear.x = twist[3];
        msg.twist.linear.y = twist[4];
        msg.twist.linear.z = twist[5];
        _pub_twist_info.publish(msg);
    }

    bool _recvT(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, tf::StampedTransform &transform, const double wait_time = 0) const
    {
        try
        {
            _listener.waitForTransform(target_frame, source_frame, time, ros::Duration(wait_time));
            _listener.lookupTransform(target_frame, source_frame, time, transform);
        }
        catch (const tf::TransformException &e)
        {
            std::cerr << e.what() << std::endl;
            return false;
        }
        return true;
    }

    virtual void _odom_info_handler(const typename OdomT::ConstPtr &odom_info) = 0;

private:
    Eigen::Matrix<double, 6, 6> _adj_child_self;
    ros::Subscriber _sub_odom_info, _sub_twist_info;
    ros::Publisher _pub_odom_info, _pub_twist_info;
    tf::TransformListener _listener;
    tf::StampedTransform _T_target_source,  _T_self_child;;
    std::string _target_frame, _source_frame, _child_frame;
    ros::Time _last_odom_stamp, _last_twist_stamp;

}; // class OdometryPublisherBase

class PoseStampedPublisher : public OdometryPublisherBase<geometry_msgs::PoseStamped>
{

protected:
    virtual void _odom_info_handler(const OdomT::ConstPtr &odom_info) override
    {
        nav_msgs::Odometry::Ptr odom(new nav_msgs::Odometry);
        odom->header.stamp = odom_info->header.stamp;
        odom->pose.pose = odom_info->pose;
        _pub_odom(odom);
    }
};

class OdometryPublisher : public OdometryPublisherBase<nav_msgs::Odometry>
{
protected:
    virtual void _odom_info_handler(const OdomT::ConstPtr &odom_info) override
    {
        _pub_odom(boost::make_shared<OdomT>(*odom_info));
    }
}; // class OdometryPublisher

class ApaOdometryPublisher : public OdometryPublisherBase<apa_msgs::VehicleOdomEst2D>
{
protected:
    virtual void _odom_info_handler(const OdomT::ConstPtr &odom_info) override
    {
        nav_msgs::Odometry::Ptr odom(new nav_msgs::Odometry());
        odom->header.stamp = odom_info->header.stamp;
        odom->pose.pose.position.x = odom_info->x;
        odom->pose.pose.position.y = odom_info->y;
        odom->pose.pose.position.z = 0;
        const auto q = tf::createQuaternionFromYaw(odom_info->yaw);
        odom->pose.pose.orientation.w = q.w();
        odom->pose.pose.orientation.x = q.x();
        odom->pose.pose.orientation.y = q.y();
        odom->pose.pose.orientation.z = q.z();
        _pub_odom(odom);
    }

}; // class ApaOdometryPublisher

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "odometry_publisher");
    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");

    ros::Time::waitForValid();

    PoseStampedPublisher odom_pub;

    ros::spin();

    return 0;
}


#include "boost/thread/thread.hpp"
#include "boost/bind.hpp"
#include "boost/thread/mutex.hpp"

#include <opencv2/opencv.hpp>
#include <vector>
#include <iostream>
#include "base64.h"

//#include <unistd.h>
#include <stdio.h>
#include <termios.h>

#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include "sensor_msgs/Imu.h"
#include "std_msgs/Bool.h"
#include <boost/regex.hpp>
#include <boost/filesystem.hpp>

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

using namespace std;
using namespace cv;
using namespace Eigen;

static void toEulerAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
// roll (x-axis rotation)
double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
roll = atan2(sinr_cosp, cosr_cosp);

// pitch (y-axis rotation)
double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
if (fabs(sinp) >= 1)
pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
else
pitch = asin(sinp);

// yaw (z-axis rotation)
double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
yaw = atan2(siny_cosp, cosy_cosp);
}
static float toAngle(double& r)
{
    return 180 * r / M_PI;
}

class receive_imu_node
{
private:
    ros::NodeHandle node_;
    ros::Subscriber sub,init_sub;
    std::string topic;
    std::string root_path;
    std::string visible_path;
    string image_name;
    int count, scount;
    long long int time_flag;
    ofstream map_points;
    stringstream gss;
    vector<int> compression_params;
    bool is_init;
    float init_imu_x,init_imu_y,init_imu_z;
    float dx,dy,dz;

public:
    receive_imu_node();
    ~receive_imu_node();
    void initCB(const std_msgs::Bool::ConstPtr &msg);
    void callback(const sensor_msgs::Imu::ConstPtr& msg);
    void update();
    void isDirectory(string path);
};

receive_imu_node::receive_imu_node()
{
    ros::param::get("/receive_imu/root_path", root_path);
    ros::param::get("/receive_imu/topic", topic);
    std::cout << "root_path:" << root_path << " topic:" << topic << std::endl;
    isDirectory(root_path);
    string record_path = root_path + "/imu.txt";
    map_points.open(record_path, ios_base::app);
    sub = node_.subscribe(topic, 1, &receive_imu_node::callback,this);
    init_sub = node_.subscribe<std_msgs::Bool>("/receive_imu/init", 1, &receive_imu_node::initCB, this);
    compression_params.push_back(CV_IMWRITE_JPEG_QUALITY);
    compression_params.push_back(100);
    is_init = true;
}

receive_imu_node::~receive_imu_node()
{
    map_points.close();
}

void receive_imu_node::initCB(const std_msgs::Bool::ConstPtr &msg){
    std::cout << "initCB " << std::endl;
    is_init = true;
}

void receive_imu_node::update()
{
    map_points << time_flag << " " << dx << " " << dy << " " << dz << std::endl;
}

void receive_imu_node::isDirectory(string path)
{
    //判断根目录下是否存在子目录，不存在创建
    if (!boost::filesystem::exists(path))
    {
        boost::filesystem::create_directory(path);
    }
}

void receive_imu_node::callback(const sensor_msgs::Imu::ConstPtr& msg)
{
  try
  {
    count = ros::Time::now().sec;
    scount = ros::Time::now().nsec / 1000000;
    time_flag = (long long int)count * 1000 + scount;
    //std::cout << "count:" << count << " scount:" << scount << " time_flag:" << time_flag << std::endl;
    geometry_msgs::Quaternion qua = msg->orientation;
    Eigen::Quaterniond q(qua.w, qua.x, qua.y, qua.z);
    double roll,pitch,yaw;
    toEulerAngle(q,roll,pitch,yaw);
    //std::cout << "roll:" << roll<< " pitch:" << pitch<< " yaw:" << yaw << std::endl;
    if(is_init){
        std::cout << "init:" << std::endl;
        init_imu_x = toAngle(roll);
        init_imu_y = toAngle(pitch);
        init_imu_z = toAngle(yaw);
        is_init = false;
    }
    dx = abs(toAngle(roll) - init_imu_x);
    dy = abs(toAngle(pitch) - init_imu_y);
    dz = abs(toAngle(yaw) - init_imu_z);
  }
  catch (Exception& e)
  {
    std::cout << "error code:" << e.code << " " << e.err << std::endl;
  }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "receive_imu_node");
    receive_imu_node node;

    ROS_INFO("receive_imu_node node started...");
    ros::Rate rate(100);

    while (ros::ok())
    {
        node.update();
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}


/*
 * FeedforwardNetwork.cpp
 *
 *  Created on: 2014/01/13
 *      Author: kryozahiro
 */

#include "FeedforwardNetwork.h"

#include <cassert>
#include <cmath>
#include <boost/algorithm/string.hpp>
#include "cpputil/GenericIo.h"

#include "../../thirdparty/fastonebigheader.h"
using namespace std;
namespace pt = boost::property_tree;

std::vector<std::shared_ptr<Program>> FeedforwardNetwork::generate(const ProgramType& programType, const boost::property_tree::ptree& node, int size, std::mt19937_64& randomEngine) {
	std::vector<std::shared_ptr<Program>> programs;
	for (int i = 0; i < size; ++i) {
		programs.push_back(make_shared<FeedforwardNetwork>(programType, node, randomEngine));
	}
	return programs;
}

void FeedforwardNetwork::crossover(FeedforwardNetwork& parent1, FeedforwardNetwork& parent2, mt19937_64& randomEngine) {
	double alpha = 0.1;
	for (unsigned int y = 0; y < parent1.weights.size(); ++y) {
		for (unsigned int x = y + 1; x < parent1.weights[y].size(); ++x) {
			if (parent1.changeable[y][x]) {
				pair<double, double> mm = minmax(parent1.weights[y][x], parent2.weights[y][x]);
				double dist = mm.second - mm.first;
				uniform_real_distribution<double> weightDist(mm.first - alpha * dist, mm.second + alpha * dist);
				parent1.weights[y][x] = weightDist(randomEngine);
				parent2.weights[y][x] = weightDist(randomEngine);
				parent1.weights[y][x] = max(-10.0, min(parent1.weights[y][x], 10.0));
				parent2.weights[y][x] = max(-10.0, min(parent2.weights[y][x], 10.0));
			}
		}
	}
}

void FeedforwardNetwork::normalMutation(FeedforwardNetwork& parent, mt19937_64& randomEngine) {
	uniform_real_distribution<double> mutateDist(0, 1);
	uniform_real_distribution<double> weightDist(0.9, 1.1);
	for (unsigned int y = 0; y < parent.weights.size(); ++y) {
		for (unsigned int x = y + 1; x < parent.weights[y].size(); ++x) {
			if (parent.changeable[y][x] and mutateDist(randomEngine) < 0.1) {
				parent.weights[y][x] = parent.weights[y][x] * weightDist(randomEngine);
				parent.weights[y][x] = max(-10.0, min(parent.weights[y][x], 10.0));
			}
		}
	}
}

double FeedforwardNetwork::unitStep(double x) {
	return (x >= 0) ? 1 : 0;
}

double FeedforwardNetwork::sigmoid(double a, double x) {
	//return 1.0 / (1.0 + exp(-a * x));
	return 1.0 / (1.0 + fasterexp(-a * x));
}

FeedforwardNetwork::FeedforwardNetwork(const ProgramType& programType, int hiddenSize, function<double(double)> activation)
		: Program(programType) {
	int nodeSize = 1 + programType.getInputType().getSize() + hiddenSize + programType.getOutputType().getSize();
	this->adjacency = boost::container::vector<boost::container::vector<bool>>(nodeSize, boost::container::vector<bool>(nodeSize, false));
	this->weights = vector<vector<double>>(nodeSize, vector<double>(nodeSize, 0));
	this->activation = activation;
	this->changeable = boost::container::vector<boost::container::vector<bool>>(nodeSize, boost::container::vector<bool>(nodeSize, false));
}

FeedforwardNetwork::FeedforwardNetwork(const ProgramType& programType, const boost::property_tree::ptree& node, std::mt19937_64& randomEngine)
		: Program(programType) {
	//活性化関数の初期化
	string activationName = node.get<string>("Activation");
	if (activationName == "Unit") {
		activation = FeedforwardNetwork::unitStep;
	} else if (activationName == "Sigmoid") {
		activation = [](double x) {
			return FeedforwardNetwork::sigmoid(1.0, x);
		};
	}

	//行列の生成
	int hiddenSize = node.get<int>("Hidden");
	int nodeSize = 1 + programType.getInputType().getSize() + hiddenSize + programType.getOutputType().getSize();
	this->adjacency = boost::container::vector<boost::container::vector<bool>>(nodeSize, boost::container::vector<bool>(nodeSize, false));
	this->weights = vector<vector<double>>(nodeSize, vector<double>(nodeSize, 0));
	this->changeable = boost::container::vector<boost::container::vector<bool>>(nodeSize, boost::container::vector<bool>(nodeSize, false));

	//接続の生成
	const pt::ptree& adjacencyTree = node.get_child("Adjacency");
	for (const pt::ptree::value_type& kvp : adjacencyTree) {
		if (kvp.first != "Connection") {
			assert(false);
		}

		int src0 = readConnectionNotation(kvp.second.get<string>("<xmlattr>.src0"));
		int src1 = readConnectionNotation(kvp.second.get<string>("<xmlattr>.src1"));
		int dest0 = readConnectionNotation(kvp.second.get<string>("<xmlattr>.dest0"));
		int dest1 = readConnectionNotation(kvp.second.get<string>("<xmlattr>.dest1"));
		setAdjacency(src0, src1, dest0, dest1, true);
		setChangeable(src0, src1, dest0, dest1, true);
	}

	//重みの初期化
	randomizeWeights(randomEngine);
}

vector<double> FeedforwardNetwork::operator()(const vector<double>& input) {
	assert(getProgramType().getInputType().accepts(input));
	vector<double> node(weights.size());
	for (unsigned int y = 0; y < weights.size(); ++y) {
		if (y == 0) {
			node[y] = 1;
		} else if (y <= (unsigned int)getProgramType().getInputType().getSize()) {
			node[y] = input[y - 1];
		} else {
			node[y] = activation(node[y]);
		}
		for (unsigned int x = y + 1; x < weights[y].size(); ++x) {
			if (adjacency[y][x]) {
				node[x] += weights[y][x] * node[y];
			}
		}
	}
	vector<double> output(node.end() - getProgramType().getOutputType().getSize(), node.end());
	getProgramType().getOutputType().clip(output);
	return output;
}

string FeedforwardNetwork::toString() const {
	return "";
}

void FeedforwardNetwork::read(istream& is) {
	/*tuple<
		ProgramType,
		boost::container::vector<boost::container::vector<bool>>,
		vector<vector<double>>,
		boost::container::vector<boost::container::vector<bool>>
	> t;
	is >> t;
	programType = get<0>(t);
	adjacency = get<1>(t);
	weights = get<2>(t);
	changeable = get<3>(t);
	assert(adjacency.size() == weights.size());
	assert(changeable.size() == weights.size());*/
	//TODO: activation
}

void FeedforwardNetwork::write(ostream& os) const {
	tuple<
		ProgramType,
		boost::container::vector<boost::container::vector<bool>>,
		vector<vector<double>>,
		boost::container::vector<boost::container::vector<bool>>
	> t(getProgramType(), adjacency, weights, changeable);
	os << t;
}

std::vector<std::shared_ptr<FeedforwardNetwork>> FeedforwardNetwork::neighborhoodImpl(const std::string& method, int size, std::mt19937_64& randomEngine) {
	std::vector<std::shared_ptr<FeedforwardNetwork>> ret;
	for (int i = 0; i < size; ++i) {
		shared_ptr<FeedforwardNetwork> neighbor = make_shared<FeedforwardNetwork>(*this);
		FeedforwardNetwork::normalMutation(*neighbor, randomEngine);
		ret.push_back(neighbor);
	}
	return ret;
}

void FeedforwardNetwork::crossoverImpl(const std::string& method, FeedforwardNetwork& other, std::mt19937_64& randomEngine) {
	FeedforwardNetwork::crossover(*this, other, randomEngine);
}

void FeedforwardNetwork::mutationImpl(const std::string& method, std::mt19937_64& randomEngine) {
	FeedforwardNetwork::normalMutation(*this, randomEngine);
}

int FeedforwardNetwork::getBias() {
	return 0;
}
int FeedforwardNetwork::getInputBegin() {
	return 1;
}
int FeedforwardNetwork::getInputEnd() {
	return 1 + getProgramType().getInputType().getSize();
}
int FeedforwardNetwork::getHiddenBegin() {
	return 1 + getProgramType().getInputType().getSize();
}
int FeedforwardNetwork::getHiddenEnd() {
	return adjacency.size() - getProgramType().getOutputType().getSize();
}
int FeedforwardNetwork::getOutputBegin() {
	return adjacency.size() - getProgramType().getOutputType().getSize();
}
int FeedforwardNetwork::getOutputEnd() {
	return adjacency.size();
}

void FeedforwardNetwork::setAdjacency(int from, int to, bool isAdjacent) {
	assert(0 <= from and from < (int)adjacency.size());
	assert(0 <= to and to < (int)adjacency.size());
	adjacency[from][to] = isAdjacent;
	adjacency[to][from] = isAdjacent;
}

void FeedforwardNetwork::setAdjacency(int first1, int last1, int first2, int last2, bool isAdjacent) {
	for (int x = first1; x < last1; ++x) {
		for (int y = first2; y < last2; ++y) {
			setAdjacency(x, y, isAdjacent);
		}
	}
}

void FeedforwardNetwork::setChangeable(int from, int to, bool isChangeable) {
	assert(0 <= from and from < (int)changeable.size());
	assert(0 <= to and to < (int)changeable.size());
	changeable[from][to] = isChangeable;
	changeable[to][from] = isChangeable;
}

void FeedforwardNetwork::setChangeable(int first1, int last1, int first2, int last2, bool isChangeable) {
	for (int x = first1; x < last1; ++x) {
		for (int y = first2; y < last2; ++y) {
			setChangeable(x, y, isChangeable);
		}
	}
}

void FeedforwardNetwork::insertNode(int pos, int size, bool addToBegin) {
	assert(0 <= pos and pos <= (int)adjacency.size());
	/*if (pos < getInputEnd() or (!addToBegin and pos == getInputEnd())) {
		programType = ProgramType(getProgramType().getInputType(), getProgramType().getInputSize() + size, getProgramType().getOutputType(), getProgramType().getOutputSize());
	} else if ((addToBegin and pos == getHiddenEnd()) or getHiddenEnd() < pos) {
		programType = ProgramType(getProgramType().getInputType(), getProgramType().getInputSize(), getProgramType().getOutputType(), getProgramType().getOutputSize() + size);
	}
	for (unsigned int y = 0; y < weights.size(); ++y) {
		adjacency[y].insert(adjacency[y].begin() + pos, size, false);
		weights[y].insert(weights[y].begin() + pos, size, 0);
		changeable[y].insert(changeable[y].begin() + pos, size, false);
	}
	adjacency.insert(adjacency.begin() + pos, size, boost::container::vector<bool>(adjacency.size() + size, false));
	weights.insert(weights.begin() + pos, size, vector<double>(weights.size() + size, 0));
	changeable.insert(changeable.begin() + pos, size, boost::container::vector<bool>(changeable.size() + size, false));*/
}

int FeedforwardNetwork::readConnectionNotation(const std::string& position) {
	string lower = boost::to_lower_copy(position);
	vector<string> splitted;
	boost::algorithm::split(splitted, lower, boost::algorithm::is_any_of("_"));

	if (splitted.size() < 2) {
		assert(false);
	}

	bool isIndex = isdigit(splitted[1][0]);
	int index = isIndex ? atoi(splitted[1].c_str()) : 0;

	if (splitted[0][0] == 'b') {
		return isIndex ? getBias() + index : getBias() + 1;
	} else if (splitted[0][0] == 'i') {
		return isIndex ? getInputBegin() + index : getInputEnd();
	} else if (splitted[0][0] == 'h') {
		return isIndex ? getHiddenBegin() + index : getHiddenEnd();
	} else if (splitted[0][0] == 'o') {
		return isIndex ? getOutputBegin() + index : getOutputEnd();
	} else {
		assert(false);
	}
	return 0;
}


#ifndef MO_CORE_TIME_HPP
#define MO_CORE_TIME_HPP
#include <MetaObject/detail/Export.hpp>

#include <boost/optional.hpp>
#include <boost/optional/optional_io.hpp>

#include <ct/reflect.hpp>

#include <chrono>

namespace mo
{
    using Duration = std::chrono::high_resolution_clock::duration;

    struct MO_EXPORTS Time : public std::chrono::high_resolution_clock::time_point
    {
        using GetTime_f = mo::Time (*)();

        static Time now();
        static void setTimeSource(GetTime_f timefunc);

        Time() = default;
        Time(const std::chrono::high_resolution_clock::time_point& t);
        Time(const Duration& d);
        explicit Time(double sec);

        double seconds() const;
        void fromSeconds(double);

        std::string print() const;
        void print(std::ostream& os,
                   bool print_days = false,
                   bool print_hours = false,
                   bool print_minutes = true,
                   bool print_seconds = true,
                   bool print_nanoseconds = false) const;
    };

    struct MO_EXPORTS FrameNumber
    {
        static uint64_t max();

        FrameNumber(uint64_t v = max());

        bool valid() const;
        operator uint64_t&();
        operator uint64_t() const;
        FrameNumber& operator=(uint64_t v);
        // bool operator==(const FrameNumber&) const;
        // bool operator==(uint64_t) const;

        uint64_t val = max();
    };

    using OptionalTime = boost::optional<Time>;

    template <class T>
    struct MorePreciseTime;

    template <>
    struct MorePreciseTime<std::chrono::hours>
    {
        using type = std::chrono::minutes;
    };
    template <>
    struct MorePreciseTime<std::chrono::minutes>
    {
        using type = std::chrono::seconds;
    };
    template <>
    struct MorePreciseTime<std::chrono::seconds>
    {
        using type = std::chrono::milliseconds;
    };
    template <>
    struct MorePreciseTime<std::chrono::milliseconds>
    {
        using type = std::chrono::microseconds;
    };
    template <>
    struct MorePreciseTime<std::chrono::microseconds>
    {
        using type = std::chrono::nanoseconds;
    };

    template <class T>
    struct TimePrefix
    {
        static Duration convert(const int64_t val)
        {
            return Duration(T(val));
        }

        static Duration convert(double val)
        {
            using MorePrecise = typename MorePreciseTime<T>::type;
            const auto integral = static_cast<unsigned long>(val);
            T whole(integral);
            val -= integral;
            using Ratio = std::ratio_divide<typename MorePrecise::period, typename T::period>;
            const auto num = Ratio::num;
            const auto den = Ratio::den;
            auto fr = static_cast<unsigned long>(val * den / num);
            MorePrecise fraction(fr);
            return Duration(whole + fraction);
        }
    };

    static const auto ns = TimePrefix<std::chrono::nanoseconds>();
    static const auto us = TimePrefix<std::chrono::microseconds>();
    static const auto ms = TimePrefix<std::chrono::milliseconds>();
    static const auto second = TimePrefix<std::chrono::seconds>();
    static const auto minutes = TimePrefix<std::chrono::minutes>();
    static const auto hours = TimePrefix<std::chrono::hours>();

    template <class T>
    Duration operator*(const TimePrefix<T>& /*lhs*/, const double rhs)
    {
        return TimePrefix<T>::convert(rhs);
    }

    template <class T>
    Duration operator*(double rhs, const TimePrefix<T>& /*lhs*/)
    {
        return TimePrefix<T>::convert(rhs);
    }

    template <class T>
    Duration operator*(const TimePrefix<T>& /*lhs*/, const int64_t rhs)
    {
        return TimePrefix<T>::convert(rhs);
    }

    template <class T>
    Duration operator*(int64_t rhs, const TimePrefix<T>& /*lhs*/)
    {
        return TimePrefix<T>::convert(rhs);
    }

    template <class T>
    Duration operator*(const TimePrefix<T>& /*lhs*/, const int32_t rhs)
    {
        return TimePrefix<T>::convert(static_cast<int64_t>(rhs));
    }

    template <class T>
    Duration operator*(int32_t rhs, const TimePrefix<T>& /*lhs*/)
    {
        return TimePrefix<T>::convert(static_cast<int64_t>(rhs));
    }
} // namespace mo

namespace std
{
    MO_EXPORTS std::ostream& operator<<(std::ostream& lhs, const mo::Time& rhs);
#ifndef _MSC_VER
    namespace chrono
    {
        template <class Rep,
                  class Period,
                  class = typename std::enable_if<std::chrono::duration<Rep, Period>::min() <
                                                  std::chrono::duration<Rep, Period>::zero()>::type>
        constexpr std::chrono::duration<Rep, Period> abs(const duration<Rep, Period> d)
        {
            return d >= d.zero() ? d : -d;
        }
    } // namespace chrono
#endif
} // namespace std

namespace cereal
{
    template <class AR>
    double save_minimal(const AR& /*ar*/, const mo::Time& time)
    {
        return time.seconds();
    }

    template <class AR>
    void load_minimal(AR& /*ar*/, mo::Time& time, const double& value)
    {
        time = mo::Time(value * mo::second);
    }
} // namespace cereal

namespace ct
{
    REFLECT_BEGIN(mo::FrameNumber)
        PUBLIC_ACCESS(val)
        MEMBER_FUNCTION("valid", &mo::FrameNumber::valid)
    REFLECT_END;
} // namespace ct
#endif // MO_CORE_TIME_HPP


#include "sound.hpp"
#include "ode_state.hpp"
#include "state.hpp"
#include <boost/numeric/ublas/io.hpp>
#include "potential.hpp"
#include "interpolation.hpp"
#include "monodromy.hpp"

Sound::Sound(const Potential& pot, bool periodic, bool monodromy, double speed_of_sound):
	mDimension( pot.getDimension() ),
	mPeriodicBoundaries( periodic ),
	mTraceMonodromy( monodromy ),
	mScalingFactor( pot.getDimension() ),
	mGridSize( pot.getDimension() ),
	mSpeedOfSound( speed_of_sound ),
	mVelocities( pot.getDimension() ),
	mVelDerivatives( pot.getDimension() * pot.getDimension() )
{
	assert(mTraceMonodromy == false);
	// calculate scale factor
	for(unsigned i = 0; i < mDimension; ++i)
	{
		mScalingFactor[i] = pot.getExtents()[i] / pot.getSupport()[i];
		mGridSize[i] = pot.getExtents()[i];
	}

	// set up grids
	for(unsigned i = 0; i < mDimension; ++i)
	{
		mVelocities[i] = pot.getPotential("velocity"+boost::lexical_cast<std::string>(i)).shallow_copy();
	}

	// the current setup requires us to set the grid as periodic here, independently of
	// whether we actually do periodic tracing.
	for(auto& deriv : mVelocities)
		deriv.setAccessMode(TransformationType::PERIODIC);

	// setup derivatives of v
	for(unsigned i = 0; i < mDimension; ++i)
	{
		std::string base_quantity = "velocity"+boost::lexical_cast<std::string>(i);
		for(unsigned j = 0; j < mDimension; ++j)
		{
			mVelDerivatives[mDimension*i + j] = pot.getDerivative(makeIndexVector(mDimension, {j}), base_quantity).shallow_copy();
		}
	}
}

void Sound::stateUpdate( const GState& state , GState& deriv, double) const
{
	assert( state.dimension() <= 3 );
	assert( state.dimension() == mDimension );
	std::array<double, 3> p;
	for(std::size_t i = 0; i < mDimension; ++i)
	{
		p[i] = state.position()[i] * mScalingFactor[i];
		// we need a safety margin of 1 here, so that interpolation can never
		// access non-existing grid cells.
		if(!mPeriodicBoundaries && (p[i] < 1 || p[i] > mGridSize[i] - 2))
			throw(1);
	}

	const double C = mSpeedOfSound;

	// dR/di = B / |B| + v/c , di = c dt
	// dR/dt = cB / |B| + v
	gen_vect B(mDimension);
	state.velocity().assignTo(B);
	double b = 0;
	for(std::size_t i = 0; i < mDimension; ++i)
	{
		b += B[i] * B[i];
	}
	b = std::sqrt(b);

	deriv.position() = (C/b) * B;
	for(unsigned i = 0; i < mDimension; ++i)
	{
		deriv.position()[i] += linearInterpolate(mVelocities[i], p.data());
	}

	// dB/dt = -grad(B dot v) for c = const
	// dB/dt_i = -d/di (B dot v) = - d/di (B_j v^j) = -B_j dv^j/di
	for(unsigned i = 0; i < mDimension; ++i)
	{
		deriv.velocity()[i] = 0;
		for(unsigned j = 0; j < mDimension; ++j)
		{
			deriv.velocity()[i] -= state.velocity()[j] * linearInterpolate(mVelDerivatives[mDimension*j + i], p.data());
		}
	}
}

bool Sound::hasMonodromy() const
{
	return false;
}

bool Sound::hasPeriodicBoundary() const
{
	return mPeriodicBoundaries;
}

void Sound::normalizeEnergy(State&, double) const
{
	/// \todo figure out what to do with energy
}

double Sound::getEnergy( const State& ) const
{
	return 0;
}


#include <chrono>
#include <cstdint>

#include <fmt/format.h>
#include <mrpt/kinematics/CVehicleVelCmd_DiffDriven.h>
#include <ros/ros.h>
#include <boost/math/constants/constants.hpp>

#include <a4wd2/toolkit/MotorController.h>
#include <a4wd2/toolkit/odometry_provider.h>

#include "laser_scan_provider.h"
#include "mrpt_nav_interface.h"

using namespace std::literals::chrono_literals;
using mrpt::kinematics::CVehicleVelCmd_DiffDriven;

namespace a4wd2
{

mrpt_nav_interface::mrpt_nav_interface(
        toolkit::MotorController& controller, const robot_properties& props,
        const laser_scan_provider& scan_provider,
        const toolkit::odometry_provider& odometry_provider)
    : m_controller{controller},
      m_props(props),
      m_scan_provider(scan_provider),
      m_odometry_provider(odometry_provider)
{
    m_logger = spdlog::get("mrpt_nav_interface");
    if (!m_logger)
    {
        m_logger = spdlog::stdout_color_mt("mrpt_nav_interface");
    }
}

bool mrpt_nav_interface::getCurrentPoseAndSpeeds(mrpt::math::TPose2D& curPose,
                                                 mrpt::math::TTwist2D& curVelGlobal,
                                                 mrpt::system::TTimeStamp& timestamp,
                                                 mrpt::math::TPose2D& curOdometry,
                                                 std::string& frame_id)
{
    if (!m_odometry_provider.get_odometry(curOdometry, curVelGlobal))
    {
        return false;
    }
    curPose = curOdometry;
    timestamp = mrpt::system::now();
    // frame_id is left default

    return true;
}

bool mrpt_nav_interface::changeSpeeds(const mrpt::kinematics::CVehicleVelCmd& vel_cmd)
{
    auto diff_cmd = dynamic_cast<const CVehicleVelCmd_DiffDriven*>(&vel_cmd);
    if (!diff_cmd)
    {
        ROS_ERROR(
                "[changeSpeeds()] velocity command type must be "
                "CVehicleVelCmd_DiffDriven");
        return false;
    }

    auto speed_left = diff_cmd->lin_vel * si::meters_per_second -
                        0.5 * diff_cmd->ang_vel / si::seconds * m_props.axle_length;
    auto speed_right = diff_cmd->lin_vel * si::meters_per_second +
                        0.5 * diff_cmd->ang_vel / si::seconds * m_props.axle_length;

    int32_t left_qpps = to_qpps(speed_left);
    int32_t right_qpps = to_qpps(speed_right);

    m_logger->debug("sending qpps command: m1: {}, m2: {}", right_qpps, left_qpps);
    m_controller.set_target_qpps_left(left_qpps);
    m_controller.set_target_qpps_right(right_qpps);

    return true;
}

bool mrpt_nav_interface::changeSpeedsNOP() { return true; }

bool mrpt_nav_interface::stop(bool isEmergencyStop)
{
    m_controller.stop();
    return true;
}

mrpt::kinematics::CVehicleVelCmd::Ptr mrpt_nav_interface::getEmergencyStopCmd()
{
    auto cmd = mrpt::make_aligned_shared<CVehicleVelCmd_DiffDriven>();
    cmd->setToStop();
    return cmd;
}

mrpt::kinematics::CVehicleVelCmd::Ptr mrpt_nav_interface::getStopCmd()
{
    return getEmergencyStopCmd();
}

bool mrpt_nav_interface::senseObstacles(mrpt::maps::CSimplePointsMap& obstacles,
                                        mrpt::system::TTimeStamp& timestamp)
{
    mrpt::obs::CObservation2DRangeScan scan;
    if (!m_scan_provider.get_range_scan(scan))
    {
        m_logger->warn("no range scan available when sensing obstacles");
        return false;
    }

    obstacles.insertionOptions.addToExistingPointsMap = false;
    obstacles.loadFromRangeScan(scan);
    timestamp = mrpt::system::now();

    // std::vector<float> xs, ys, zs;
    // obstacles.getAllPoints(xs, ys, zs);
    // std::string msg = "[";
    // for (int i = 0; i < xs.size(); ++i)
    //{
    //    msg += fmt::format("x: {}, y: {}, z: {}, \n", xs[i], ys[i], zs[i]);
    //}
    // msg += "]";
    // m_logger->debug("obstacles: {}", msg);

    return true;
}

mrpt::kinematics::CVehicleVelCmd::Ptr mrpt_nav_interface::getAlignCmd(
        const double relative_heading_radians)
{
    using boost::math::constants::pi;

    auto cmd = mrpt::make_aligned_shared<CVehicleVelCmd_DiffDriven>();
    cmd->lin_vel = 0;
    cmd->ang_vel = std::min(10 * std::abs(relative_heading_radians), 0.4 * pi<double>());
    cmd->ang_vel *= relative_heading_radians > 0 ? 1 : -1;

    return cmd;
}

void mrpt_nav_interface::sendApparentCollisionEvent()
{
    // if (!m_navigating) return;

    m_logger->warn("Apparent collision event!!");

    // mrpt::kinematics::CVehicleVelCmd_DiffDriven vel_cmd;
    // vel_cmd.ang_vel = 0;
    // vel_cmd.lin_vel = -0.2;  // m/s
    // changeSpeeds(vel_cmd);

    // std::this_thread::sleep_until(std::chrono::steady_clock::now() + 2s);

    // vel_cmd.ang_vel = 0;
    // vel_cmd.lin_vel = 0;
    // changeSpeeds(vel_cmd);
}

}  // namespace a4wd2


#include <iostream>
#include <fstream>
#include <Eigen/Eigen>
//include the bie header files
#include "material.hh"
#include "precomputed_kernel.hh"
#include "bimat_interface.hh"
#include "infinite_boundary.hh"
//include the fem header files
#include "mesh_Abaqus_multi_faults.hpp"
#include "bcdof.hpp"
#include "bcdof_ptr.hpp"
#include "cal_ke.hpp"
#include "cal_fe_global_vary_ke.hpp"
#include "mapglobal.hpp"
#include "maplocal.hpp"
#include "cal_M.hpp"
#include "Slip_Weakening_lumpM.hpp"
#include "cal_slip_sliprate_angle.hpp"
#include "time_advance.hpp"
#include "BIE_correct.hpp"
#include <math.h>
using namespace Eigen;
using namespace std;

typedef Eigen::Matrix<int, -1, -1,RowMajor> MatrixXi_rm;

double time_fem=0;
double time_bie;
int main() {
    //std::string filename = "abaqus_cpp_two_faults_5degree_0d_flat.inp";
   // std::string filename = "abaqus_Rice_fish_bone_8_25m.inp";
    std::string filename = "abaqus_cpp_three_fault.inp";
    std::cout<<filename<<std::endl;
   // double fault_angle  = 0/180.0*M_PI;
    int dim = 2.0;
    double x_max = 5.0e3;
    double x_min = -5.0e3;
    double dx = 50.0;
    double dy = 50.0;
    std::vector<int> BIE_top, BIE_bot;
    MatrixXd Node;
    MatrixXi_rm Element;
    int num_faults = 3;
    std::vector<std::vector<int>> fault_nodes(2*num_faults);
    mesh_Abaqus_multi_faults(filename, Node, Element, BIE_top, BIE_bot,fault_nodes,num_faults);
    int nx_BIE = BIE_top.size();
    int n_nodes = Node.rows();
    int n_el = Element.rows();
    int Ndofn = 2;
    int Nnel = Element.cols();
    // Material
    double density = 2670.0;
    double v_s =3.464e3;
    double v_p = 6.0e3;
    double G= pow(v_s,2)*density;
    double Lambda = pow(v_p,2)*density-2.0*G;
    double E  = G*(3.0*Lambda+2.0*G)/(Lambda+G);
    double nu = Lambda/(2.0*(Lambda+G));
    // Slip weakening friction parameters
    double Dc = 0.2;
    double mu_d= 0.5;
    double mu_s = 0.6;
    // Intialization
    // disp velocity current and next time step (new)
    VectorXd u_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd u_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd a_n = VectorXd::Zero(n_nodes*Ndofn,1);
    // slip and slip-rate
    std::vector<Eigen::Array<double, -1, 1> > delt_u_n(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > delt_v_n(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > T_c(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > T_0(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > tau_s(num_faults);

    // Vector containing number of elements on each faults (nx)
    std::vector<int> nx_faults(num_faults);
    for (int i=0;i<num_faults;i++)
    {
        nx_faults[i] =fault_nodes[2*i].size();
    }

    for (int i=0;i<num_faults;i++)
    {
        delt_u_n[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        delt_v_n[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        T_c[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        T_0[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        tau_s[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
    }
    VectorXd F_ext_global = VectorXd::Zero(n_nodes*Ndofn,1);
    // Write the x coordiantes for each fault
    std::vector<double> fault_angle(num_faults);
    for (int j=0;j<num_faults; j++)
    {
        VectorXd x = VectorXd::Zero(nx_faults[j], 1);
        VectorXd y = VectorXd::Zero(nx_faults[j], 1);

        for (int i=0; i<x.size(); i++)
        {
            x(i) = Node(fault_nodes[2*j][i],0);
            y(i) = Node(fault_nodes[2*j][i],1);
        }

        //std::ofstream x_output("results/fault_x_coord.txt");
        std::string  x_fault= "results/x_fault_"+std::to_string(j)+".txt";
        std::ofstream x_output(x_fault);
        x_output<<x;
        //std::cout<<x<<std::endl;
        fault_angle[j]= std::atan(std::abs(y.maxCoeff()-y.minCoeff())/std::abs(x.maxCoeff()-x.minCoeff()));
    }

    // Calcualte the resolving shear and nommal stress on each fault
    std::vector<double> sigma_f(num_faults);
    std::vector<double> tau_f(num_faults);
    double sigma_xx = 0.0e6;
    double sigma_yy = -50.0e6;
    double tau_xy = -27.5e6;
    for (int i=0; i<num_faults; i++)
    {
        double c = std::cos(fault_angle[i]);
        double s = std::sin(fault_angle[i]);

        sigma_f[i] = sigma_xx*s*s+sigma_yy*c*c+tau_xy*s*c;
        tau_f[i] = -sigma_xx*s*c+sigma_yy*s*c-tau_xy*(c*c-s*s);
    }

   // std::cout<<x<<std::endl;
    // Setting intial stress on the all fault according the the stress depcomposition.
    for (int j=0; j<num_faults;j++)
    {
        for (int i=0; i<nx_faults[j]; i++)
        {
            T_0[j](2*i+1) = sigma_f[j];
            T_0[j](2*i) = tau_f[j];
        }
    }
    //Adding pertubation to the main fault 0
    VectorXd x_main = VectorXd::Zero(nx_faults[0], 1);

    for (int i=0; i<x_main.size(); i++)
    {
        x_main(i) = Node(fault_nodes[0][i],0);
    }

    for (int i=0 ; i<nx_faults[0]; i++)
    {
        if ((x_main(i)<=(x_main.minCoeff()+x_main.maxCoeff())/2+0.6e3)&&(x_main(i)>=(x_main.minCoeff()+x_main.maxCoeff())/2-0.6e3))
        {
            T_0[0](2*i) = 31.0e6;
        }
    }

    // Get the index degree of freedome for each element
    VectorXi index_el = VectorXi::Zero(Ndofn*Nnel,1);
    MatrixXi index_store = MatrixXi::Zero(Nnel*Ndofn,n_el);
    for (int i=0;i<n_el;i++)
    {
        bcdof(Element.row(i),dim,index_el);
        index_store.col(i) = index_el;
    }
  //  VectorXi BIE_top_surf_index = VectorXi::Zero(Ndofn*(BIE_top_surf_nodes.size()),1);
   // VectorXi BIE_bot_surf_index = VectorXi::Zero(Ndofn*(BIE_bot_surf_nodes.size()),1);
    VectorXi BIE_top_surf_index = VectorXi::Zero(Ndofn*(BIE_top.size()),1);
    VectorXi BIE_bot_surf_index = VectorXi::Zero(Ndofn*(BIE_bot.size()),1);
    std::vector<Eigen::ArrayXi> Fault_surf_index(num_faults*2);
    for (int i=0; i <num_faults*2; i++)
    {
        Fault_surf_index[i] = ArrayXi::Zero(Ndofn*(fault_nodes[i].size()),1);
    }

    // Getting the faults DOF index
    for (int i=0; i<num_faults*2;i++)
    {
        bcdof_ptr(fault_nodes[i], dim, Fault_surf_index[i].data());
    }

    bcdof_ptr(BIE_top,dim,BIE_top_surf_index.data());
    bcdof_ptr(BIE_bot,dim,BIE_bot_surf_index.data());
    // Calculating the Global Mass Vector (lumped mass)
    // Element stiffness matrix,Element mass
    std::vector<MatrixXd> ke_store(n_el);
    MatrixXd M_el = MatrixXd::Zero(Nnel*Ndofn,Nnel*Ndofn);
    VectorXd M_el_vec = VectorXd::Zero(Nnel*Ndofn,1);
    VectorXd M_global_vec=VectorXd::Zero(n_nodes*Ndofn,1);
    std::vector<double> dx_min_store;
    for (int i=0; i<n_el;i++)
    {
        MatrixXd coord = MatrixXd::Zero(4,2);
        VectorXi Element_0= Element.row(i);
        coord.row(0) = Node.row(Element_0(0));
        coord.row(1) = Node.row(Element_0(1));
        coord.row(2) = Node.row(Element_0(2));
        coord.row(3) = Node.row(Element_0(3));
        VectorXd dx_cal = VectorXd::Zero(4,1);
        dx_cal(0)=std::sqrt(pow(std::abs(coord(0,0)-coord(1,0)),2)+pow(std::abs(coord(0,1)-coord(1,1)),2));
        dx_cal(1)=std::sqrt(pow(std::abs(coord(1,0)-coord(2,0)),2)+pow(std::abs(coord(1,1)-coord(2,1)),2));
        dx_cal(2)=std::sqrt(pow(std::abs(coord(2,0)-coord(3,0)),2)+pow(std::abs(coord(2,1)-coord(3,1)),2));
        dx_cal(3)=std::sqrt(pow(std::abs(coord(3,0)-coord(0,0)),2)+pow(std::abs(coord(3,1)-coord(0,1)),2));
        double dx_min = dx_cal.minCoeff();
        dx_min_store.push_back(dx_min);
       // std::cout<<dx_cal<<std::endl;

        cal_ke (coord,E,nu,ke_store[i]);
        cal_M(coord, density,M_el);
        M_el_vec = M_el.rowwise().sum();
        index_el = index_store.col(i);
        mapglobal(index_el,M_global_vec,M_el_vec);

    }
    double dx_min = *std::min_element(std::begin(dx_min_store), std::end(dx_min_store));
    // Time
    double alpha = 0.4;
    double dt = alpha*dx_min/v_p;
    // Reyleigh Damping
    double beta =0.1;
    double q = beta*dt;
    double time_run = 6.0;
    int numt = time_run/dt;
    //
    std::ofstream time_output("results/time.txt");
    time_output<<time_run<<"\n";
    time_output<<dt<<"\n";
    time_output<<numt<<std::endl;

    //numt = 1;
    VectorXd time = dt*VectorXd::LinSpaced(numt,1,numt);
//    MatrixXd ke = MatrixXd::Zero(8,8);
//    MatrixXd coord = MatrixXd::Zero(4,2);
//    VectorXi Element_0= Element.row(0);
//    coord.row(0) = Node.row(Element_0(0));
//    coord.row(1) = Node.row(Element_0(1));
//    coord.row(2) = Node.row(Element_0(2));
//    coord.row(3) = Node.row(Element_0(3));
//    cal_ke (coord,E,nu,ke);

    // BIE part initiation
    // Setting up the material property for the BIE code
    Material BIE_top_mat = Material(E,nu,density);
    Material BIE_bot_mat = Material(E,nu,density);
    double length = x_max-x_min;
    // infinte bc BIE call infinite_boundary.cc
    PrecomputedKernel h11("kernels/nu_.25_h11.dat");
    PrecomputedKernel h12("kernels/nu_.25_k12.dat");
    PrecomputedKernel h22("kernels/nu_.25_h22.dat");
    InfiniteBoundary BIE_inf_top(length,nx_BIE,1.0,&BIE_top_mat,&h11,&h12,&h22);
    InfiniteBoundary BIE_inf_bot(length,nx_BIE,-1.0,&BIE_bot_mat,&h11,&h12,&h22);
    // BIE setting time step
    BIE_inf_top.setTimeStep(dt);
    BIE_inf_bot.setTimeStep(dt);
    // BIE initialization
    BIE_inf_top.init();
    BIE_inf_bot.init();
    printf("ready to start\n");
    // Output
    ofstream file;
    file.open("results/num_nodes_fault.bin",ios::binary);
    file.write((char*)(nx_faults.data()),nx_faults.size()*sizeof(int));
    file.close();

    file.open("results/u_n.bin",ios::binary);
   // file.write((char*)(u_n.data()),u_n.size()*sizeof(double));
    file.close();
    file.open("results/v_n.bin",ios::binary);
    // file.write((char*)(u_n.data()),u_n.size()*sizeof(double));
    file.close();

    for (int i=0;i<num_faults;i++)
    {
        std::string slip = "results/slip_"+std::to_string(i)+".bin";
        file.open(slip);
        file.close();
        std::string slip_rate = "results/slip_rate_"+std::to_string(i)+".bin";
        file.open(slip_rate);
        file.close();
        std::string shear = "results/shear_"+std::to_string(i)+".bin";
        file.open(shear);
        file.close();
    }
    std::ofstream Element_output("results/Element.txt");
    std::ofstream Node_output("results/Node.txt");
    Node_output<<Node;
    Element_output<<Element;
    //    std::ofstream shear_x("results/shear.txt");
    //double start = omp_get_wtime();

    // Main time loop
    for (int j=0;j<numt;j++)
    {
        // Compute the global internal force
        VectorXd fe_global= VectorXd::Zero(n_nodes*Ndofn,1);
        cal_fe_global_vary_ke(n_nodes, n_el, index_store, q, u_n, v_n, Ndofn, ke_store, fe_global);
        // Friction subroutine
        VectorXd F_total = F_ext_global-fe_global;
        for (int i=0 ; i<num_faults; i++)
        {
            VectorXd F_fault = VectorXd::Zero(Ndofn*(nx_faults[i]),1);
            VectorXd M_pos = VectorXd::Zero(nx_faults[i], 1);
            VectorXd M_neg = VectorXd::Zero(nx_faults[i],1);
            maplocal(Fault_surf_index[2*i], M_global_vec, M_pos);
            maplocal(Fault_surf_index[2*i+1], M_global_vec, M_neg);
            //std::cout<<M_pos<<std::endl;
            Slip_Weakening_lumpM(M_global_vec, Fault_surf_index[2*i], Fault_surf_index[2*i+1], fe_global, dt, dx, dy, nx_faults[i]-1, delt_v_n[i], delt_u_n[i], T_0[i], tau_s[i], mu_s, mu_d, Dc, Ndofn, M_pos, M_neg , F_fault, T_c[i], fault_angle[i]);
            mapglobal(Fault_surf_index[2*i],F_total,-F_fault);
            mapglobal(Fault_surf_index[2*i+1],F_total,F_fault);
        }

        // Central Difference Time integration
        time_advance(u_n, v_n, F_total, M_global_vec, dt);
        // Get the slip and slip rate

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

            cal_slip_slip_rate_angle(u_n, v_n,  Fault_surf_index[2*i], Fault_surf_index[2*i+1] , Ndofn, nx_faults[i]-1, delt_u_n[i], delt_v_n[i],fault_angle[i]);
        }
        // Correct the BIE surf nodes solutions from FEM with the BIE solution
        BIE_correct(BIE_top_surf_index, BIE_bot_surf_index, fe_global, Ndofn, nx_BIE-1, dx, BIE_inf_top, BIE_inf_bot, u_n, v_n);

      //  if (j%4==3)
       // {
        for (int i=0;i<num_faults;i++)
        {
            std::string slip = "results/slip_"+std::to_string(i)+".bin";
            file.open(slip,ios::binary | ios::app);
            file.write((char*)(delt_u_n[i].data()),delt_u_n[i].size()*sizeof(double));
            file.close();

            std::string slip_rate = "results/slip_rate_"+std::to_string(i)+".bin";
            file.open(slip_rate,ios::binary | ios::app);
            file.write((char*)(delt_v_n[i].data()),delt_v_n[i].size()*sizeof(double));
            file.close();

            std::string shear = "results/shear_"+std::to_string(i)+".bin";
            file.open(shear,ios::binary | ios::app);
            file.write((char*)(T_c[i].data()),T_c[i].size()*sizeof(double));
            file.close();
        }
        file.open("results/u_n.bin",ios::binary | ios::app);
        file.write((char*)(u_n.data()),u_n.size()*sizeof(double));
        file.close();
        file.open("results/v_n.bin",ios::binary | ios::app);
        file.write((char*)(v_n.data()),v_n.size()*sizeof(double));
        file.close();
       // }
        printf("Simulation time = %f\n",time(j));
      //  double end_t = omp_get_wtime();
       // std::cout<<"time_cpu_t="<<end_t-start<<std::endl;
    //   std::cout<<"time_fem="<< time_fem<<std::endl;
    //   std::cout<<"time_bie="<< time_bie<<std::endl;
    }
    //double end = omp_get_wtime();
    //std::cout<<"time_cpu="<<end-start<<std::endl;
    return 0;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_RSQRT_HPP_INCLUDED

#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return bs::rec(bs::sqrt(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bd::scalar_< bd::single_<A0> >
                         )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0 a0) const BOOST_NOEXCEPT
    {
      typedef bd::as_integer_t<A0> i_t;

      // Quake III Arena RSQRT approximation
      i_t x = bitwise_cast<i_t>(a0);
      i_t y = 0x5f3759df - (x >> 1);

      // make negative values be NaN
      y |= x >> (sizeof(i_t)*CHAR_BIT-1);

      A0 x2 = a0 * 0.5f;
      A0 y2 = bitwise_cast<A0>(y);

      // Newton-Rhapson refinement steps: 2 NR steps for precision purpose
      y2    = y2 * ( 1.5f - ( x2 * y2 * y2 ) );
      return  y2 * ( 1.5f - ( x2 * y2 * y2 ) );
    }
  };
  BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bd::scalar_< bd::double_<A0> >
                         )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0 a0) const BOOST_NOEXCEPT
    {
      return bs::rsqrt(a0);
    }
  };
} } }

#endif


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_EXPO_BASE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_EXPO_BASE_HPP_INCLUDED

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/if_allbits_else.hpp>
#include <boost/simd/function/is_nan.hpp>
#endif

#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/arch/common/detail/scalar/expo_reduction.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace detail
{
  template<typename A0, typename Tag>
  struct exponential < A0, Tag, tag::simd_type, double>
  {
    typedef exp_reduction<A0,Tag>                        reduc_t;
    // compute exp(ax)
    static BOOST_FORCEINLINE A0 expa(const A0& a0)
    {
      A0 hi, lo, x;
      A0 k = reduc_t::reduce(a0, hi, lo, x);
      A0 c = reduc_t::approx(x);
      c = reduc_t::finalize(x, c, hi, lo);
      c = if_zero_else(reduc_t::isleminlog(a0), ldexp(c, k));
      c = if_else(reduc_t::isgemaxlog(a0), bs::Inf<A0>(), c);
#ifdef BOOST_SIMD_NO_INVALIDS
      return c;
#else
      return if_allbits_else(is_nan(a0), c);
#endif
    }
  };

  template < typename A0, typename Tag >
  struct exponential < A0, Tag, tag::simd_type, float>
  {
    typedef exp_reduction<A0,Tag>                        reduc_t;
    // compute exp(ax)
    static BOOST_FORCEINLINE A0 expa(const A0& a0)
    {
      A0 x;
      A0 k = reduc_t::reduce(a0, x);
      x = reduc_t::approx(x);
      x = if_zero_else(reduc_t::isleminlog(a0), ldexp(x, k));
      x = if_else(reduc_t::isgemaxlog(a0), bs::Inf<A0>(), x);
#ifdef BOOST_SIMD_NO_INVALIDS
      return x;
#else
      return if_allbits_else(is_nan(a0), x);
#endif
    }
  };
} } }

#endif


#include "camera_calibration.h"
#include <Eigen/Cholesky>
#include <iostream>
#include <algorithm>
#include <limits>
#include "field.h"
#include "geomalgo.h"

CameraParameters::CameraParameters(RoboCupCalibrationHalfField & _field) : field(_field), p_alpha(Eigen::VectorXd(1))
{
  focal_length = new VarDouble("focal length", 500.0);
  principal_point_x = new VarDouble("principal point x", 390.0);
  principal_point_y = new VarDouble("principal point y", 290.0);
  distortion = new VarDouble("distortion", 0.0, 0.0, 2.0);
  //do not overwrite min/max ranges with values from config file
  distortion->addFlags(VARTYPE_FLAG_NOLOAD_ATTRIBUTES);

  q0 = new VarDouble("q0", 0.7);
  q1 = new VarDouble("q1", -0.7);
  q2 = new VarDouble("q2", .0);
  q3 = new VarDouble("q3", .0);

  tx = new VarDouble("tx", 0);
  ty = new VarDouble("ty", 1250);

  tz = new VarDouble("tz", 3500, 0, 5000);
  //do not overwrite min/max ranges with values from config file
  tz->addFlags(VARTYPE_FLAG_NOLOAD_ATTRIBUTES);

  additional_calibration_information = new AdditionalCalibrationInformation();

  q_rotate180 = Quaternion<double>(0, 0, 1.0,0);
}

CameraParameters::~CameraParameters()
{
  delete focal_length;
  delete principal_point_x;
  delete principal_point_y;
  delete distortion;
  delete q0;
  delete q1;
  delete q2;
  delete q3;
  delete tx;
  delete ty;
  delete tz;
  delete additional_calibration_information;
}

#ifndef NO_PROTOBUFFERS
void CameraParameters::toProtoBuffer(SSL_GeometryCameraCalibration & buffer, int camera_id) const {
  buffer.set_focal_length(focal_length->getDouble());
  buffer.set_principal_point_x(principal_point_x->getDouble());
  buffer.set_principal_point_y(principal_point_y->getDouble());
  buffer.set_distortion(distortion->getDouble());
  buffer.set_q0(q0->getDouble());
  buffer.set_q1(q1->getDouble());
  buffer.set_q2(q2->getDouble());
  buffer.set_q3(q3->getDouble());
  buffer.set_tx(tx->getDouble());
  buffer.set_ty(ty->getDouble());
  buffer.set_tz(tz->getDouble());
  buffer.set_camera_id(camera_id);

  //--Set derived parameters:
  //compute camera world coordinates:
  Quaternion<double> q;
  q.set(q0->getDouble(),q1->getDouble(),q2->getDouble(),q3->getDouble());
  q.invert();

  GVector::vector3d<double> v_in(tx->getDouble(),ty->getDouble(),tz->getDouble());
  v_in=(-(v_in));

  GVector::vector3d<double> v_out = q.rotateVectorByQuaternion(v_in);
  buffer.set_derived_camera_world_tx(v_out.x);
  buffer.set_derived_camera_world_ty(v_out.y);
  buffer.set_derived_camera_world_tz(v_out.z);

}

void CameraParameters::fromProtoBuffer(const SSL_GeometryCameraCalibration & buffer) {
  focal_length->setDouble(buffer.focal_length());
  principal_point_x->setDouble(buffer.principal_point_x());
  principal_point_y->setDouble(buffer.principal_point_y());
  distortion->setDouble(buffer.distortion());
  q0->setDouble(buffer.q0());
  q1->setDouble(buffer.q1());
  q2->setDouble(buffer.q2());
  q3->setDouble(buffer.q3());
  tx->setDouble(buffer.tx());
  ty->setDouble(buffer.ty());
  tz->setDouble(buffer.tz());
}
#endif

void CameraParameters::addSettingsToList(VarList& list)
{
  list.addChild(focal_length);
  list.addChild(principal_point_x);
  list.addChild(principal_point_y);
  list.addChild(distortion);
  list.addChild(q0);
  list.addChild(q1);
  list.addChild(q2);
  list.addChild(q3);
  list.addChild(tx);
  list.addChild(ty);
  list.addChild(tz);
}

double CameraParameters::radialDistortion(double ru) const
{
  if((distortion->getDouble())<=DBL_MIN)
    return ru;
  double rd = 0;
  double a = distortion->getDouble();
  double b = -9.0*a*a*ru + a*sqrt(a*(12.0 + 81.0*a*ru*ru));
  //Pre-computing numerical constants to optimize for speed...
  //b = b<0.0?-pow(b,1.0/3.0):pow(b,1.0/3.0);
  //rd = pow(2.0/3.0,1.0/3.0)/b - b/(pow(2.0*3.0*3.0,1.0/3.0)*a);
  b = b<0.0?-pow(-b,0.33333333333333333333333333333333333333333333333333):pow(b,0.33333333333333333333333333333333333333333333333333);
  rd = 0.87358046473629886904722042681399875674647588190788/b - b/(2.62074139420889660714166128044199627023942764572363*a);
  return rd;
}

double CameraParameters::radialDistortion(double ru, double dist) const
{
  if(dist<=DBL_MIN)
    return ru;
  double rd = 0;
  double a = dist;
  double b = -9.0*a*a*ru + a*sqrt(a*(12.0 + 81.0*a*ru*ru));
  //Pre-computing numerical constants to optimize for speed...
  //b = b<0.0?-pow(b,1.0/3.0):pow(b,1.0/3.0);
  //rd = pow(2.0/3.0,1.0/3.0)/b - b/(pow(2.0*3.0*3.0,1.0/3.0)*a);
  b = b<0.0?-pow(-b,0.33333333333333333333333333333333333333333333333333):pow(b,0.33333333333333333333333333333333333333333333333333);
  rd = 0.87358046473629886904722042681399875674647588190788/b - b/(2.62074139420889660714166128044199627023942764572363*a);
  return rd;
}

double CameraParameters::radialDistortionInv(double rd) const
{
  double ru = rd*(1.0+rd*rd*distortion->getDouble());
  return ru;
}

void CameraParameters::radialDistortionInv(GVector::vector2d<double> &pu, const GVector::vector2d<double> &pd) const
{
  double ru = radialDistortionInv(pd.length());
  pu = pd;
  pu = pu.norm(ru);
}

void CameraParameters::radialDistortion(const GVector::vector2d<double> pu, GVector::vector2d<double> &pd) const
{
  double rd = radialDistortion(pu.length());
  pd = pu;
  pd = pd.norm(rd);
}

void CameraParameters::radialDistortion(const GVector::vector2d<double> pu, GVector::vector2d<double> &pd, double dist) const
{
  double rd = radialDistortion(pu.length(), dist);
  pd = pu;
  pd = pd.norm(rd);
}

void CameraParameters::field2image(const GVector::vector3d<double> &p_f, GVector::vector2d<double> &p_i) const
{
  Quaternion<double> q_field2cam = Quaternion<double>(q0->getDouble(),q1->getDouble(),q2->getDouble(),q3->getDouble());
  q_field2cam.norm();
  GVector::vector3d<double> translation = GVector::vector3d<double>(tx->getDouble(),ty->getDouble(),tz->getDouble());

  // First transform the point from the field into the coordinate system of the camera
  GVector::vector3d<double> p_c = q_field2cam.rotateVectorByQuaternion(p_f) + translation;
  GVector::vector2d<double> p_un = GVector::vector2d<double>(p_c.x/p_c.z, p_c.y/p_c.z);

  // Apply distortion
  GVector::vector2d<double> p_d;
  radialDistortion(p_un,p_d);

  // Then project from the camera coordinate system onto the image plane using the instrinsic parameters
  p_i = focal_length->getDouble() * p_d +
      GVector::vector2d<double>(principal_point_x->getDouble(), principal_point_y->getDouble());
}

void CameraParameters::field2image(GVector::vector3d<double> &p_f, GVector::vector2d<double> &p_i, Eigen::VectorXd &p)
{
  Quaternion<double> q_field2cam = Quaternion<double>(q0->getDouble(),q1->getDouble(),q2->getDouble(),q3->getDouble());
  q_field2cam.norm();
  GVector::vector3d<double> translation = GVector::vector3d<double>(tx->getDouble(),ty->getDouble(),tz->getDouble());

  GVector::vector3d<double> aa_diff(p[Q_1], p[Q_2], p[Q_3]);
  GVector::vector3d<double> t_diff(p[T_1], p[T_2], p[T_3]);

  // Create a quaternion out of the 3D rotational representation
  Quaternion<double> q_diff;
  q_diff.setAxis(aa_diff.norm(), aa_diff.length());

  // First transform the point from the field into the coordinate system of the camera
  GVector::vector3d<double> p_c = (q_diff * q_field2cam ).rotateVectorByQuaternion(p_f) + translation + t_diff;
  GVector::vector2d<double> p_un = GVector::vector2d<double>(p_c.x/p_c.z, p_c.y/p_c.z);

  // Apply distortion
  GVector::vector2d<double> p_d;
  radialDistortion(p_un,p_d,distortion->getDouble() + p[DIST]);

  // Then project from the camera coordinate system onto the image plane using the instrinsic parameters
  p_i = (focal_length->getDouble() + p[FOCAL_LENGTH]) * p_d +
      GVector::vector2d<double>(principal_point_x->getDouble() + p[PP_X], principal_point_y->getDouble() + p[PP_Y]);
}

void CameraParameters::image2field(GVector::vector3d<double> &p_f, GVector::vector2d<double> &p_i, double z) const
{
  // Undo scaling and offset
  GVector::vector2d<double> p_d((p_i.x - principal_point_x->getDouble()) / focal_length->getDouble(),
                                  (p_i.y - principal_point_y->getDouble()) / focal_length->getDouble());

  // Compensate for distortion (undistort)
  GVector::vector2d<double> p_un;
  radialDistortionInv(p_un,p_d);

  // Now we got a ray on the z axis
  GVector::vector3d<double> v(p_un.x, p_un.y, 1);

  // Transform this ray into world coordinates
  Quaternion<double> q_field2cam = Quaternion<double>(q0->getDouble(),q1->getDouble(),q2->getDouble(),q3->getDouble());
  q_field2cam.norm();
  GVector::vector3d<double> translation = GVector::vector3d<double>(tx->getDouble(),ty->getDouble(),tz->getDouble());

  Quaternion<double> q_field2cam_inv = q_field2cam;
  q_field2cam_inv.invert();
  GVector::vector3d<double> v_in_w = q_field2cam_inv.rotateVectorByQuaternion(v);
  GVector::vector3d<double> zero_in_w = q_field2cam_inv.rotateVectorByQuaternion(GVector::vector3d<double>(0,0,0) - translation);

  // Compute the the point where the rays intersects the field
  double t = GVector::ray_plane_intersect(GVector::vector3d<double>(0,0,z), GVector::vector3d<double>(0,0,1).norm(), zero_in_w, v_in_w.norm());

  // Set p_f
  p_f = zero_in_w + v_in_w.norm() * t;
}

double CameraParameters::calc_chisqr(std::vector<GVector::vector3d<double> > &p_f, std::vector<GVector::vector2d<double> > &p_i, Eigen::VectorXd &p, int cal_type)
{
  assert(p_f.size() == p_i.size());

  double cov_cx_inv = 1 / additional_calibration_information->cov_corner_x->getDouble();
  double cov_cy_inv = 1 / additional_calibration_information->cov_corner_y->getDouble();

  double cov_lsx_inv = 1 / additional_calibration_information->cov_ls_x->getDouble();
  double cov_lsy_inv = 1 / additional_calibration_information->cov_ls_y->getDouble();

  double chisqr(0);

  // Iterate over manual points
  std::vector<GVector::vector3d<double> >::iterator it_p_f  = p_f.begin();
  std::vector<GVector::vector2d<double> >::iterator it_p_i  = p_i.begin();

  for(; it_p_f != p_f.end(); it_p_f++, it_p_i++)
  {
    GVector::vector2d<double> proj_p;
    field2image(*it_p_f, proj_p, p);
    chisqr += (proj_p.x - it_p_i->x) * (proj_p.x - it_p_i->x) * cov_cx_inv +
        (proj_p.y - it_p_i->y) * (proj_p.y - it_p_i->y) * cov_cy_inv;
  }

  // Iterate of line edge points, but only when performing a full estimation
  if (cal_type & FULL_ESTIMATION)
  {
    std::vector<CalibrationData>::iterator ls_it = calibrationSegments.begin();

    int i = 0;
    for (; ls_it != calibrationSegments.end(); ls_it++)
    {
      std::vector< std::pair<GVector::vector2d<double>,bool> >::iterator imgPts_it = (*ls_it).imgPts.begin();
      for(; imgPts_it != (*ls_it).imgPts.end(); imgPts_it++)
      {
        // Integrate only if a valid point on line
        if (imgPts_it->second)
        {
          GVector::vector2d<double> proj_p;
          double alpha = p_alpha(i) + p(STATE_SPACE_DIMENSION + i);

          // Calculate point on segment
          GVector::vector3d<double> alpha_point;
          if(ls_it->straightLine){
            alpha_point = alpha * (*ls_it).p1 + (1.0 - alpha) * (*ls_it).p2;
          }else{
            double theta = alpha * (*ls_it).theta1 + (1.0 - alpha) * (*ls_it).theta2;
            alpha_point = ls_it->center + ls_it->radius*GVector::vector3d<double>(cos(theta),sin(theta),0.0);
          }

          // Project into image plane
          field2image(alpha_point, proj_p, p);

          chisqr += (proj_p.x-imgPts_it->first.x) * (proj_p.x-imgPts_it->first.x) * cov_lsx_inv +
              (proj_p.y - imgPts_it->first.y) * (proj_p.y - imgPts_it->first.y) * cov_lsy_inv;
          i++;
        }
      }
    }
  }

  return chisqr;
}

void CameraParameters::do_calibration(int cal_type)
{
  std::vector<GVector::vector3d<double> > p_f;
  std::vector<GVector::vector2d<double> > p_i;

  AdditionalCalibrationInformation* aci = additional_calibration_information;

  p_i.push_back(GVector::vector2d<double>(aci->left_corner_image_x->getDouble(), aci->left_corner_image_y->getDouble()));
  p_i.push_back(GVector::vector2d<double>(aci->right_corner_image_x->getDouble(), aci->right_corner_image_y->getDouble()));
  p_i.push_back(GVector::vector2d<double>(aci->left_centerline_image_x->getDouble(), aci->left_centerline_image_y->getDouble()));
  p_i.push_back(GVector::vector2d<double>(aci->right_centerline_image_x->getDouble(), aci->right_centerline_image_y->getDouble()));

  p_f.push_back(GVector::vector3d<double>(field.left_corner_x->getDouble(), field.left_corner_y->getDouble(),0));
  p_f.push_back(GVector::vector3d<double>(field.right_corner_x->getDouble(), field.right_corner_y->getDouble(),0));
  p_f.push_back(GVector::vector3d<double>(field.left_centerline_x->getDouble(), field.left_centerline_y->getDouble(),0));
  p_f.push_back(GVector::vector3d<double>(field.right_centerline_x->getDouble(), field.right_centerline_y->getDouble(),0));

  calibrate(p_f, p_i, cal_type);
}

void CameraParameters::reset()
{
  focal_length->resetToDefault();
  principal_point_x->resetToDefault();
  principal_point_y->resetToDefault();
  distortion->resetToDefault();
  tx->resetToDefault();
  ty->resetToDefault();
  tz->resetToDefault();
  q0->resetToDefault();
  q1->resetToDefault();
  q2->resetToDefault();
  q3->resetToDefault();
}

void CameraParameters::calibrate(std::vector<GVector::vector3d<double> > &p_f, std::vector<GVector::vector2d<double> > &p_i, int cal_type)
{
  assert(p_f.size() == p_i.size());
  assert(cal_type != 0);

  p_to_est.clear();
  p_to_est.push_back(FOCAL_LENGTH);
  p_to_est.push_back(Q_1);
  p_to_est.push_back(Q_2);
  p_to_est.push_back(Q_3);
  p_to_est.push_back(T_1);
  p_to_est.push_back(T_2);

  int num_alpha(0);

  if (cal_type & FULL_ESTIMATION)
  {
    int count_alpha(0); //The number of well detected line segment points
    std::vector<CalibrationData>::iterator ls_it = calibrationSegments.begin();
    for (; ls_it != calibrationSegments.end(); ls_it++)
    {
      std::vector< std::pair<GVector::vector2d<double>,bool> >::iterator pts_it = (*ls_it).imgPts.begin();
      for(; pts_it != (*ls_it).imgPts.end(); pts_it++)
      {
        if (pts_it->second)
          count_alpha ++;
      }
    }

    if (count_alpha > 0)
    {
      p_alpha = Eigen::VectorXd(count_alpha);

      count_alpha = 0;
      ls_it = calibrationSegments.begin();
      for (; ls_it != calibrationSegments.end(); ls_it++)
      {
        std::vector< std::pair<GVector::vector2d<double>,bool> >::iterator pts_it = (*ls_it).imgPts.begin();
        std::vector< double >::iterator alphas_it = (*ls_it).alphas.begin();
        for(; pts_it != (*ls_it).imgPts.end() && alphas_it != (*ls_it).alphas.end(); pts_it++, alphas_it++)
        {
          if (pts_it->second)
          {
            p_alpha(count_alpha++) = (*alphas_it);
          }
        }
      }
    }

    p_to_est.push_back(PP_X);
    p_to_est.push_back(PP_Y);
    p_to_est.push_back(DIST);

    num_alpha = count_alpha;
  }

  double lambda(0.01);

  Eigen::VectorXd p(STATE_SPACE_DIMENSION + num_alpha);
  p.setZero();

  // Calculate first chisqr for all points using the start parameters
  double old_chisqr = calc_chisqr(p_f, p_i, p, cal_type);

#ifndef NDEBUG
  std::cerr << "Chi-square: "<< old_chisqr << std::endl;
#endif

  // Create and fill corner measurement covariance matrix
  Eigen::Matrix2d cov_corner_inv;
  cov_corner_inv << 1 / additional_calibration_information->cov_corner_x->getDouble(), 0 , 0 , 1 / additional_calibration_information->cov_corner_y->getDouble();

  // Create and fill line segment measurement covariance matrix
  Eigen::Matrix2d cov_ls_inv;
  cov_ls_inv << 1 / additional_calibration_information->cov_ls_x->getDouble(), 0 , 0 , 1 / additional_calibration_information->cov_ls_y->getDouble();

  // Matrices for A, b and the Jacobian J
  Eigen::MatrixXd alpha(STATE_SPACE_DIMENSION + num_alpha, STATE_SPACE_DIMENSION + num_alpha);
  Eigen::VectorXd beta(STATE_SPACE_DIMENSION + num_alpha, 1);
  Eigen::MatrixXd J(2, STATE_SPACE_DIMENSION + num_alpha);

  bool stop_optimization(false);
  int convergence_counter(0);
  double t_start=GetTimeSec();
  while(!stop_optimization) {
    // Calculate Jacobi-Matrix, alpha and beta
    // Iterate over alle point pairs
    std::vector<GVector::vector3d<double> >::iterator it_p_f  = p_f.begin();
    std::vector<GVector::vector2d<double> >::iterator it_p_i  = p_i.begin();

    double epsilon = sqrt(std::numeric_limits<double>::epsilon());

    alpha.setZero();
    beta.setZero();

    for(; it_p_f != p_f.end(); it_p_f++, it_p_i++)
    {
      J.setZero();

      GVector::vector2d<double> proj_p;
      field2image(*it_p_f, proj_p, p);
      proj_p = proj_p - *it_p_i;

      std::vector<int>::iterator it = p_to_est.begin();
      for(; it != p_to_est.end(); it++)
      {
        int i = *it;

        Eigen::VectorXd p_diff = p;
        p_diff(i) = p_diff(i) + epsilon;

        GVector::vector2d<double> proj_p_diff;
        field2image(*it_p_f, proj_p_diff, p_diff);
        J(0,i) = ((proj_p_diff.x - (*it_p_i).x) - proj_p.x) / epsilon;
        J(1,i) = ((proj_p_diff.y - (*it_p_i).y) - proj_p.y) / epsilon;
      }

      alpha += J.transpose() * cov_corner_inv * J;
      beta += J.transpose() * cov_corner_inv * Eigen::Vector2d(proj_p.x, proj_p.y);
    }

    if (cal_type & FULL_ESTIMATION)
    {
      // First, calculate how many alpha we need to estimate
      std::vector<CalibrationData>::iterator ls_it = calibrationSegments.begin();

      int i = 0;
      for (; ls_it != calibrationSegments.end(); ls_it++)
      {
        std::vector< std::pair<GVector::vector2d<double>,bool> >::iterator pts_it = (*ls_it).imgPts.begin();
        for(; pts_it != (*ls_it).imgPts.end(); pts_it++)
        {
          if(pts_it->second)
          {
            GVector::vector2d<double> proj_p;
            GVector::vector3d<double >alpha_point;
            if(ls_it->straightLine){
              alpha_point = p_alpha(i) * (*ls_it).p1 + (1 - p_alpha(i)) * (*ls_it).p2;
            }else{
              double theta = p_alpha(i) * (*ls_it).theta1 + (1.0 - p_alpha(i)) * (*ls_it).theta2;
              alpha_point = ls_it->center + ls_it->radius*GVector::vector3d<double>(cos(theta),sin(theta),0.0);
            }
            field2image(alpha_point, proj_p, p);
            proj_p = proj_p - (*pts_it).first;

            J.setZero();

            std::vector<int>::iterator it = p_to_est.begin();
            for(; it != p_to_est.end(); it++)
            {
              int j = *it;

              Eigen::VectorXd p_diff = p;
              p_diff(j) = p_diff(j) + epsilon;

              GVector::vector2d<double> proj_p_diff;
              field2image(alpha_point, proj_p_diff, p_diff);
              J(0,j) = ((proj_p_diff.x - (*pts_it).first.x) - proj_p.x) / epsilon;
              J(1,j) = ((proj_p_diff.y - (*pts_it).first.y) - proj_p.y) / epsilon;
            }

            double my_alpha = p_alpha(i) + epsilon;
            if(ls_it->straightLine){
              alpha_point = my_alpha * (*ls_it).p1 + (1 - my_alpha) * (*ls_it).p2;
            }else{
              double theta = my_alpha * (*ls_it).theta1 + (1.0 - my_alpha) * (*ls_it).theta2;
              alpha_point = ls_it->center + ls_it->radius*GVector::vector3d<double>(cos(theta),sin(theta),0.0);
            }

            GVector::vector2d<double> proj_p_diff;
            field2image(alpha_point, proj_p_diff);
            J(0,STATE_SPACE_DIMENSION + i) = ((proj_p_diff.x - (*pts_it).first.x) - proj_p.x) / epsilon;
            J(1,STATE_SPACE_DIMENSION + i) = ((proj_p_diff.y - (*pts_it).first.y) - proj_p.y) / epsilon;

            alpha += J.transpose() * cov_ls_inv * J;
            beta += J.transpose() * cov_ls_inv * Eigen::Vector2d(proj_p.x, proj_p.y);
            i++;
          }
        }
      }
    }

    // Augment alpha
    alpha += Eigen::MatrixXd::Identity(STATE_SPACE_DIMENSION + num_alpha, STATE_SPACE_DIMENSION + num_alpha) * lambda;

    // Solve for x
    Eigen::VectorXd new_p(STATE_SPACE_DIMENSION + num_alpha);

    // Due to an API change we need to check for
    // the right call at compile time
#ifdef EIGEN_WORLD_VERSION
    alpha.llt().solve(-beta, &new_p);
#else
    Eigen::Cholesky<Eigen::MatrixXd> c(alpha);
    new_p = c.solve(-beta);
#endif

    // Calculate chisqr again
    double chisqr = calc_chisqr(p_f, p_i, new_p, cal_type);

    if (chisqr < old_chisqr)
    {
      focal_length->setDouble(focal_length->getDouble() + new_p[FOCAL_LENGTH]);
      principal_point_x->setDouble(principal_point_x->getDouble() + new_p[PP_X]);
      principal_point_y->setDouble(principal_point_y->getDouble() + new_p[PP_Y]);
      distortion->setDouble(distortion->getDouble() + new_p[DIST]);
      tx->setDouble(tx->getDouble() + new_p[T_1]);
      ty->setDouble(ty->getDouble() + new_p[T_2]);
      tz->setDouble(tz->getDouble() + new_p[T_3]);

      Quaternion<double> q_diff;
      GVector::vector3d<double> aa_diff(new_p[Q_1], new_p[Q_2], new_p[Q_3]);
      q_diff.setAxis(aa_diff.norm(), aa_diff.length());
      Quaternion<double> q_field2cam = Quaternion<double>(q0->getDouble(),q1->getDouble(),q2->getDouble(),q3->getDouble());
      q_field2cam.norm();
      q_field2cam = q_diff * q_field2cam ;
      q0->setDouble(q_field2cam.x);
      q1->setDouble(q_field2cam.y);
      q2->setDouble(q_field2cam.z);
      q3->setDouble(q_field2cam.w);

      for (int i=0; i < num_alpha; i++)
        p_alpha[i] += new_p[STATE_SPACE_DIMENSION + i];

      // Normalize focal length an orientation when the optimization tends to go into the wrong
      // of both possible projections
      if (focal_length->getDouble() < 0)
      {
        focal_length->setDouble(-focal_length->getDouble());
        q_field2cam = q_rotate180 * q_field2cam;
        q0->setDouble(q_field2cam.x);
        q1->setDouble(q_field2cam.y);
        q2->setDouble(q_field2cam.z);
        q3->setDouble(q_field2cam.w);
      }

      if (old_chisqr - chisqr < 0.001)
        stop_optimization = true;
      else
      {
        lambda /= 10;
        convergence_counter = 0;
      }

      old_chisqr = chisqr;
#ifndef NDEBUG
      std::cerr << "Chi-square: "<< old_chisqr << std::endl;
#endif
    }
    else
    {
      lambda *= 10;

      if (convergence_counter++ > 10)
        stop_optimization = true;

    }
    if ((GetTimeSec() - t_start) > additional_calibration_information->convergence_timeout->getDouble()) stop_optimization=true;
  }

// Debug output starts here
#ifndef NDEBUG

  // Estimated parameters
  std::cerr << "Estimated parameters: " << std::endl;
  std::cerr << focal_length->getDouble() << " " << principal_point_x->getDouble() << " " << principal_point_y->getDouble() << " " << distortion->getDouble() << std::endl;
  std::cerr << q0->getDouble() << " " << q1->getDouble() << " " << q2->getDouble() << " " << q3->getDouble() << std::endl;
  std::cerr << tx->getDouble() << " " << ty->getDouble() << " " << tz->getDouble() <<  std::endl;
  std::cerr << "alphas: " << p_alpha << std::endl;

  // Testing calibration by projecting the four field points into the image plane
  // and calculate MSE
  std::vector<GVector::vector3d<double> >::iterator it_p_f  = p_f.begin();
  std::vector<GVector::vector2d<double> >::iterator it_p_i  = p_i.begin();

  double corner_x(0);
  double corner_y(0);
  for(; it_p_f != p_f.end(); it_p_f++, it_p_i++)
  {
    GVector::vector2d<double> proj_p;
    field2image(*it_p_f, proj_p);
    GVector::vector3d<double> some_point;
    image2field(some_point,proj_p, 150);
    std::cerr << "Point in world: ("<< it_p_f->x << "," << it_p_f->y << "," << it_p_f->z  << ")" << std::endl;
    std::cerr << "Point should be at (" << it_p_i->x << "," << it_p_i->y << ") and is projected at (" << proj_p.x << "," << proj_p.y <<")" << std::endl;

    corner_x += (proj_p.x - it_p_i->x) * (proj_p.x - it_p_i->x);
    corner_y += (proj_p.y - it_p_i->y) * (proj_p.y - it_p_i->y);
  }

  std::cerr << "RESIDUAL CORNER POINTS: " << sqrt(corner_x/4) << " " << sqrt(corner_y/4) << std::endl;

 if (cal_type & FULL_ESTIMATION)
 {
  // Testing calibration by projecting the points on the lines into the image plane
  // and calculate MSE
  double line_x(0);
  double line_y(0);

  std::vector<CalibrationData>::iterator ls_it = calibrationSegments.begin();

  int i = 0;
  for (; ls_it != calibrationSegments.end(); ls_it++)
  {
    std::vector< std::pair<GVector::vector2d<double>,bool> >::iterator pts_it = (*ls_it).imgPts.begin();
    for(; pts_it != (*ls_it).imgPts.end(); pts_it++)
    {
      if(pts_it->second)
      {
        GVector::vector2d<double> proj_p;
        double alpha = p_alpha(i);
        GVector::vector3d<double >alpha_point;
        if(ls_it->straightLine){
          alpha_point = alpha * (*ls_it).p1 + (1 - alpha) * (*ls_it).p2;
        }else{
          double theta = alpha * (*ls_it).theta1 + (1.0 - alpha) * (*ls_it).theta2;
          alpha_point = ls_it->center + ls_it->radius*GVector::vector3d<double>(cos(theta),sin(theta),0.0);
        }

        field2image(alpha_point, proj_p, p);

        line_x += (proj_p.x - pts_it->first.x) * (proj_p.x - pts_it->first.x);
        line_y += (proj_p.y - pts_it->first.y) * (proj_p.y - pts_it->first.y);

        i++;
      }
    }
  }

  if (i != 0)
    std::cerr << "RESIDUAL LINE POINTS: " << sqrt(line_x/(double)i) << " " << sqrt(line_y/(double)i) << std::endl;
 }

#endif
}

CameraParameters::AdditionalCalibrationInformation::AdditionalCalibrationInformation()
{
  left_corner_image_x = new VarDouble("left corner in image x", 10.0);
  left_corner_image_y = new VarDouble("left corner in image y", 10.0);
  right_corner_image_x = new VarDouble("right corner in image x", 300.0);
  right_corner_image_y = new VarDouble("right corner in image y", 10.0);
  left_centerline_image_x = new VarDouble("left centerline in image x", 10.0);
  left_centerline_image_y = new VarDouble("left centerline in image y", 200.0);
  right_centerline_image_x = new VarDouble("right centerline in image x", 300.0);
  right_centerline_image_y = new VarDouble("right centerline in image y", 200.0);
  initial_distortion = new VarDouble("initial distortion", 1.0);
  camera_height = new VarDouble("camera height", 4000.0);
  line_search_corridor_width = new VarDouble("line search corridor width", 280.0);
  convergence_timeout = new VarDouble("convergence timeout (s)", 10.0);
  cov_corner_x = new VarDouble("Cov corner measurement x", 1.0);
  cov_corner_y = new VarDouble("Cov corner measurement y", 1.0);
  cov_ls_x = new VarDouble("Cov line segment measurement x", 1.0);
  cov_ls_y = new VarDouble("Cov line segment measurement y", 1.0);
  pointsPerLine = new VarInt("Points per line",20);
  pointsInsideGoal = new VarInt("Points inside the goal",5);
  pointsInsideCenterCircle = new VarInt("Points inside center circle",6);
  pointsOnCenterCircle = new VarInt("Points on center circle",12);
  pointsOnDefenseAreaArc = new VarInt("Points on defense area arc",5);
  pointsOnDefenseStretch = new VarInt("Points on defense stretch",4);
}

CameraParameters::AdditionalCalibrationInformation::~AdditionalCalibrationInformation()
{
  delete left_corner_image_x;
  delete left_corner_image_y;
  delete right_corner_image_x;
  delete right_corner_image_y;
  delete left_centerline_image_x;
  delete left_centerline_image_y;
  delete right_centerline_image_x;
  delete right_centerline_image_y;
  delete initial_distortion;
  delete camera_height;
  delete line_search_corridor_width;
  delete convergence_timeout;
  delete cov_corner_x;
  delete cov_corner_y;
  delete cov_ls_x;
  delete cov_ls_y;
  delete pointsPerLine;
  delete pointsInsideGoal;
  delete pointsInsideCenterCircle;
  delete pointsOnCenterCircle;
  delete pointsOnDefenseAreaArc;
  delete pointsOnDefenseStretch;
}

void CameraParameters::AdditionalCalibrationInformation::addSettingsToList(VarList& list)
{
  list.addChild(left_corner_image_x);
  list.addChild(left_corner_image_y);
  list.addChild(right_corner_image_x);
  list.addChild(right_corner_image_y);
  list.addChild(left_centerline_image_x);
  list.addChild(left_centerline_image_y);
  list.addChild(right_centerline_image_x);
  list.addChild(right_centerline_image_y);
  list.addChild(initial_distortion);
  list.addChild(camera_height);
  list.addChild(line_search_corridor_width);
  list.addChild(convergence_timeout);
  list.addChild(cov_corner_x);
  list.addChild(cov_corner_y);
  list.addChild(cov_ls_x);
  list.addChild(cov_ls_y);
  list.addChild(pointsPerLine);
  list.addChild(pointsInsideGoal);
  list.addChild(pointsInsideCenterCircle);
  list.addChild(pointsOnCenterCircle);
  list.addChild(pointsOnDefenseAreaArc);
  list.addChild(pointsOnDefenseStretch);
}