// Copyright 2015-2019 Autoware Foundation
//
// 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.

#ifndef UTILIZATION__UTIL_HPP_
#define UTILIZATION__UTIL_HPP_

#include <lanelet2_extension/utility/query.hpp>
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <tier4_autoware_utils/trajectory/trajectory.hpp>
#include <utilization/boost_geometry_helper.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_auto_planning_msgs/msg/path.hpp>
#include <autoware_auto_planning_msgs/msg/path_point.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <tier4_planning_msgs/msg/stop_reason.hpp>
#include <visualization_msgs/msg/marker.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/geometry/Point.h>
#include <lanelet2_routing/RoutingGraph.h>
#include <pcl/point_types.h>
#include <tf2/utils.h>

#include <algorithm>
#include <limits>
#include <string>
#include <vector>

namespace tier4_autoware_utils
{
template <>
inline geometry_msgs::msg::Point getPoint(
  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
{
  return p.point.pose.position;
}
}  // namespace tier4_autoware_utils

namespace behavior_velocity_planner
{
struct SearchRangeIndex
{
  size_t min_idx;
  size_t max_idx;
};
struct DetectionRange
{
  bool use_right = true;
  bool use_left = true;
  double interval;
  double min_longitudinal_distance;
  double max_longitudinal_distance;
  double min_lateral_distance;
  double max_lateral_distance;
};
struct PointWithSearchRangeIndex
{
  geometry_msgs::msg::Point point;
  SearchRangeIndex index;
};

using Point2d = boost::geometry::model::d2::point_xy<double>;
using autoware_auto_planning_msgs::msg::PathPoint;
using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
using Polygons2d = std::vector<Polygon2d>;
namespace planning_utils
{
using geometry_msgs::msg::Pose;
inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) { return p; }
inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Pose & p) { return p.position; }
inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseStamped & p)
{
  return p.pose.position;
}
inline geometry_msgs::msg::Point getPoint(const autoware_auto_planning_msgs::msg::PathPoint & p)
{
  return p.pose.position;
}
inline geometry_msgs::msg::Point getPoint(
  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
{
  return p.point.pose.position;
}
inline geometry_msgs::msg::Point getPoint(
  const autoware_auto_planning_msgs::msg::TrajectoryPoint & p)
{
  return p.pose.position;
}
inline geometry_msgs::msg::Pose getPose(
  const autoware_auto_planning_msgs::msg::Path & path, int idx)
{
  return path.points.at(idx).pose;
}
inline geometry_msgs::msg::Pose getPose(
  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, int idx)
{
  return path.points.at(idx).point.pose;
}
inline geometry_msgs::msg::Pose getPose(
  const autoware_auto_planning_msgs::msg::Trajectory & traj, int idx)
{
  return traj.points.at(idx).pose;
}

// create detection area from given range return false if creation failure
bool createDetectionAreaPolygons(
  Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & current_pose,
  const DetectionRange & da_range, const double obstacle_vel_mps, const double min_velocity = 1.0);
PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio);
Point2d calculateOffsetPoint2d(const Pose & pose, const double offset_x, const double offset_y);
void extractClosePartition(
  const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions,
  BasicPolygons2d & close_partition, const double distance_thresh = 30.0);
void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys);
void setVelocityFrom(const size_t idx, const double vel, PathWithLaneId * input);
void insertVelocity(
  PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v,
  size_t & insert_index, const double min_distance = 0.001);
inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); }

inline double square(const double & a) { return a * a; }
double normalizeEulerAngle(double euler);
geometry_msgs::msg::Quaternion getQuaternionFromYaw(double yaw);

template <class T1, class T2>
double calcSquaredDist2d(const T1 & a, const T2 & b)
{
  return square(getPoint(a).x - getPoint(b).x) + square(getPoint(a).y - getPoint(b).y);
}

template <class T1, class T2>
double calcDist2d(const T1 & a, const T2 & b)
{
  return std::sqrt(calcSquaredDist2d<T1, T2>(a, b));
}

template <class T>
bool calcClosestIndex(
  const T & path, const geometry_msgs::msg::Pose & pose, int & closest, double dist_thr = 3.0,
  double angle_thr = M_PI_4);

template <class T>
bool calcClosestIndex(
  const T & path, const geometry_msgs::msg::Point & point, int & closest, double dist_thr = 3.0);

geometry_msgs::msg::Pose transformRelCoordinate2D(
  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin);
geometry_msgs::msg::Pose transformAbsCoordinate2D(
  const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin);
SearchRangeIndex getPathIndexRangeIncludeLaneId(
  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t lane_id);
/**
 * @brief find nearest segment index with search range
 */
template <class T>
size_t findNearestSegmentIndex(const T & points, const PointWithSearchRangeIndex & point_with_index)
{
  const auto & index = point_with_index.index;
  const auto point = point_with_index.point;

  tier4_autoware_utils::validateNonEmpty(points);

  double min_dist = std::numeric_limits<double>::max();
  size_t nearest_idx = 0;

  for (size_t i = index.min_idx; i <= index.max_idx; ++i) {
    const auto dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), point);
    if (dist < min_dist) {
      min_dist = dist;
      nearest_idx = i;
    }
  }

  if (nearest_idx == 0) {
    return 0;
  }
  if (nearest_idx == points.size() - 1) {
    return points.size() - 2;
  }

  const double signed_length =
    tier4_autoware_utils::calcLongitudinalOffsetToSegment(points, nearest_idx, point);

  if (signed_length <= 0) {
    return nearest_idx - 1;
  }

  return nearest_idx;
}
/**
 * @brief find nearest segment index within distance threshold
 */
template <class T>
PointWithSearchRangeIndex findFirstNearSearchRangeIndex(
  const T & points, const geometry_msgs::msg::Point & point, const double distance_thresh = 9.0)
{
  tier4_autoware_utils::validateNonEmpty(points);

  bool min_idx_found = false;
  PointWithSearchRangeIndex point_with_range = {point, {static_cast<size_t>(0), points.size() - 1}};
  for (size_t i = 0; i < points.size(); i++) {
    const auto & p = points.at(i).point.pose.position;
    const double dist = std::hypot(point.x - p.x, point.y - p.y);
    if (dist < distance_thresh) {
      if (!min_idx_found) {
        point_with_range.index.min_idx = i;
        min_idx_found = true;
      }
      point_with_range.index.max_idx = i;
    }
  }
  return point_with_range;
}
/**
 * @brief calcSignedArcLength from point to point with search range
 */
template <class T>
double calcSignedArcLengthWithSearchIndex(
  const T & points, const PointWithSearchRangeIndex & src_point_with_range,
  const PointWithSearchRangeIndex & dst_point_with_range)
{
  tier4_autoware_utils::validateNonEmpty(points);
  const size_t src_idx = planning_utils::findNearestSegmentIndex(points, src_point_with_range);
  const size_t dst_idx = planning_utils::findNearestSegmentIndex(points, dst_point_with_range);
  const double signed_length = tier4_autoware_utils::calcSignedArcLength(points, src_idx, dst_idx);
  const double signed_length_src_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment(
    points, src_idx, src_point_with_range.point);
  const double signed_length_dst_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment(
    points, dst_idx, dst_point_with_range.point);
  return signed_length - signed_length_src_offset + signed_length_dst_offset;
}
Polygon2d toFootprintPolygon(const autoware_auto_perception_msgs::msg::PredictedObject & object);
bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin);
Polygon2d generatePathPolygon(
  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx,
  const size_t end_idx, const double width);

double calcJudgeLineDistWithAccLimit(
  const double velocity, const double max_stop_acceleration, const double delay_response_time);

double calcJudgeLineDistWithJerkLimit(
  const double velocity, const double acceleration, const double max_stop_acceleration,
  const double max_stop_jerk, const double delay_response_time);

double calcDecelerationVelocityFromDistanceToTarget(
  const double max_slowdown_jerk, const double max_slowdown_accel, const double current_accel,
  const double current_velocity, const double distance_to_target);

double findReachTime(
  const double jerk, const double accel, const double velocity, const double distance,
  const double t_min, const double t_max);

tier4_planning_msgs::msg::StopReason initializeStopReason(const std::string & stop_reason);

void appendStopReason(
  const tier4_planning_msgs::msg::StopFactor stop_factor,
  tier4_planning_msgs::msg::StopReason * stop_reason);

std::vector<geometry_msgs::msg::Point> toRosPoints(
  const autoware_auto_perception_msgs::msg::PredictedObjects & object);

geometry_msgs::msg::Point toRosPoint(const pcl::PointXYZ & pcl_point);
geometry_msgs::msg::Point toRosPoint(const Point2d & boost_point, const double z);

LineString2d extendLine(
  const lanelet::ConstPoint3d & lanelet_point1, const lanelet::ConstPoint3d & lanelet_point2,
  const double & length);

template <class T>
std::vector<T> concatVector(const std::vector<T> & vec1, const std::vector<T> & vec2)
{
  auto concat_vec = vec1;
  concat_vec.insert(std::end(concat_vec), std::begin(vec2), std::end(vec2));
  return concat_vec;
}

}  // namespace planning_utils
}  // namespace behavior_velocity_planner

#endif  // UTILIZATION__UTIL_HPP_


/*
	Test case for ticket #7951
	tests whether or not xxx() == xxx(0) for various engines
	Thanks to Stephen T. Lavavej for his close reading of 26.5.3.3 [rand.eng.sub]/7
*/

#include <boost/random/ranlux.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/mersenne_twister.hpp>

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4244)
#endif

BOOST_AUTO_TEST_CASE(test_zero_seed)
{
    BOOST_CHECK(boost::random::ranlux24_base(0) == boost::random::ranlux24_base());
    BOOST_CHECK(boost::random::minstd_rand0(0)  == boost::random::minstd_rand0());
    BOOST_CHECK(boost::random::mt19937(0)       != boost::random::mt19937());

    BOOST_CHECK(boost::random::ranlux48_base(0) == boost::random::ranlux48_base ());

    BOOST_CHECK(boost::random::ranlux_base_01(0) == boost::random::ranlux_base_01 ());
    BOOST_CHECK(boost::random::ranlux64_base_01(0) == boost::random::ranlux64_base_01 ());
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// 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
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_BLOCK_CIPHER_STATE_HPP
#define CRYPTO3_BLOCK_CIPHER_STATE_HPP

#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/accumulators/framework/features.hpp>

#include <boost/crypto3/block/accumulators/block.hpp>

namespace boost {
    namespace crypto3 {
        namespace block {
            /*!
             * @brief Accumulator set with pre-defined block cipher accumulator params.
             *
             * Meets the requirements of AccumulatorSet
             *
             * @ingroup block
             *
             * @tparam Mode Cipher state preprocessing mode type (e.g. isomorphic_encryption_mode<aes128>)
             * @tparam Endian
             * @tparam ValueBits
             * @tparam LengthBits
             */
            template<typename ProcessingMode>
            using accumulator_set = boost::accumulators::accumulator_set<
                digest<ProcessingMode::block_bits>,
                boost::accumulators::features<accumulators::tag::block<ProcessingMode>>, std::size_t>;
        }    // namespace block
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_BLOCK_CIPHER_STATE_HPP


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

#include <iostream>

#include <boost/make_shared.hpp>
#include <boost/lambda/lambda.hpp>

#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"

namespace tudat
{

namespace simulation_setup
{

using namespace ephemerides;
using namespace gravitation;

//! Function that determines the order in which bodies are to be created
std::vector< std::pair< std::string, std::shared_ptr< BodySettings > > > determineBodyCreationOrder(
        const std::map< std::string, std::shared_ptr< BodySettings > >& bodySettings )
{
    std::vector< std::pair< std::string, std::shared_ptr< BodySettings > > > outputVector;

    // Create vector of pairs (body name and body settings) that is to be created.
    for( std::map< std::string, std::shared_ptr< BodySettings > >::const_iterator bodyIterator
         = bodySettings.begin( );
         bodyIterator != bodySettings.end( ); bodyIterator ++ )
    {
        outputVector.push_back( std::make_pair( bodyIterator->first, bodyIterator->second ) );
    }

    return outputVector;
}

//! Function to create a map of bodies objects.
NamedBodyMap createBodies(
        const std::map< std::string, std::shared_ptr< BodySettings > >& bodySettings )
{
    std::vector< std::pair< std::string, std::shared_ptr< BodySettings > > > orderedBodySettings
            = determineBodyCreationOrder( bodySettings );

    // Declare map of bodies that is to be returned.
    NamedBodyMap bodyMap;

    // Create empty body objects.
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        bodyMap[ orderedBodySettings.at( i ).first ] = std::make_shared< Body >( );
    }

    // Define constant mass for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        const double constantMass = orderedBodySettings.at( i ).second->constantMass;
        if ( constantMass == constantMass )
        {
            bodyMap[ orderedBodySettings.at( i ).first ]->setConstantBodyMass( constantMass );
        }
    }

    // Create ephemeris objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->ephemerisSettings != nullptr )
        {
            bodyMap[ orderedBodySettings.at( i ).first ]->setEphemeris(
                        createBodyEphemeris( orderedBodySettings.at( i ).second->ephemerisSettings,
                                             orderedBodySettings.at( i ).first ) );
        }
    }

    // Create atmosphere model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->atmosphereSettings != nullptr )
        {
            bodyMap[ orderedBodySettings.at( i ).first ]->setAtmosphereModel(
                        createAtmosphereModel( orderedBodySettings.at( i ).second->atmosphereSettings,
                                               orderedBodySettings.at( i ).first ) );
        }
    }

    // Create body shape model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->shapeModelSettings != nullptr )
        {
            bodyMap[ orderedBodySettings.at( i ).first ]->setShapeModel(
                        createBodyShapeModel( orderedBodySettings.at( i ).second->shapeModelSettings,
                                              orderedBodySettings.at( i ).first ) );
        }
    }

    // Create rotation model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->rotationModelSettings != nullptr )
        {
            bodyMap[ orderedBodySettings.at( i ).first ]->setRotationalEphemeris(
                        createRotationModel( orderedBodySettings.at( i ).second->rotationModelSettings,
                                             orderedBodySettings.at( i ).first ) );
        }
    }

    // Create gravity field model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->gravityFieldSettings != nullptr )
        {
            bodyMap[ orderedBodySettings.at( i ).first ]->setGravityFieldModel(
                        createGravityFieldModel( orderedBodySettings.at( i ).second->gravityFieldSettings,
                                                 orderedBodySettings.at( i ).first, bodyMap,
                                                 orderedBodySettings.at( i ).second->gravityFieldVariationSettings ) );
        }
    }

    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->gravityFieldVariationSettings.size( ) > 0 )
        {
            bodyMap[ orderedBodySettings.at( i ).first ]->setGravityFieldVariationSet(
                        createGravityFieldModelVariationsSet(
                            orderedBodySettings.at( i ).first, bodyMap,
                            orderedBodySettings.at( i ).second->gravityFieldVariationSettings ) );
        }
    }

    // Create aerodynamic coefficient interface objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->aerodynamicCoefficientSettings != nullptr )
        {
            bodyMap[ orderedBodySettings.at( i ).first ]->setAerodynamicCoefficientInterface(
                        createAerodynamicCoefficientInterface(
                            orderedBodySettings.at( i ).second->aerodynamicCoefficientSettings,
                            orderedBodySettings.at( i ).first ) );
        }
    }

    // Create radiation pressure coefficient objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        std::map< std::string, std::shared_ptr< RadiationPressureInterfaceSettings > >
                radiationPressureSettings
                = orderedBodySettings.at( i ).second->radiationPressureSettings;
        for( std::map< std::string, std::shared_ptr< RadiationPressureInterfaceSettings > >::iterator
             radiationPressureSettingsIterator = radiationPressureSettings.begin( );
             radiationPressureSettingsIterator != radiationPressureSettings.end( );
             radiationPressureSettingsIterator++ )
        {
            bodyMap[ orderedBodySettings.at( i ).first ]->setRadiationPressureInterface(
                        radiationPressureSettingsIterator->first,
                        createRadiationPressureInterface(
                            radiationPressureSettingsIterator->second,
                            orderedBodySettings.at( i ).first, bodyMap ) );
        }

    }

    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        for( unsigned int j = 0; j < orderedBodySettings.at( i ).second->groundStationSettings.size( ); j++ )
        {
            createGroundStation( bodyMap.at( orderedBodySettings.at( i ).first ), orderedBodySettings.at( i ).first,
                     orderedBodySettings.at( i ).second->groundStationSettings.at( j ) );
        }
    }
    return bodyMap;

}

} // namespace simulation_setup

} // namespace tudat


#include <chrono>
#include <boost/predef.h>
#include <algorithm>

#include "StreamFlowControlFilter.hpp"

#define BUFFER_SIZE 300000
#define MAX_CHUNK_SIZE 10000

StreamFlowControlFilter* StreamFlowControlFilter::createNew(UsageEnvironment& env,
	                                                            FramedSource* inputSource,
																unsigned long bandwidth)
{
	return new StreamFlowControlFilter(env, inputSource, bandwidth);
}

StreamFlowControlFilter::StreamFlowControlFilter(UsageEnvironment& env,
	FramedSource* inputSource,
	unsigned long bandwidth)
	:
	FramedFilter(env, inputSource),
	bandwidth(bandwidth),
	buffer(new unsigned char[BUFFER_SIZE]),
	bufferSize(0),
	processedBytes(0)
{
}

void StreamFlowControlFilter::doGetNextFrame()
{
	if (processedBytes < bufferSize)
	{
		deliverChunk();
	}
	else
	{
		// Read directly from our input source into our client's buffer:
		fFrameSize = 0;
		fInputSource->getNextFrame(buffer,
								   BUFFER_SIZE,
			                       afterGettingFrame0,
			                       this,
			                       FramedSource::handleClosure,
			                       this);
	}
}

void StreamFlowControlFilter::afterGettingFrame0(void* clientData,
	                                               unsigned frameSize,
                                                   unsigned numTruncatedBytes,
                                                   struct timeval presentationTime,
	                                               unsigned durationInMicroseconds)
{
	StreamFlowControlFilter* framer = (StreamFlowControlFilter*)clientData;
	framer->afterGettingFrame(frameSize, presentationTime);
}

void StreamFlowControlFilter::afterGettingFrame(unsigned frameSize,
                                                struct timeval presentationTime)
{
	bufferSize = frameSize;
	fPresentationTime = presentationTime;
	processedBytes = 0;
	deliverChunk();
}

void StreamFlowControlFilter::deliverChunk()
{
	// Fill buffer with next chunk
	fFrameSize = (std::min)((unsigned int)MAX_CHUNK_SIZE, fMaxSize);
	fFrameSize = (std::min)(fFrameSize, (unsigned int)(bufferSize - processedBytes));
	memcpy(fTo, buffer + processedBytes, fFrameSize);
	processedBytes += fFrameSize;

#if BOOST_OS_WINDOWS
	auto durationPerKBit = std::chrono::nanoseconds(boost::ratio_multiply<boost::giga, boost::kilo>::num / bandwidth);

	LARGE_INTEGER StartingTime, EndingTime, ElapsedMicroseconds;
	LARGE_INTEGER Frequency;

	QueryPerformanceFrequency(&Frequency);
	QueryPerformanceCounter(&StartingTime);
#endif

	// Activity to be timed
	afterGetting(this);

#if BOOST_OS_WINDOWS
	QueryPerformanceCounter(&EndingTime);
	ElapsedMicroseconds.QuadPart = EndingTime.QuadPart - StartingTime.QuadPart;

	//
	// We now have the elapsed number of ticks, along with the
	// number of ticks-per-second. We use these values
	// to convert to the number of elapsed microseconds.
	// To guard against loss-of-precision, we convert
	// to microseconds *before* dividing by ticks-per-second.
	//

	ElapsedMicroseconds.QuadPart *= 1000000;
	ElapsedMicroseconds.QuadPart /= Frequency.QuadPart;

	auto isDuration = std::chrono::microseconds(ElapsedMicroseconds.QuadPart);
	auto shouldDuration = durationPerKBit * fFrameSize * 8 / 1000;
	auto coolDownDuration = shouldDuration - isDuration;

	// Let network cool down
	QueryPerformanceCounter(&StartingTime);
	std::chrono::microseconds elapsedCoolDownDuration;
	do
	{
		QueryPerformanceCounter(&EndingTime);
		ElapsedMicroseconds.QuadPart = EndingTime.QuadPart - StartingTime.QuadPart;
		ElapsedMicroseconds.QuadPart *= 1000000;
		ElapsedMicroseconds.QuadPart /= Frequency.QuadPart;
		elapsedCoolDownDuration = std::chrono::microseconds(ElapsedMicroseconds.QuadPart);
	} while (elapsedCoolDownDuration < coolDownDuration);
#endif
}


// Boost.Geometry

// Copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_RESULT_INVERSE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_RESULT_INVERSE_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/detail/flattening.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry { namespace detail
{

template <typename T>
struct result_inverse
{
    void set(T const& d, T const& a)
    {
        distance = d;
        azimuth = a;
    }

    T distance;
    T azimuth;
};

}}} // namespace geofeatures_boost::geometry::detail

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_RESULT_INVERSE_HPP


/******************************************************************************
 * Copyright 2017 Baidu Robotic Vision Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "IBA/IBA.h"
#include "feature_utils.h"
#include "image_utils.h"
#include "xp_quaternion.h"
#include "param.h"  // calib
#include "basic_datatype.h"
#include "iba_helper.h"
#include "pose_viewer.h"
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <glog/logging.h>
#include <gflags/gflags.h>
#include <opencv2/core.hpp>

//ros相关
#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>

#include <tf/transform_broadcaster.h>
#include "../ros_visualization/visualization.h"
#include <eigen3/Eigen/Dense>

#include <thread>
#include <mutex>
#include <condition_variable>

#include <algorithm>
#include <string>
#include <fstream>
#include <vector>
#include <queue>
#include <memory>

//闭环
#include "LoopClosing.h"
#include "CameraBase.hpp"
#include "NCameraSystem.hpp"
#include "ParamsInit.h"

DEFINE_bool(show_track_img,true,"output trackimg(ros msg)");
DEFINE_string(config_file, "", "像vinsfuison,给总配置文件的路径");
DEFINE_int32(grid_row_num, 1, "Number of rows of detection grids");
DEFINE_int32(grid_col_num, 1, "Number of cols of detection grids");
DEFINE_int32(max_num_per_grid, 150, "Max number of points per grid");
DEFINE_double(feat_quality, 0.07, "Tomasi-Shi feature quality level");
DEFINE_double(feat_min_dis, 10, "Tomasi-Shi feature minimal distance");
DEFINE_bool(not_use_fast, false, "Whether or not use FAST");
DEFINE_int32(pyra_level, 2, "Total pyramid levels");
DEFINE_int32(start_idx, 0, "The image index of the first detection (from 0)");
DEFINE_int32(end_idx, -1, "The image index of the last detection");
DEFINE_double(uniform_radius, 40, "< 5 disables uniformaty enforcement");
DEFINE_int32(ft_len, 125, "The feature track length threshold when dropout kicks in");
DEFINE_double(ft_droprate, 0.05, "The drop out rate when acc feature track exceeds ft_len");
DEFINE_bool(show_feat_only, false, "wether or not show detection results only");
DEFINE_int32(fast_thresh, 10, "FAST feature threshold (only meaningful if use_fast=true)");
DEFINE_double(min_feature_distance_over_baseline_ratio,
              4, "Used for slave image feature detection");
DEFINE_double(max_feature_distance_over_baseline_ratio,
              3000, "Used for slave image feature detection");
DEFINE_string(iba_param_path, "", "iba parameters path");
DEFINE_string(gba_result_path, "", "Save the gab_result");
DEFINE_string(result_folder, "", "Save the result");
DEFINE_bool(stereo, false, "monocular or stereo mode");
DEFINE_bool(save_feature, false, "Save features to .dat file");
DEFINE_string(image0_topic, "", "左目话题");
DEFINE_string(image1_topic, "", "右目话题");
DEFINE_string(imu_topic, "", "imu话题");
DEFINE_bool(LoopClosure, true, "use LoopClosure?");
DEFINE_bool(GetGT, true, "GTdata");
DEFINE_bool(UseIMU,true, "use imu data?");
DEFINE_string(GT_path, "", "");
DEFINE_string(dbow3_voc_path, "", "dbow3_voc_path,must set!");
DEFINE_int32(mono_init_th,3,"大于3次认为ok");

std::queue<IBA::RelativeConstraint> loop_info;

std::queue<IBA::CurrentFrame> CFs_buf;
std::queue<IBA::KeyFrame> KFs_buf;
std::mutex m_buf,m_syn_buf,m_solver_buf,m_loop_buf;
std::condition_variable con;
std::unique_ptr<XP::FeatureTrackDetector> feat_track_detector_ptr;
std::unique_ptr<XP::ImgFeaturePropagator> slave_img_feat_propagator_ptr;
std::vector<float> total_time;

std::unique_ptr<LC::LoopClosing> LoopCloser_ptr;

XP::DuoCalibParam duo_calib_param;
//绘图器
XP::PoseViewer pose_viewer;
//求解器初始化
IBA::Solver solver;
//外参
Eigen::Matrix4f T_Cl_Cr;
Eigen::Matrix4d T_Cl_Cr_d;
Eigen::Matrix4d Tbc0;
//用于LBA回调的绘图器
Eigen::Vector3f last_position = Eigen::Vector3f::Zero();
float travel_dist = 0.f;
double offset_ts = 0;
bool set_relative_pose = false;
bool init_first_track = true;

Eigen::Matrix4d relative_pose; //Twc(first) * Twc.inv(gt)
// Create masks based on FOVs computed from intrinsics
std::vector<cv::Mat_<uchar> > masks(2);

float prev_img_time_stamp = 0.0f;
// load previous image//左目之前的特征点和描述子
std::vector<cv::KeyPoint> pre_image_key_points;
cv::Mat pre_image_features;
int mono_init_count = 0;
bool need_mono_init = true;

std::map<int,Eigen::Vector3f> cur_Mp_info; //前一帧追踪到的地图点的3d坐标
std::vector<std::tuple<int,int,cv::KeyPoint>> cur_track;//地图点id,左右目,观测 ;
Eigen::Matrix4f last_pose_Twc0;

bool process_frontend()
{
    while (true)
    {
        std::vector<std::pair<std::vector<XP::ImuData>, stereo_Img_info_f >> measurements;

        std::unique_lock <std::mutex> lk(m_buf);
        con.wait(lk, [&] {
            return (measurements = getMeasurements()).size() != 0;//将目前所有的测量取出来
        });

        lk.unlock();

        //处理多组观测
        for (auto &measurement : measurements)
        {
            stereo_Img_info_f cur_img_info = measurement.second;
            // get timestamp from image file name (s)
            //这里用的时间戳都是相对于imu第一帧的时间戳,time_stamp为左图像时间戳
            const float img_time_stamp = cur_img_info.first;
            std::vector<XP::ImuData> imu_meas = measurement.first;//两帧图像之间imu的原始测量数据
            cv::Mat img_in_raw;
            if(cur_img_info.second.first.type() == CV_8UC1 )
                img_in_raw = cur_img_info.second.first;
            else if(cur_img_info.second.first.type() == CV_8UC3)
            {
                cv::cvtColor(cur_img_info.second.first, img_in_raw, CV_BGR2GRAY);
            }
            CHECK_EQ(img_in_raw.rows, duo_calib_param.Camera.img_size.height);
            CHECK_EQ(img_in_raw.cols, duo_calib_param.Camera.img_size.width);
            cv::Mat img_in_smooth;
            cv::blur(img_in_raw, img_in_smooth, cv::Size(3, 3));//图像去噪
            if (img_in_smooth.rows == 0) {
                std::cerr << "Cannot smooth " <<std::endl;
                return EXIT_FAILURE;
            }
            // load slave image
            cv::Mat slave_img_smooth;  // for visualization later

            if (FLAGS_stereo) //也就是双目的情况
            {//读取右目相机,一样做降噪处理
                cv::Mat slave_img_in;
                if(cur_img_info.second.second.type() == CV_8UC1 )
                    slave_img_in = cur_img_info.second.second;
                else if(cur_img_info.second.second.type() == CV_8UC3)
                {
                    cv::cvtColor(cur_img_info.second.second, slave_img_in, CV_BGR2GRAY);
                }
                cv::blur(slave_img_in, slave_img_smooth, cv::Size(3, 3));
            }

            std::vector<cv::KeyPoint> key_pnts;//左目提取到的特征点
            cv::Mat orb_feat;//左目orb描述子
            cv::Mat pre_img_in_smooth;
            // load slave image
            std::vector<cv::KeyPoint> key_pnts_slave;
            cv::Mat orb_feat_slave;
            if(init_first_track) //首次追踪
            {
                // first frame
                //第一帧左相机提取特征点
                //输入左相机图片,图像掩码,最大提取的特征点数量,金字塔层数,fast点阈值,特征点,描述子
                //主要就是提点,计算描述子
                feat_track_detector_ptr->detect(img_in_smooth,
                                                masks[0],
                                                FLAGS_max_num_per_grid * FLAGS_grid_row_num * FLAGS_grid_col_num,
                                                FLAGS_pyra_level,
                                                FLAGS_fast_thresh,
                                                &key_pnts,
                                                nullptr);
                feat_track_detector_ptr->build_img_pyramids(img_in_smooth,//构建图像金字塔,存到前一帧缓存器中
                                                            XP::FeatureTrackDetector::BUILD_TO_PREV);
            } else
            {

                VLOG(1) << "pre_image_key_points.size(): " << pre_image_key_points.size();
                const int request_feat_num = FLAGS_max_num_per_grid * FLAGS_grid_row_num * FLAGS_grid_col_num;//最大提取的特征点数量
                feat_track_detector_ptr->build_img_pyramids(img_in_smooth,
                                                            XP::FeatureTrackDetector::BUILD_TO_CURR);//存储当前左目的金字塔
                if (imu_meas.size() > 1) {//如果有imu的话
                    // Here we simply the transformation chain to rotation only and assume zero translation
                    cv::Matx33f old_R_new;
                    XP::XpQuaternion I_new_q_I_old;  // The rotation between the new {I} and old {I}
                    //RK4预积分,算出Ri(cur)_i(pre)
                    for (size_t i = 1; i < imu_meas.size(); ++i) {
                        XP::XpQuaternion q_end;
                        XP::IntegrateQuaternion(imu_meas[i - 1].ang_v/*k时刻的角速度*/,
                                                imu_meas[i].ang_v/*k+1时刻的角速度*/,
                                                I_new_q_I_old,
                                                imu_meas[i].time_stamp - imu_meas[i - 1].time_stamp/*delta_t*/,
                                                &q_end);
                        I_new_q_I_old = q_end;
                    }
                    Eigen::Matrix3f I_new_R_I_old = I_new_q_I_old.ToRotationMatrix();
                    Eigen::Matrix4f I_T_C =//左相机到imu的变换
                            duo_calib_param.Imu.D_T_I.inverse() * duo_calib_param.Camera.D_T_C_lr[0];//Ti_d * Td_cl = Ti_cl
                    Eigen::Matrix3f I_R_C = I_T_C.topLeftCorner<3, 3>();//外参旋转部分
                    //imu测出是imu系前后两帧之间的旋转，需要转到左相机坐标系的旋转
                    Eigen::Matrix3f C_new_R_C_old = I_R_C.transpose() * I_new_R_I_old * I_R_C;// Rcl_i * Ri(cur)_i(pre) * Ri_cl = Rcl(cur)_cl(pre)
                    //Rcl(pre)_cl(cur)
                    for (int i = 0; i < 3; ++i) {
                        for (int j = 0; j < 3; ++j) {
                            old_R_new(j, i) = C_new_R_C_old(i, j);
                        }
                    }

                    if (VLOG_IS_ON(1)) {
                        XP::XpQuaternion C_new_q_C_old;
                        C_new_q_C_old.SetFromRotationMatrix(C_new_R_C_old);
                        VLOG(1) << "C_new_R_C_old = \n" << C_new_R_C_old;
                        VLOG(1) << "ea =\n" << C_new_q_C_old.ToEulerRadians() * 180 / M_PI;
                    }
                    feat_track_detector_ptr->optical_flow_and_detect(masks[0]/*左相机掩码*/,
                                                                     pre_image_features/*左相机上一帧检测到的特征点的描述子*/,
                                                                     pre_image_key_points/*左相机上一帧检测到的特征点*/,
                                                                     request_feat_num/*最大要求提取的特征点数量*/,
                                                                     FLAGS_pyra_level/*金字塔层*/,
                                                                     FLAGS_fast_thresh/*fast阈值*/,
                                                                     &key_pnts/*左相机当前帧提取到的特征点*/,
                                                                     nullptr/*左相机当前帧提取到的特征点对应的描述子*/,
                                                                     duo_calib_param.Camera.fishEye,
                                                                     cv::Vec2f(0, 0),  // shift init pixels
                                                                     &duo_calib_param.Camera.cv_camK_lr[0]/*cv形式的左右相机内参*/,
                                                                     &duo_calib_param.Camera.cv_dist_coeff_lr[0]/*左右相机的畸变参数*/,
                                                                     &old_R_new/*Rcl(pre)_cl(cur)左相机当前帧到前一帧的旋转*/);
                } else {
                    feat_track_detector_ptr->optical_flow_and_detect(masks[0],
                                                                     pre_image_features,
                                                                     pre_image_key_points,
                                                                     request_feat_num,
                                                                     FLAGS_pyra_level,
                                                                     FLAGS_fast_thresh,
                                                                     &key_pnts,
                                                                     nullptr);
                }
                feat_track_detector_ptr->update_img_pyramids();//更新金字塔buffer
                VLOG(1) << "after OF key_pnts.size(): " << key_pnts.size() << " requested # "
                        << FLAGS_max_num_per_grid * FLAGS_grid_row_num * FLAGS_grid_col_num;
            }

            //如果是双目的话
            if (slave_img_smooth.rows > 0)
            {
                CHECK(orb_feat_slave.empty());
                auto det_slave_img_start = std::chrono::high_resolution_clock::now();
                //输入右左目图片，左目提取的特征点,左右目外参,右目提取的特征点,描述子,是否要输出debug信息
                //用svo的块匹配的思路算出右相机中特征点的坐标,并且计算描述子
                slave_img_feat_propagator_ptr->PropagateFeatures(slave_img_smooth,  // cur 右目
                                                                 img_in_smooth,  // ref 左目
                                                                 key_pnts, //左目提取的特征点
                                                                 T_Cl_Cr,  // T_ref_cur //左右目外参
                                                                 &key_pnts_slave,
                                                                 nullptr,
                                                                 false);  // draw_debug
                VLOG(1) << "detect slave key_pnts.size(): " << key_pnts_slave.size() << " takes "
                        << std::chrono::duration_cast<std::chrono::microseconds>(
                                std::chrono::high_resolution_clock::now() - det_slave_img_start).count() / 1e3
                        << " ms";
            }
            //根据class_id(与地图点id一致)进行排序,不过本来就是这个顺序吧
            std::sort(key_pnts.begin(), key_pnts.end(), cmp_by_class_id);
            std::sort(key_pnts_slave.begin(), key_pnts_slave.end(), cmp_by_class_id);

            if(init_first_track)
                init_first_track = false;
            else
            {
                if(FLAGS_show_track_img)
                    pubTrackImage(img_in_smooth,slave_img_smooth,pre_image_key_points,key_pnts,key_pnts_slave,(double)img_time_stamp + offset_ts);
            }
            if(!FLAGS_stereo)//单目简单的判断一下第一次需要开始吧
            {//算一下运动
                mono_begin_compute(pre_image_key_points,key_pnts,duo_calib_param.Camera.cv_camK_lr[0],
                                   &duo_calib_param.Camera.cv_dist_coeff_lr[0],
                                   duo_calib_param.Camera.fishEye,
                                   masks[0],&mono_init_count);
                if(need_mono_init && mono_init_count >= FLAGS_mono_init_th )//大于5次就认为在激励,开始追踪吧
                {
                    need_mono_init = false;
                }
                else if(need_mono_init)
                {
                    pre_image_key_points = key_pnts;
                    pre_image_features = orb_feat.clone();

                    prev_img_time_stamp = img_time_stamp;
                    continue;
                }
            }

            // push to IBA
            IBA::CurrentFrame CF;
            IBA::KeyFrame KF;
            //输入左相机特征点,右相机特征点,imu测量,左目时间戳,当前帧,关键帧
            //进行点管理(新旧地图点的观测更新),以及关键帧判断以及生成
            create_iba_frame(key_pnts, key_pnts_slave, imu_meas, img_time_stamp, &CF, &KF);

            if(FLAGS_LoopClosure)
            {
                if(KF.iFrm != -1)
                {

                    std::vector<cv::KeyPoint> loop_key_pnts;//左目提取到的特征点
                    cv::Mat loop_orb_feat;//左目orb描述子

                    feat_track_detector_ptr->detect_for_loop(img_in_smooth,
                                                             masks[0],
                                                             1000,
                                                             FLAGS_pyra_level,
                                                             FLAGS_fast_thresh,
                                                             &loop_key_pnts,
                                                             &loop_orb_feat);

                    cv::Mat cur_orb_feat;
                    feat_track_detector_ptr->ComputeDescriptors(img_in_smooth,&key_pnts,&cur_orb_feat);
                    LoopCloser_ptr ->InsertKeyFrame(std::shared_ptr<LC::KeyFrame> (new LC::KeyFrame(KF.iFrm,key_pnts,cur_orb_feat,loop_key_pnts,loop_orb_feat,img_in_smooth)));
                }
            }

            m_solver_buf.lock();
            CFs_buf.push(CF);
            if(KF.iFrm != -1)
                KFs_buf.push(KF);
            m_solver_buf.unlock();

            pre_image_key_points = key_pnts;
            pre_image_features = orb_feat.clone();

            prev_img_time_stamp = img_time_stamp;
        }

    }
}

void process_backend()
{
    while(1)
    {
        IBA::CurrentFrame CF;
        IBA::KeyFrame KF;
        CF.iFrm = -1;
        KF.iFrm = -1;
        m_solver_buf.lock();
        if (!CFs_buf.empty())
        {
            CF = CFs_buf.front();
            CFs_buf.pop();
            if(!KFs_buf.empty())
            {
                if( CF.iFrm == KFs_buf.front().iFrm)//KF里的索引只可能>= cf的
                {
                    KF = KFs_buf.front();
                    KFs_buf.pop();
                }
            }
        }
        m_solver_buf.unlock();
        if(CF.iFrm != -1)
        {
            if(FLAGS_LoopClosure)
            {
                std::vector<IBA::RelativeConstraint> loop_Relative_priors;
                m_loop_buf.lock();
                while(!loop_info.empty())
                {
                    loop_Relative_priors.push_back(loop_info.front());
                    loop_info.pop();
                }
                m_loop_buf.unlock();
                //将当前帧以及关键帧(如果有的话)放进求解器
                if(!loop_Relative_priors.empty())
                {
                    for (int i = 0; i < loop_Relative_priors.size(); ++i)
                    {
                        solver.PushRelativeConstraint(loop_Relative_priors[i]);
                    }
                    solver.Wakeup_GBA();
                }
            }

            if(total_time.size() > CF.iFrm)//先这么存一下吧,先不考虑上限的问题
                total_time[CF.iFrm] = CF.t;
            else
            {
                total_time.resize(total_time.size()+300);
                total_time[CF.iFrm] = CF.t;
            }

            if(!FLAGS_UseIMU)
            {
                Get_Track(CF,cur_track);

                if(SolveMulticamPnP(cur_track,cur_Mp_info,last_pose_Twc0))
                {
                    Eigen::Matrix3f pose_pnp_Rc0w = last_pose_Twc0.block<3,3>(0,0).transpose();
                    Eigen::Vector3f pose_pnp_twc0 = last_pose_Twc0.block<3,1>(0,3);
                    for (int i = 0; i < 3; ++i)
                    {
                        CF.Cam_state.Cam_pose.p[i] = pose_pnp_twc0[i];
                        for (int j = 0; j < 3; ++j) {
                            CF.Cam_state.Cam_pose.R[i][j] = pose_pnp_Rc0w(i,j);
                        }

                    }
                    KF.Cam_pose = CF.Cam_state.Cam_pose;
                }
            }
            solver.PushCurrentFrame(CF, KF.iFrm == -1 ? nullptr : &KF);//先说明一下,我习惯的求解增量的表达是Hx=b,但是这里是Hx=-b
//          show pose
            pose_viewer.displayTo("trajectory");
            cv::waitKey(1);
        }

    }
}

//euroc测试代码
int main(int argc, char** argv)
{
    //初始化glog相关程序
    google::InitGoogleLogging(argv[0]);
    google::ParseCommandLineFlags(&argc, &argv, true);
    google::InstallFailureSignalHandler();

    ros::init(argc, argv, "ice_ba");
    ros::NodeHandle n("~");

    registerPub(n);

    total_time.resize(4000);

    //如果没有给image文件夹的话
    if (FLAGS_config_file.empty() )//
    {
        google::ShowUsageWithFlags(argv[0]);
        return -1;
    }

    if(!FLAGS_result_folder.empty())
    {
        if (!fs::is_directory(FLAGS_result_folder + "/dat")) {
            fs::create_directories(FLAGS_result_folder + "/dat");
        }

    }

    try {//输出标定参数（内外参,立体矫正,去完畸变后的映射）
        load_Parameters(FLAGS_config_file, duo_calib_param);
    } catch (...){
        LOG(ERROR) << "Load calibration file error";
        return -1;
    }

    int Num_Cam = FLAGS_stereo ? 2:1;
    for (int lr = 0; lr < Num_Cam; ++lr) {
        float fov;
        //输出每一目的mask和视场角
        if (XP::generate_cam_mask(duo_calib_param.Camera.cv_camK_lr[lr],
                                  duo_calib_param.Camera.cv_dist_coeff_lr[lr],
                                  duo_calib_param.Camera.fishEye,
                                  duo_calib_param.Camera.img_size,
                                  &masks[lr],
                                  &fov)) {
            std::cout << "camera " << lr << " fov: " << fov << " deg\n";
        }
    }

    if(FLAGS_LoopClosure)
    {
        LoopCloser_ptr.reset(new LC::LoopClosing(FLAGS_dbow3_voc_path,
                                                 duo_calib_param.Camera.cameraK_lr[0],  // ref_camK 左相机内参
                                                 duo_calib_param.Camera.cv_dist_coeff_lr[0],  // ref_dist_coeff 左相机畸变
                                                 duo_calib_param.Camera.fishEye,
                                                 masks[0]));
    }

    //初始化特征提取器
    feat_track_detector_ptr.reset(new XP::FeatureTrackDetector(FLAGS_ft_len/*图像大小*/,
                                                               FLAGS_ft_droprate/*图像大小*/,
                                                               !FLAGS_not_use_fast/*是否用fast点*/,
                                                               FLAGS_uniform_radius/*图像大小*/,
                                                               duo_calib_param.Camera.img_size/*图像大小*/));

    if(FLAGS_stereo)
    {
        //配置左右相机的投影和畸变模型,目前只支持针孔+radtan
        slave_img_feat_propagator_ptr.reset(new XP::ImgFeaturePropagator(
                duo_calib_param.Camera.cameraK_lr[1],  // cur_camK 右相机内参
                duo_calib_param.Camera.cameraK_lr[0],  // ref_camK 左相机内参
                duo_calib_param.Camera.cv_dist_coeff_lr[1],  // cur_dist_coeff 右相机畸变
                duo_calib_param.Camera.cv_dist_coeff_lr[0],  // ref_dist_coeff 左相机畸变
                duo_calib_param.Camera.fishEye,
                masks[1],//右相机的掩码
                FLAGS_pyra_level,//所有金字塔层数
                FLAGS_min_feature_distance_over_baseline_ratio,//特征点最小深度比例,用于极线搜索
                FLAGS_max_feature_distance_over_baseline_ratio));//特征点最大深度比例,用于极线搜索)

        //双目外参
        T_Cl_Cr = T_Cl_Cr_d.cast<float>();

    }
    //绘制前清除画布
    pose_viewer.set_clear_canvas_before_draw(true);
    if (FLAGS_save_feature) {
        //存储外参,内参文件
        IBA::SaveCalibration(FLAGS_result_folder + "/calibration.dat", to_iba_calibration(duo_calib_param));
    }

    //左右目的内外参,,是否要输出细节内容,是否输出debug信息,,参数所在位置
    solver.Create(to_iba_calibration(duo_calib_param),
                  257,
                  IBA_VERBOSE_NONE,
                  IBA_DEBUG_NONE,
                  257,
                  FLAGS_iba_param_path,//iba的配置文件
                  "" /* iba directory */);

    if(FLAGS_LoopClosure)
    {
        LoopCloser_ptr->SetCallback([&](const vector<Eigen::Matrix4f> & rKFpose/*参考关键帧Twc*/,
                                        const Eigen::Matrix4f & lKFpose,vector<int> riFrm,int liFrm)
        {
            pubLoopCamPose(lKFpose.cast<double>());
            for (int k = 0; k < rKFpose.size(); ++k)
            {
                LA::AlignedMatrix6x6f S;
                IBA::RelativeConstraint Z;

                Eigen::Matrix4f Tlr = lKFpose.inverse() * rKFpose[k];////Tc0(观测关键帧)c0(参考关键帧)
                float pose_f[3][4];
                for (int i = 0; i < 3; ++i)
                {
                    for (int j = 0; j < 4; ++j)
                    {
                        pose_f[i][j] = Tlr(i,j);
                    }

                }
                Rigid3D T;
                T.Set(pose_f);////Tc0(观测关键帧)c0(参考关键帧)

                Z.iFrm1 = riFrm[k];
                Z.iFrm2 = liFrm;
                T.Rotation3D::Get(Z.T.R);
                T.GetPosition().Get(Z.T.p);
                S.MakeDiagonal(LOOP_S2P, LOOP_S2R);//固定一下参考关键帧
                S.Get(Z.S.S);
                m_loop_buf.lock();
                loop_info.push(Z);
                m_loop_buf.unlock();
            }

        });

    }
    //对GBA求解器设置回调函数m_callback,用来可视化
    solver.SetCallbackGBA([&](const int iFrm,/*最新一帧的id*/ const float ts/*最新一帧的时间戳*/)
    {

        if(FLAGS_LoopClosure)
        {
            IBA::Global_Map GM;
            solver.GetUpdateGba(&GM);
            LoopCloser_ptr->UpdateKfInfo(GM);
        }

        std::vector<std::pair<int,IBA::CameraPose>> total_kfs = solver.Get_Total_KFs();
        std::vector<std::pair<double,Eigen::Matrix4d>> kf_poses;
        kf_poses.resize(total_kfs.size());
        for (int kf_id = 0; kf_id < total_kfs.size(); ++kf_id)
        {
            kf_poses[kf_id].first = (double)total_time[total_kfs[kf_id].first] + offset_ts;
            kf_poses[kf_id].second = Eigen::Matrix4d::Identity();

            for (int i = 0; i < 3; ++i) {
                kf_poses[kf_id].second(i, 3) = (double)total_kfs[kf_id].second.p[i];
                for (int j = 0; j < 3; ++j) {
                    kf_poses[kf_id].second(i,j) = (double)total_kfs[kf_id].second.R[j][i];
                }
            }
        }
        pubKFsPose(kf_poses);
    });

    //对LBA求解器设置回调函数m_callback,用来可视化
    solver.SetCallbackLBA([&](const int iFrm,/*最新一帧的id*/ const float ts/*最新一帧的时间戳*/)
    {
        // as we may be able to send out information directly in the callback arguments
        IBA::SlidingWindow sliding_window;
        //获得LBA中的滑窗中更新了的普通帧以及更新了的关键帧还有更新了的地图点
        solver.GetSlidingWindow(&sliding_window);
        const IBA::CameraIMUState& X = sliding_window.CsLF.back();//最新的一帧
        const IBA::CameraPose& C = X.Cam_pose;
        Eigen::Matrix4f W_vio_T_S = Eigen::Matrix4f::Identity();  // Twc0最新的
        for (int i = 0; i < 3; ++i) {
            W_vio_T_S(i, 3) = C.p[i];
            for (int j = 0; j < 3; ++j) {
                W_vio_T_S(i, j) = C.R[j][i];  //因为存储的是C.R里是Rc0w,所以要转成Rwc0     Cam_state.R is actually R_SW
            }
        }
        Eigen::Matrix4d Twc0 = W_vio_T_S.cast<double>();
        Eigen::Matrix<float, 9, 1> speed_and_biases;
        Eigen::Matrix<double , 9, 1> speed_and_biases_d;
        for (int i = 0; i < 3; ++i) {
            speed_and_biases(i) = X.v[i];
            speed_and_biases(i + 3) = X.ba[i];
            speed_and_biases(i + 6) = X.bw[i];

            speed_and_biases_d(i) = (double)X.v[i];
            speed_and_biases_d(i + 3) = (double)X.ba[i];
            speed_and_biases_d(i + 6) = (double)X.bw[i];
        }

        Eigen::Vector3f cur_position = W_vio_T_S.topRightCorner(3, 1);
        travel_dist += (cur_position - last_position).norm();
        last_position = cur_position;
        pose_viewer.addPose(W_vio_T_S, speed_and_biases, travel_dist);

        CHECK_EQ(sliding_window.iFrms.size(),sliding_window.CsLF.size());

        double cur_time =(double)total_time[sliding_window.iFrms.back()] + offset_ts;

        if(!FLAGS_UseIMU)
        {
            vector<int> track_idx;
            for (int i = 0; i < cur_track.size(); ++i)
                track_idx.push_back(std::get<0>(cur_track[i]));
            solver.Get_cur_Mps(track_idx,cur_Mp_info);
            last_pose_Twc0 = W_vio_T_S;
        }

        //ROS pub
//        pubUpdatePointClouds(sliding_window.Xs,cur_time);
        pubLatestCameraPose(Twc0,speed_and_biases_d.block<3,1>(0,0),cur_time);
        pubTF(Twc0,cur_time);

    });

    //启动求解器
    solver.Start();

    //左右相机同步
    std::thread sync_thread{sync_stereo};
    //前端
    std::thread measurement_process{process_frontend};
    //后端
    std::thread Backend_process{process_backend};
    //
    ros::Subscriber sub_imu = n.subscribe(FLAGS_imu_topic, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    ros::Subscriber sub_img0 = n.subscribe(FLAGS_image0_topic, 100, img0_callback);
    ros::Subscriber sub_img1 = n.subscribe(FLAGS_image1_topic, 100, img1_callback);

    ros::Subscriber sub_odom = n.subscribe("/ice_ba/imu_pose", 1000, odom_callback);

    ros::spin();

    return 0;
}


// ======================================================================
/*!
 * \file shapepack.cpp
 * \brief A program to convert a shapefile into a packed attribute table
 */
// ======================================================================
/*!
 * \page shapepack shapepack
 *
 * shapepack takes as input a single file shapefile containing latlon
 * polygonal data, the name of the desired attribute, and converts the
 * shape into packed 2D table. The default bounding box for the data
 * is the world from -180 to 180 longitude, -90 to 90 latitude, but
 * it can be altered via the command line.
 *
 * The program was originally created for getting a quick method to
 * determine the timezone of any given coordinate on the planet.
 * The selected attribute was the name of the timezone at the coordinate.
 * For this reason the 2D table is always indexed so that latitude
 * is the inner index in the actual binary encoded table.
 *
 * The format of the shapepacked file is as follows
 * \code
 * SHAPEPACK
 * width_in_pixels height_in_pixels
 * lon1 lat1 lon2 lat2
 * number_of_attributes
 * attribute1
 * attribute2
 * ...
 * attributeN
 * table2d
 * \endcode
 * The header part is in plain ASCII, the 2D table is in the following
 * binary format:
 * \code
 * uint32 number_of_entries
 * uint32 position1 (always 0)
 * uint16 attribute_index_1
 * uint32 position2
 * uint16 attribute_index_2
 * ...
 * uint32 positionN (always width*height)
 * uint16 0
 * \endcode
 * The indices start at one. Zero return values are used to indicate
 * missing data. The final zero is output only for programming convenience.
 *
 * The position for a particular latlon coordinate is calculated as
 * follows:
 * \code
 * ypos = (lat-lat1)/(lat2-lat1)*(height-1)
 * xpos = (lon-lon1)/(lon2-lon1)*(width-1)
 * pos = ypos+xpos*height
 * \endcode
 * ypos and xpos are rounded to nearest integers before the final
 * integer position is calculated.
 *
 */
// ======================================================================

#include <imagine/NFmiEsriPolygon.h>
#include <imagine/NFmiEsriShape.h>
#include <imagine/NFmiFillMap.h>
#include <imagine/NFmiImage.h>

#include <macgyver/WorldTimeZones.h>

#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

#include <fstream>
#include <iostream>
#include <map>
#include <set>
#include <stdexcept>
#include <string>

extern "C"
{
#include <stdint.h>
}

using namespace std;
using namespace Imagine;

// ----------------------------------------------------------------------
/*!
 * \brief Command line options
 */
// ----------------------------------------------------------------------

struct Options
{
  string shapefile;
  string packfile;
  string pngfile;
  string zonefile;
  int width;
  int height;
  float lon1;
  float lat1;
  float lon2;
  float lat2;
  string attribute;
  bool verbose;
  bool accurate;

  Options()
      : shapefile(),
        packfile(),
        pngfile(),
        zonefile(),
        width(-1),
        height(-1),
        lon1(-180),
        lat1(-90),
        lon2(180),
        lat2(90),
        attribute(),
        verbose(false),
        accurate(false)
  {
  }
};

Options options;

// ----------------------------------------------------------------------
/*!
 * \brief Parse the command line
 */
// ----------------------------------------------------------------------

bool parse_options(int argc, char *argv[])
{
  namespace po = boost::program_options;

  po::options_description desc("Allowed options");
  desc.add_options()("help,h", "print out help message")(
      "verbose,v", po::bool_switch(&options.verbose), "set verbose mode on")(
      "version,V", "display version number")(
      "attribute,a", po::value(&options.attribute), "shapefile attribute name")(
      "shapefile,s", po::value(&options.shapefile), "shapefile (without suffix)")(
      "output,o", po::value(&options.packfile), "output filename")(
      "pngfile,p", po::value(&options.pngfile), "optional image filename")(
      "zonefile,z",
      po::value(&options.zonefile),
      "optional shapepack with which to combine the information with")(
      "width,W", po::value(&options.width), "width of rendered image")(
      "height,H", po::value(&options.height), "height of rendered image")(
      "lon1", po::value(&options.lon1), "bottom left longitude (default is -180)")(
      "lat1", po::value(&options.lat1), "bottom left latitude (default is -90)")(
      "lon2", po::value(&options.lon2), "top right longitude (default is 180)")(
      "lat2", po::value(&options.lat2), "top right latitude (default is 90)")(
      "accurate,A", po::bool_switch(&options.accurate));

  po::positional_options_description p;
  p.add("shapefile", 1);
  p.add("output", 1);

  po::variables_map opt;
  po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), opt);

  po::notify(opt);

  if (opt.count("version") != 0)
  {
    cout << "shapepack v1.0 (" << __DATE__ << ' ' << __TIME__ << ')' << endl;
  }

  if (opt.count("help"))
  {
    cout << "Usage: shapepack [options] shapefile outfile" << endl
         << endl
         << "shapepack picks an attribute from a polygonal shapefile" << endl
         << "and converts the data into a fast access 2D table." << endl
         << endl
         << desc << endl;
    return false;
  }

  if (opt.count("shapefile") == 0)
    throw runtime_error("shapefile name not specified");

  if (opt.count("output") == 0)
    throw runtime_error("output name not specified");

  // Check bounding box

  if (options.lon1 >= options.lon2 || options.lat1 >= options.lat2)
    throw runtime_error("Stupid bounding box, fix it");

  // Preserve image aspect if one size attribute is missing

  if (options.width < 0 && options.height < 0)
    throw runtime_error("Either image width or height must be specified");

  if (options.width < 0 && options.height > 0)
  {
    options.width = static_cast<int>(options.height * (options.lon2 - options.lon1) /
                                     (options.lat2 - options.lat1));
  }

  if (options.height < 0 && options.width > 0)
  {
    options.height = static_cast<int>(options.width * (options.lat2 - options.lat1) /
                                      (options.lon2 - options.lon1));
  }

  if (options.height == 0 || options.width == 0)
    throw runtime_error("Try a larger image, this is pointless");

  return true;
}

// ----------------------------------------------------------------------
/*!
 * \brief Get attribute value as string
 */
// ----------------------------------------------------------------------

string get_attribute_value(const NFmiEsriElement &theElement, const NFmiEsriAttributeName &theName)
{
  string name = theName.Name();
  switch (theName.Type())
  {
    case kFmiEsriString:
      return theElement.GetString(name);
    case kFmiEsriInteger:
      return boost::lexical_cast<string>(theElement.GetInteger(name));
    case kFmiEsriDouble:
      return boost::lexical_cast<string>(theElement.GetDouble(name));
    default:
      throw runtime_error("Unknown attribute value type");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Find the given attribute name
 */
// ----------------------------------------------------------------------

NFmiEsriShape::attributes_type::const_iterator find_attribute(
    const NFmiEsriShape::attributes_type &theAttributes, const std::string &theName)
{
  NFmiEsriShape::attributes_type::const_iterator it = theAttributes.begin();
  for (; it != theAttributes.end(); ++it)
  {
    if ((*it)->Name() == theName)
      break;
  }
  if (it == theAttributes.end())
    throw runtime_error("No attribute named '" + theName + "' in the shape");
  return it;
}

// ----------------------------------------------------------------------
/*!
 * \brief Find all unique attribute values for the given attribute
 */
// ----------------------------------------------------------------------

set<string> find_unique_attributes(const NFmiEsriShape &theShape, const string &theAttribute)
{
  const NFmiEsriShape::attributes_type &attributes = theShape.Attributes();

  NFmiEsriShape::attributes_type::const_iterator at = find_attribute(attributes, theAttribute);

  set<string> values;

  for (NFmiEsriShape::const_iterator it = theShape.Elements().begin();
       it != theShape.Elements().end();
       ++it)
  {
    if (*it == nullptr)
      continue;
    string value = get_attribute_value(**it, **at);
    values.insert(value);
  }

  return values;
}

// ----------------------------------------------------------------------
/*!
 * \brief Verbose printout of unique attribute values
 */
// ----------------------------------------------------------------------

void print_uniques(const set<string> &theValues)
{
  cout << "There were " << theValues.size() << " unique values in the shape:" << endl;
  int pos = 1;
  for (set<string>::const_iterator it = theValues.begin(); it != theValues.end(); ++it, ++pos)
  {
    cout << pos << ' ' << *it << endl;
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Map set of strings to integer indices
 */
// ----------------------------------------------------------------------

map<string, int> make_attribute_map(const set<string> &theSet)
{
  map<string, int> ret;
  int i = 1;
  for (set<string>::const_iterator it = theSet.begin(); it != theSet.end(); ++it)
  {
    ret.insert(map<string, int>::value_type(*it, i++));
  }
  return ret;
}

// ----------------------------------------------------------------------
/*!
 * \brief Pixel scaling
 */
// ----------------------------------------------------------------------

inline float xpixel(float x)
{
  return (x + 180) / 360 * options.width;
}

inline float ypixel(float y)
{
  return (y + 90) / 180 * options.height;
}

inline float lonpixel(int x)
{
  return (360.0 * x / options.width - 180);
}

inline float latpixel(int y)
{
  return (180.0 * y / options.height - 90);
}

// ----------------------------------------------------------------------
/*!
 * \brief Insert polygon to fillmap
 */
// ----------------------------------------------------------------------

void polygon_to_fillmap(NFmiFillMap &theMap, const NFmiEsriElement *theElement)
{
  switch (theElement->Type())
  {
    case kFmiEsriPolygon:
    case kFmiEsriPolygonM:
    case kFmiEsriPolygonZ:
    {
      const NFmiEsriPolygon *elem = static_cast<const NFmiEsriPolygon *>(theElement);
      for (int part = 0; part < elem->NumParts(); part++)
      {
        int i1, i2;
        i1 = elem->Parts()[part];  // start of part
        if (part + 1 == elem->NumParts())
          i2 = elem->NumPoints() - 1;  // end of part
        else
          i2 = elem->Parts()[part + 1] - 1;  // end of part

        for (int i = i1 + 1; i <= i2; i++)
          theMap.Add(xpixel(elem->Points()[i - 1].X()),
                     ypixel(elem->Points()[i - 1].Y()),
                     xpixel(elem->Points()[i].X()),
                     ypixel(elem->Points()[i].Y()));
      }
      break;
    }
    default:
      break;
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Render the image to be compressed
 */
// ----------------------------------------------------------------------

void render_image(NFmiImage &theImage,
                  const NFmiEsriShape &theShape,
                  const map<string, int> &theValues)
{
  if (options.verbose)
  {
    cout << "Rendering " << theImage.Width() << "x" << theImage.Height()
         << " size image, this may take a while" << endl;
  }

  // Attribute information

  const NFmiEsriShape::attributes_type &attributes = theShape.Attributes();

  NFmiEsriShape::attributes_type::const_iterator at = find_attribute(attributes, options.attribute);

  for (NFmiEsriShape::const_iterator it = theShape.Elements().begin();
       it != theShape.Elements().end();
       ++it)
  {
    if (*it == nullptr)
      continue;

    const string value = get_attribute_value(**it, **at);
    NFmiColorTools::Color color = theValues.find(value)->second;

    NFmiFillMap fillmap;
    polygon_to_fillmap(fillmap, *it);
    fillmap.Fill(theImage, color, NFmiColorTools::kFmiColorCopy);
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Test if the given point is inside the polygon
 *
 * Copied from shapefind.cpp
 */
// ----------------------------------------------------------------------

bool is_inside(const NFmiEsriPolygon &thePoly, double theX, double theY)
{
  // Check bounding box first
  const NFmiEsriBox &box = thePoly.Box();

  if (!box.IsValid())
    return false;

  if (theX < box.Xmin() || theX > box.Xmax() || theY < box.Ymin() || theY > box.Ymax())
    return false;

  // Then do an actual inside-test if it is possible the point is inside

  int counter = 0;

  for (int part = 0; part < thePoly.NumParts(); part++)
  {
    int i1 = thePoly.Parts()[part];  // start of part
    int i2;
    if (part + 1 == thePoly.NumParts())
      i2 = thePoly.NumPoints() - 1;  // end of part
    else
      i2 = thePoly.Parts()[part + 1] - 1;  // end of part

    if (i2 >= i1)
    {
      double x1 = thePoly.Points()[i1].X();
      double y1 = thePoly.Points()[i1].Y();

      for (int i = i1 + 1; i <= i2; i++)
      {
        double x2 = thePoly.Points()[i].X();
        double y2 = thePoly.Points()[i].Y();
        if (theY > std::min(y1, y2) && theY <= std::max(y1, y2) && theX <= std::max(x1, x2) &&
            y1 != y2)
        {
          const double xinters = (theY - y1) * (x2 - x1) / (y2 - y1) + x1;
          if (x1 == x2 || theX <= xinters)
            counter++;
        }
        x1 = x2;
        y1 = y2;
      }
    }
  }

  return (counter % 2 != 0);
}

// ----------------------------------------------------------------------
/*!
 * \brief Find polygon surrounding point from polygon shapefile
 */
// ----------------------------------------------------------------------

string find_enclosing_polygon(const NFmiEsriShape &theShape, float theLon, float theLat)
{
  // Find the first match

  const NFmiEsriShape::elements_type &elements = theShape.Elements();

  NFmiEsriShape::elements_type::size_type i;
  for (i = 0; i < elements.size(); i++)
  {
    if (elements[i] == 0)  // null element?
      continue;

    const NFmiEsriPolygon *elem = static_cast<const NFmiEsriPolygon *>(elements[i]);

    bool enclosed = is_inside(*elem, theLon, theLat);

    if (enclosed)
      break;
  }

  // Print the results.

  if (i < elements.size())
  {
    const NFmiEsriPolygon *elem = static_cast<const NFmiEsriPolygon *>(elements[i]);
    const NFmiEsriShape::attributes_type &attributes = theShape.Attributes();
    NFmiEsriShape::attributes_type::const_iterator at =
        find_attribute(attributes, options.attribute);
    const string value = get_attribute_value(*elem, **at);
    return value;
  }
  else
    return "";
}

// ----------------------------------------------------------------------
/*!
 * \brief Test if pixel color is the same if the coordinate is inside image
 *
 * We intentionally return true for pixels outside the image.
 */
// ----------------------------------------------------------------------

inline bool same_color(const NFmiImage &theImage, int i, int j, NFmiColorTools::Color c)
{
  return (i < 0 || j < 0 || i >= theImage.Width() || j >= theImage.Height() || theImage(i, j) == c);
}

// ----------------------------------------------------------------------
/*!
 * \brief Check if pixel is surrounded by indentical pixels or not
 */
// ----------------------------------------------------------------------

inline bool is_boundary_pixel(const NFmiImage &theImage, int i, int j)
{
  const NFmiColorTools::Color color = theImage(i, j);

  return !(same_color(theImage, i - 1, j - 1, color) && same_color(theImage, i + 1, j - 1, color) &&
           same_color(theImage, i, j - 1, color) && same_color(theImage, i - 1, j, color) &&
           same_color(theImage, i + 1, j, color) && same_color(theImage, i - 1, j + 1, color) &&
           same_color(theImage, i, j + 1, color) && same_color(theImage, i + 1, j + 1, color));
}

// ----------------------------------------------------------------------
/*!
 * \brief Guess timezone from nearest pixels if filling fails
 *
 * This is needed mostly to get values for pixels at -180/+180 longitudes
 * since those coordinates are not strictly inside polygons. We only
 * check horizontally or vertically adjacent pixels.
 */
// ----------------------------------------------------------------------

inline int find_nearest_zone(const NFmiImage &theImage, int i, int j)
{
  if (i - 1 >= 0 && theImage(i - 1, j) >= 0)
    return theImage(i, j);
  if (i + 1 < theImage.Width() && theImage(i + 1, j) >= 0)
    return theImage(i + 1, j);
  if (j - 1 >= 0 && theImage(i, j - 1) >= 0)
    return theImage(i, j - 1);
  if (j + 1 < theImage.Height() && theImage(i, j + 1) >= 0)
    return theImage(i, j + 1);
  return -1;
}

// ----------------------------------------------------------------------
/*!
 * \brief Check accuracy of image near borders
 */
// ----------------------------------------------------------------------

void refine_image(NFmiImage &theImage,
                  const NFmiEsriShape &theShape,
                  const map<string, int> &theValues)
{
  int checks = 0;
  int changes = 0;
  int pixels = 0;

  int percentage = 0;
  int perstep = 1;
  int total_pixels = theImage.Width() * theImage.Height();

  if (options.verbose)
    cout << "Validating border areas..." << endl;

  for (int i = 0; i < theImage.Width(); i++)
    for (int j = 0; j < theImage.Height(); j++)
    {
      ++pixels;
      if (pixels * 100.0 / total_pixels - percentage > perstep)
      {
        percentage += perstep;
        if (options.verbose)
          cout << "\t" << percentage << "%" << endl;
      }

      if (theImage(i, j) < 0 || is_boundary_pixel(theImage, i, j))
      {
        ++checks;

        string tz = find_enclosing_polygon(theShape, lonpixel(i), latpixel(j));

        if (!tz.empty())
        {
          int idx = theValues.find(tz)->second;
          if (theImage(i, j) != idx)
          {
            if (options.verbose)
              cout << "Changed " << i << "," << j << " value from " << theImage(i, j) << " to "
                   << idx << " (" << tz << ")" << endl;
            ++changes;
            theImage(i, j) = idx;
          }
        }
      }

      if (theImage(i, j) < 0)
      {
        auto idx = find_nearest_zone(theImage, i, j);
        if (idx > 0)
        {
          theImage(i, j) = idx;
          ++changes;
          if (options.verbose)
            cout << "Changed " << i << "," << j << " value from -1 to " << idx << endl;
        }
      }
    }

  if (options.verbose)
    cout << "Total checks:  " << checks << endl << "Total changes: " << changes << endl;
}

// ----------------------------------------------------------------------
/*!
 * \brief Output uint32 binary representation
 */
// ----------------------------------------------------------------------

void output_uint32(std::ostream &out, uint32_t theValue)
{
  uint32_t value = theValue;
  out.write(reinterpret_cast<char *>(&value), sizeof(uint32_t));
}

// ----------------------------------------------------------------------
/*!
 * \brief Output uint16 binary representation
 */
// ----------------------------------------------------------------------

void output_uint16(std::ostream &out, uint16_t theValue)
{
  uint16_t value = theValue;
  out.write(reinterpret_cast<char *>(&value), sizeof(uint16_t));
}

// ----------------------------------------------------------------------
/*!
 * \brief Compress the image
 */
// ----------------------------------------------------------------------

string compress_image(const NFmiImage &theImage, const map<string, int> &theMap)
{
  // Header
  ostringstream out;
  out << "SHAPEPACK\n"
      << theImage.Width() << ' ' << theImage.Height() << '\n'
      << options.lon1 << ' ' << options.lat1 << ' ' << options.lon2 << ' ' << options.lat2 << '\n';

  // Attribute index

  out << theMap.size() << '\n';

  for (map<string, int>::const_iterator it = theMap.begin(); it != theMap.end(); ++it)
  {
    out << it->first << '\n';
  }

  // Raw data

  ostringstream table;
  int startpos = 0;
  int entries = 0;
  int lastcolor = theImage(0, 0);
  int duplicates = 0;

#if 1
  std::cout << "Unique values:" << std::endl;
  std::set<int> uniques;
  int last = -1000;
  for (int i = 0; i < theImage.Width(); i++)
    for (int j = 0; j < theImage.Height(); j++)
    {
      if (theImage(i, j) != last)
        uniques.insert(theImage(i, j));
      last = theImage(i, j);
    }
  BOOST_FOREACH (int c, uniques)
    std::cout << c << std::endl;
#endif

  if (options.verbose)
    std::cout << "Compressing the image data" << std::endl;

  for (int i = 0; i < theImage.Width(); i++)
    for (int j = 0; j < theImage.Height(); j++)
    {
      if (theImage(i, j) == lastcolor)
        ++duplicates;
      else
      {
        ++entries;
        output_uint32(table, startpos);
        output_uint16(table, lastcolor);
        startpos = j + i * theImage.Height();
        lastcolor = theImage(i, j);
      }
    }
  // Flush remaining line
  if (duplicates > 0)
  {
    output_uint32(table, startpos);
    output_uint16(table, lastcolor);
  }
  // And terminate table
  output_uint32(table, theImage.Width() * theImage.Height());
  output_uint16(table, 0);

  // Final output

  output_uint32(out, entries);
  out << table.str();

  return out.str();
}

// ----------------------------------------------------------------------
/*!
 * \brief Render a shapepack onto an image
 */
// ----------------------------------------------------------------------

void render_shapepack(NFmiImage &img,
                      const Fmi::WorldTimeZones &zones,
                      const map<string, int> &attmap)
{
  if (options.verbose)
    std::cout << "Rendering background shapepack" << std::endl;

  for (int i = 0; i < img.Width(); i++)
    for (int j = 0; j < img.Height(); j++)
    {
      try
      {
        std::string tz = zones.zone_name(lonpixel(i), latpixel(j));
        map<string, int>::const_iterator it = attmap.find(tz);
        if (it == attmap.end())
        {
          cerr << "Failed to find index for timezone " << tz << " at coordinate " << i << "," << j
               << " at lonlat " << lonpixel(i) << "," << latpixel(j) << endl;
        }
        else
          img(i, j) = attmap.find(tz)->second;
      }
      catch (...)
      {
      }
    }
}

// ----------------------------------------------------------------------
/*!
 * \brief The main program
 *
 * -# read the shapefile
 * -# verify chosen attribute is valid
 * -# list all unique attribute values
 * -# render the polygons using attribute indices as values
 * -# compress the image
 * -# output the image
 */
// ----------------------------------------------------------------------

int domain(int argc, char *argv[])
{
  if (!parse_options(argc, argv))
    return 0;

  // Read the shape

  NFmiEsriShape shape;
  if (!shape.Read(options.shapefile, true))
    throw runtime_error("Failed to read '" + options.shapefile + "'");

  const NFmiEsriShape::attributes_type &attributes = shape.Attributes();

  if (attributes.size() == 0)
    throw runtime_error("shapefile does not contain any attributes");

  // If no attribute option was given, we accept it as long as
  // the shape contains exactly one attribute

  if (options.attribute.empty())
  {
    if (attributes.size() > 1)
    {
      ostringstream out;
      out << "shapefile contains multiple attributes, choose one: ";
      int pos = 0;
      for (NFmiEsriShape::attributes_type::const_iterator at = attributes.begin();
           at != attributes.end();
           ++at, ++pos)
      {
        if (pos > 0)
          out << ",";
        out << (*at)->Name();
      }

      throw runtime_error(out.str());
    }
    options.attribute = (*attributes.begin())->Name();
  }

  // Help informatio

  boost::shared_ptr<Fmi::WorldTimeZones> zones;
  if (!options.zonefile.empty())
    zones.reset(new Fmi::WorldTimeZones(options.zonefile));

  // Unique attributes

  set<string> uniques = find_unique_attributes(shape, options.attribute);

  if (options.verbose)
    print_uniques(uniques);

  if (zones)
  {
    BOOST_FOREACH (const string &z, zones->zones())
      uniques.insert(z);
  }

  map<string, int> attmap = make_attribute_map(uniques);

  if (options.verbose)
    print_uniques(uniques);

  // Render the image

  NFmiImage img(options.width, options.height, -1);
  if (zones)
    render_shapepack(img, *zones, attmap);

  render_image(img, shape, attmap);

  if (options.accurate)
    refine_image(img, shape, attmap);

  if (!options.pngfile.empty())
    img.WritePng(options.pngfile);

  // Compress the data

  string data = compress_image(img, attmap);

  ofstream out(options.packfile.c_str());
  if (!out)
    throw runtime_error("Could not open '" + options.packfile + "' for writing");
  out << data;
  out.close();

  return 0;
}

// ----------------------------------------------------------------------
/*!
 * \brief Main program
 */
// ----------------------------------------------------------------------

int main(int argc, char *argv[])
{
  try
  {
    return domain(argc, argv);
  }
  catch (exception &e)
  {
    cerr << "Error: " << e.what() << endl;
  }
  catch (...)
  {
    cerr << "Error: An unknown exception occurred" << endl;
  }
  return 1;
}


#include "../elysium.h"
#include "../rules.h"
#include "../sp.h"

#include "base58.h"
#include "chainparams.h"
#include "test/test_bitcoin.h"

#include <boost/test/unit_test.hpp>

#include <limits>

using namespace elysium;

BOOST_FIXTURE_TEST_SUITE(elysium_elysium_tests, TestingSetup)

BOOST_AUTO_TEST_CASE(elysium_mints_overflow)
{
    _my_sps = new CMPSPInfo(pathTemp / "MP_spinfo_test", false);

    CMPSPInfo::Entry sp;
    sp.denominations = {MAX_INT_8_BYTES};
    auto property = _my_sps->putSP(0, sp); // non-standard

    std::vector<uint8_t> denoms = {0, 0};
    BOOST_CHECK_EXCEPTION(
        SumDenominationsValue(property, denoms.begin(), denoms.end()),
        std::overflow_error,
        [](std::overflow_error const &e) -> bool {
            return std::string("summation of mints is overflow") == e.what();
        }
    );
}

BOOST_AUTO_TEST_SUITE_END()