/*
 *  Copyright 2011-2013 Maxim Milakov
 *
 *  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 "normalize_data_transformer.h"

#include "neural_network_exception.h"

#include <opencv2/core/core.hpp>
#include <boost/format.hpp>
#include <boost/uuid/uuid_io.hpp>

namespace nnforge
{
	// {9BABFBB9-3502-4C58-AA38-A550C1AA843D}
	const boost::uuids::uuid normalize_data_transformer::normalizer_guid =
	{ 0x9b, 0xab, 0xfb, 0xb9
	, 0x35, 0x02
	, 0x4c, 0x58
	, 0xaa, 0x38
	, 0xa5, 0x50, 0xc1, 0xaa, 0x84, 0x3d };

	normalize_data_transformer::normalize_data_transformer()
	{
	}

	normalize_data_transformer::normalize_data_transformer(const std::vector<feature_map_data_stat>& feature_map_data_stat_list)
	{
		for (std::vector<feature_map_data_stat>::const_iterator it = feature_map_data_stat_list.begin(); it != feature_map_data_stat_list.end(); ++it)
		{
			const feature_map_data_stat& current_stat = *it;
			float mult;
			float add;
			float t1 = std::max(current_stat.max - current_stat.average, current_stat.average - current_stat.min);
			if (t1 > 0.0F)
			{
				mult = 1.0F / t1;
				add = -current_stat.average * mult;
			}
			else
			{
				if (current_stat.average == 0.0F)
				{
					mult = 1.0F;
					add = 1.0F;
				}
				else
				{
					mult = 1.0F / current_stat.average;
					add = 0.0F;
				}
			}

			mul_add_list.push_back(std::make_pair(mult, add));
		}
	}

	normalize_data_transformer::~normalize_data_transformer()
	{
	}

	nnforge_shared_ptr<normalize_data_transformer> normalize_data_transformer::get_inverted_transformer() const
	{
		nnforge_shared_ptr<normalize_data_transformer> res(new normalize_data_transformer());

		for(std::vector<std::pair<float, float> >::const_iterator mul_add_it = mul_add_list.begin(); mul_add_it != mul_add_list.end(); ++mul_add_it)
		{
			float new_mult = 1.0F / mul_add_it->first;
			float new_add = -new_mult * mul_add_it->second;

			res->mul_add_list.push_back(std::make_pair(new_mult, new_add));
		}

		return res;
	}

	void normalize_data_transformer::transform(
		const void * data,
		void * data_transformed,
		neuron_data_type::input_type type,
		const layer_configuration_specific& original_config,
		unsigned int sample_id)
	{
		if (type != neuron_data_type::type_float)
			throw neural_network_exception("normalize_data_transformer is implemented for data stored as floats only");

		float * dt = static_cast<float *>(data_transformed);
		unsigned int elem_count_per_feature_map = original_config.get_neuron_count_per_feature_map();

		for(std::vector<std::pair<float, float> >::const_iterator mul_add_it = mul_add_list.begin(); mul_add_it != mul_add_list.end(); ++mul_add_it, dt += elem_count_per_feature_map)
			std::transform(dt, dt + elem_count_per_feature_map, dt, normalize_helper_struct(mul_add_it->first, mul_add_it->second));
	}

	void normalize_data_transformer::write(std::ostream& binary_stream_to_write_to) const
	{
		binary_stream_to_write_to.exceptions(std::ostream::eofbit | std::ostream::failbit | std::ostream::badbit);

		binary_stream_to_write_to.write(reinterpret_cast<const char*>(normalizer_guid.data), sizeof(normalizer_guid.data));

		unsigned int feature_map_count = static_cast<unsigned int>(mul_add_list.size());
		binary_stream_to_write_to.write(reinterpret_cast<const char*>(&feature_map_count), sizeof(feature_map_count));

		for(std::vector<std::pair<float, float> >::const_iterator it =  mul_add_list.begin(); it != mul_add_list.end(); ++it)
		{
			binary_stream_to_write_to.write(reinterpret_cast<const char*>(&(it->first)), sizeof(float));
			binary_stream_to_write_to.write(reinterpret_cast<const char*>(&(it->second)), sizeof(float));
		}

		binary_stream_to_write_to.flush();
	}

	void normalize_data_transformer::read(std::istream& binary_stream_to_read_from)
	{
 		mul_add_list.clear();

		binary_stream_to_read_from.exceptions(std::ostream::eofbit | std::ostream::failbit | std::ostream::badbit);

		boost::uuids::uuid normalizer_guid_read;
		binary_stream_to_read_from.read(reinterpret_cast<char*>(normalizer_guid_read.data), sizeof(normalizer_guid_read.data));
		if (normalizer_guid_read != normalizer_guid)
			throw neural_network_exception((boost::format("Unknown normalizer GUID encountered in input stream: %1%") % normalizer_guid_read).str());

		unsigned int feature_map_count_read;
		binary_stream_to_read_from.read(reinterpret_cast<char*>(&feature_map_count_read), sizeof(feature_map_count_read));

		for(unsigned int i = 0; i < feature_map_count_read; ++i)
		{
			float mult;
			binary_stream_to_read_from.read(reinterpret_cast<char*>(&mult), sizeof(float));
			float add;
			binary_stream_to_read_from.read(reinterpret_cast<char*>(&add), sizeof(float));

			mul_add_list.push_back(std::make_pair(mult, add));
		}
	}

 	bool normalize_data_transformer::is_deterministic() const
	{
		return true;
	}
}


/**
 * @file /sophus_ros_conversions/include/sophus_ros_conversions/eigen.hpp
 */
/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef sophus_ros_conversions_EIGEN_HPP_
#define sophus_ros_conversions_EIGEN_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <Eigen/Geometry>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace sophus_ros_conversions {

/*****************************************************************************
** Interfaces
*****************************************************************************/

void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3f &e);
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaternionf &e);
Eigen::Quaternionf quaternionMsgToEigen(const geometry_msgs::Quaternion &m);

geometry_msgs::Point eigenToPointMsg(Eigen::Vector3f &e);
geometry_msgs::Quaternion eigenToQuaternionMsg(Eigen::Quaternionf &e);

/*****************************************************************************
** Trailers
*****************************************************************************/

} // namespace sophus_ros_conversions

#endif /* sophus_ros_conversions_EIGEN_HPP_ */


#pragma once
#include <vector>
#include <cstdint>
#include <span>
#include <boost/circular_buffer.hpp>
#include <imgui.h>
#include "../imgui_commons.hpp"
namespace gui
{
    class PlotThroughput
    {
    public:
        explicit PlotThroughput(size_t window = 100);
        void addData(std::span<uint8_t> data);
        void drawPlot();

    private:
        struct DataPoint
        {
            using DataType = uint64_t;
            DataType x;
            DataType y;
        };
        ScrollingBuffer<DataPoint, DataPoint::DataType> data_sizes_;
        ScrollingBuffer<DataPoint, DataPoint::DataType> avg_data_sizes_;
        std::pair<DataPoint::DataType, DataPoint::DataType> minmax_y_;
        std::pair<DataPoint::DataType, DataPoint::DataType> minmax_x_;
    };
} // namespace gui


#include <pkmeans/pkmeans.h>
#include <pkmeans/pkmeans_lowmem.h>
#include <time.h>

#include <boost/program_options.hpp>
#include <cstdint>
#include <iostream>
#include <memory>
#include <string>

using namespace pkmeans;
using namespace std;
namespace po = boost::program_options;

int main(int argc, char **argv) {
  // Make I/O faster
  ios_base::sync_with_stdio(false);
  std::cin.tie(NULL);

  po::options_description desc(
      "pkmeans, a parallized k-means++ application using EMD.\n");

  desc.add_options ()
    ("help,?",              "produce help message")
    ("clusters,k",          po::value<unsigned int> ()->default_value (8),                      "number of clusters")
    ("threads,t",           po::value<unsigned int> ()->default_value (1),                      "number of threads")
    ("in_filename,i",       po::value<std::string> ()->default_value ("in.txt"),                "input filename for distributions")
    ("assignments_out,a",   po::value<std::string> ()->default_value ("assignments_out.txt"),   "cluster assignments to distributions output filename")
    ("clusters_out,c",      po::value<std::string> ()->default_value ("clusters_out.txt"),      "clusters output filename")
    ("confidence_prob,p",   po::value<float> ()->default_value       (0.1f),                    "confidence probability of the stopping criteria upper bound for random restarts")
    ("missing_mass,m",      po::value<float> ()->default_value       (0.1f),                    "maximum acceptable missing mass for random restarts")
    ("seed,s",              po::value<size_t> ()->default_value      (size_t(-1)),              "starting random seed")
    ("backup_every,b",      po::value<size_t> ()->default_value      (0),                       "backup every b itterations")
    ("euclidean,e",         "use euclidean L2 distance instead of earth mover's distance")
    ("low_mem,l",           "use low memory version")
    ("quiet,q",             "reduce logs in console");

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv)
                .style(po::command_line_style::unix_style)
                .options(desc)
                .run(),
            vm);
  po::notify(vm);

  // check for help
  if (vm.count("help") || argc == 1) {
    std::cout << desc << std::endl;
    return 1;
  }

  const auto numClusters = vm["clusters"].as<unsigned int>();
  const auto numThreads = vm["threads"].as<unsigned int>();
  const auto confidenceProb = vm["confidence_prob"].as<float>();
  const auto missingMass = vm["missing_mass"].as<float>();
  const auto seed = vm["seed"].as<size_t>();
  const auto useTimeSeed = size_t(-1) == seed;
  const auto backupEvery = vm["backup_every"].as<size_t>();
  const auto inFilename = vm["in_filename"].as<std::string>();
  const auto assignmentsOut = vm["assignments_out"].as<std::string>();
  const auto clustersOut = vm["clusters_out"].as<std::string>();
  const auto lowMem = vm.count("low_mem") > 0;
  const auto euclidean = vm.count("euclidean") > 0;
  const auto quiet = vm.count("quiet") > 0;

  if (lowMem) {
    auto pkmeans = std::make_unique<PKMeans<std::uint8_t>>();
    pkmeans->run(numClusters, numThreads, confidenceProb, missingMass, seed,
                 backupEvery, useTimeSeed, inFilename, assignmentsOut,
                 clustersOut, euclidean, quiet);
  } else {
    auto pkmeans = std::make_unique<PKMeans<float>>();
    pkmeans->run(numClusters, numThreads, confidenceProb, missingMass, seed,
                 backupEvery, useTimeSeed, inFilename, assignmentsOut,
                 clustersOut, euclidean, quiet);
  }
  return 0;
}


#ifndef BART_SRC_PROBLEM_PARAMETERS_DEALII_HANDLER_HPP_
#define BART_SRC_PROBLEM_PARAMETERS_DEALII_HANDLER_HPP_

#include <map>
#include <string>
#include <unordered_map>

#include <deal.II/base/parameter_handler.h>

#include "problem/parameters_i.hpp"
#include "problem/parameter_types.hpp"

namespace bart::problem {

/*!
 * \brief Problem parameters derived using a dealii ParameterHandler object.
 *
 * The ParameterHandler can be given directly or a file that can be parsed into
 * a ParameterHandler object can be passed.
 */

class ParametersDealiiHandler : public ParametersI {
 public:
  using K_EffectiveUpdaterName = eigenvalue::k_eigenvalue::K_EffectiveUpdaterName;
  struct KeyWords {
    /*!
     * \brief Data struct to contain keywords for input files
     * These are the strings that the ParameterHandler will look for in provided
     * input files.
     */

    // Equation parameters
    const std::string kNEnergyGroups_{ "number of groups" }; // change to energy groups
    const std::string kTransportModel_{ "transport model"} ;

    // Domain parameters
    const std::string kHaveReflectiveBC_{ "have reflective boundary" };
    const std::string kNCells_{ "number of cells for x, y, z directions" };
    const std::string kReflectiveBoundary_{ "reflective boundary names" };
    const std::string kSpatialDimension_{ "problem dimension" };
    const std::string kSpatialMax_{ "x, y, z max values of boundary locations" };
    const std::string kUniformRefinements_{ "uniform refinements" };

    // FEM Parameters
    const std::string kDiscretization_{ "ho spatial discretization" };
    const std::string kFEPolynomialDegree_{ "finite element polynomial degree" };

    // Material parameters
    const std::string kMaterialSubsection_{ "material ID map" };
    const std::string kMaterialMapFilename_{ "material id file name" };
    const std::string kMaterialFilenames_{ "material id file name map" };
    const std::string kNumberOfMaterials_{ "number of materials" };

    // Acceleration parameters
    const std::string kUseTwoGridAcceleration_{ "use two-grid acceleration" };
    const std::string kDoNDA_{ "do nda" };

    // Solver parameters
    const std::string kEigenSolver_{ "eigen solver name" };
    const std::string kK_EffectiveUpdaterType_{ "k_effective updater type" };
    const std::string kInGroupSolver_{ "in group solver name" };
    const std::string kLinearSolver_{ "ho linear solver name" };

    // Quadrature
    const std::string kAngularQuad_{ "angular quadrature name" };
    const std::string kAngularQuadOrder_{ "angular quadrature order" };

    // Instrumentation and output
    const std::string kOutputFilenameBase_{ "output file name base" };
    const std::string kOutputAggregatedSourceData_{ "output aggregated source data"};
    const std::string kOutputScalarFluxAsVTU_{ "output scalar flux as vtu"};
    const std::string kOutputFissionSourceAsVTU_{ "output fission source as vtu"};
    const std::string kOutputScatteringSourceAsVTU_{ "output scattering source as vtu"};
    const std::string kOutputInnerIterationsToFile_{ "output inner iterations to file"};

    // Fourier analysis
    const std::string kDoDFTOfError_{ "do dft of error" };
  };

  ParametersDealiiHandler() = default;
  /*! Constructor that parses a given filename in the appropriate format to be
   * read by a ParameterHandler object.
   */
  explicit ParametersDealiiHandler(const std::string& filename);

  /*! \brief Parses a ParameterHandler object for problem parameters */
  auto Parse(dealii::ParameterHandler&) -> void;

  /*! \brief Set up a ParameterHandler object for problem parameters.
   * This setup includes default values.
   */
  auto SetUp(dealii::ParameterHandler&) const -> void;

  // Equation parameters
  auto NEnergyGroups() const -> int override { return n_groups_; }
  auto TransportModel() const -> EquationType override { return transport_model_; }

  // Domain parameters
  auto HaveReflectiveBC() const -> bool override { return have_reflective_bc_; }
  auto NCells() const -> std::vector<int> override { return n_cells_; }
  auto ReflectiveBoundary() const -> std::map<Boundary, bool> override { return reflective_boundary_; }
  auto SpatialDimension() const -> int override { return spatial_dimension_; }
  auto SpatialMax() const -> std::vector<double> override { return spatial_max; }
  auto UniformRefinements() const -> int override { return uniform_refinements_; }

  // FEM Parameters
  auto Discretization() const -> DiscretizationType override { return discretization_; }
  auto FEPolynomialDegree() const -> int override { return fe_polynomial_degree_; }

  // Material parameters
  auto MaterialMapFilename() const -> std::string override { return material_map_filename_; }
  auto MaterialFilenames() const -> std::unordered_map<int, std::string> override { return material_filenames_; }
  auto NumberOfMaterials() const -> int override { return n_materials_; }

  // Acceleration parameters
  auto UseTwoGridAcceleration() const -> bool override { return use_two_grid_acceleration_; };
  auto DoNDA() const -> bool override { return do_nda_; }

  // Solver parameters
  auto EigenSolver() const -> EigenSolverType override { return eigen_solver_; }
  auto K_EffectiveUpdaterType() const -> K_EffectiveUpdaterName override { return k_effective_updater_type_; };
  auto InGroupSolver() const -> InGroupSolverType override { return in_group_solver_; }
  auto LinearSolver() const -> LinearSolverType override { return linear_solver_; }

  // Quadrature parameters
  auto AngularQuad() const -> AngularQuadType override { return angular_quad_; }
  auto AngularQuadOrder() const -> int override { return angular_quad_order_; }

  // Instrumentation and output
  auto DoDiscreteFourierTransformOfError() const noexcept -> bool override { return do_dft_of_error_; }
  auto OutputFilenameBase() const noexcept -> std::string override { return output_filename_base_; }
  auto OutputAggregatedSourceData() const -> bool override { return output_aggregated_source_data_; }
  auto OutputScalarFluxAsVTU() const -> bool override { return output_scalar_flux_as_vtu_; }
  auto OutputFissionSourceAsVTU() const -> bool override { return output_fission_source_as_vtu_; }
  auto OutputScatteringSourceAsVTU() const -> bool override { return output_scattering_source_as_vtu_; }
  auto OutputInnerIterationsToFile() const -> bool override { return output_inner_iterations_to_file_; }

  auto GetKeyWords() const noexcept -> KeyWords { return key_words_; }

 private:
  // Equation parameters
  int                                  n_groups_{ 0 };
  EquationType                         transport_model_{ EquationType::kNone };
  // Domain parameters
  bool                                 have_reflective_bc_{ false };
  std::vector<int>                     n_cells_{};
  std::map<Boundary, bool>             reflective_boundary_{};
  int                                  spatial_dimension_{ 0 };
  std::vector<double>                  spatial_max{};
  int                                  uniform_refinements_{ 0 };
  // FEM Parameters
  DiscretizationType                   discretization_{ DiscretizationType::kNone };
  int                                  fe_polynomial_degree_{ 0 };
  // Material parameters
  int                                  n_materials_{ 0 };
  std::string                          material_map_filename_{};
  std::unordered_map<int, std::string> material_filenames_{};
  // Acceleration parameters
  bool                                 use_two_grid_acceleration_{ false };
  bool                                 do_nda_{ false };
  // Solver parameters
  EigenSolverType                      eigen_solver_{ EigenSolverType::kNone };
  K_EffectiveUpdaterName               k_effective_updater_type_{ K_EffectiveUpdaterName::kCalculatorViaFissionSource };
  InGroupSolverType                    in_group_solver_{ InGroupSolverType::kNone };
  LinearSolverType                     linear_solver_{ LinearSolverType::kNone };
  //Quadrature
  AngularQuadType                      angular_quad_{ AngularQuadType::kNone };
  int                                  angular_quad_order_{ 0 };

  // Instrumentation and output
  std::string                          output_filename_base_{};
  bool                                 do_dft_of_error_{ false };
  bool                                 output_aggregated_source_data_{ false };
  bool                                 output_scalar_flux_as_vtu_{ false };
  bool                                 output_fission_source_as_vtu_{ false };
  bool                                 output_scattering_source_as_vtu_{ false };
  bool                                 output_inner_iterations_to_file_{ false };

  // Key-words struct
  KeyWords                             key_words_{};

  // Options mapping
  // The KeyWords struct provides the "key words" that dealii identifies when looking for options, these are
  // the options and what they map to.

  const std::unordered_map<std::string, Boundary> kBoundaryMap_ {
    {"xmin", Boundary::kXMin}, {"xmax", Boundary::kXMax},
    {"ymin", Boundary::kYMin}, {"ymax", Boundary::kYMax},
    {"zmin", Boundary::kZMin}, {"zmax", Boundary::kZMax},
    }; /*!< Maps boundaries to strings used in parsed input files. */

  const std::unordered_map<std::string, DiscretizationType> kDiscretizationTypeMap_ {
    {"none", DiscretizationType::kNone},
    {"cfem", DiscretizationType::kContinuousFEM},
    {"dfem", DiscretizationType::kDiscontinuousFEM}, }; //!< Maps discretization type to strings used in parsed input

  const std::unordered_map<std::string, EquationType> kEquationTypeMap_ {
      {"none", EquationType::kNone},
      {"diffusion", EquationType::kDiffusion},
      {"saaf",      EquationType::kSelfAdjointAngularFlux} }; /*!< Maps equation type to strings used in parsed input files. */

  const std::unordered_map<std::string, EigenSolverType> kEigenSolverTypeMap_ {
    {"pi",   EigenSolverType::kPowerIteration},
    {"none", EigenSolverType::kNone},
        }; /*!< Maps eigen solver type to strings used in parsed input files. */

  const std::unordered_map<std::string, K_EffectiveUpdaterName> kK_EffectiveUpdaterNameMap_ {
      {"fission source", K_EffectiveUpdaterName::kCalculatorViaFissionSource},
      {"rayleigh quotient", K_EffectiveUpdaterName::kCalculatorViaRayleighQuotient}
  };

  const std::unordered_map<std::string, InGroupSolverType>
  kInGroupSolverTypeMap_ {
    {"si",   InGroupSolverType::kSourceIteration},
    {"none", InGroupSolverType::kNone},
        }; /*!< Maps in-group solver type to strings used in parsed input
            * files. */

  const std::unordered_map<std::string, LinearSolverType> kLinearSolverTypeMap_ {
    {"gmres",    LinearSolverType::kGMRES},
    {"none",     LinearSolverType::kNone},
        };  /*!< Maps linear solver type to strings used in parsed input
             * files. */

  const std::unordered_map<std::string, AngularQuadType> kAngularQuadTypeMap_ {
    {"level_symmetric_gaussian", AngularQuadType::kLevelSymmetricGaussian},
    {"gauss_legendre",           AngularQuadType::kGaussLegendre},
    {"none",                     AngularQuadType::kNone},
  }; /*!< Maps angular quadrature type to strings used in parsed input files. */

  // Setup functions
  /*! \brief Set up basic problem parameters */
  auto SetUpBasicParameters(dealii::ParameterHandler&) const -> void;
  /*! \brief Set up mesh parameters */
  auto SetUpMeshParameters(dealii::ParameterHandler&) const -> void;
  /*! \brief Set up material parameters */
  auto SetUpMaterialParameters(dealii::ParameterHandler&) const -> void;
  /*! \brief Set up acceleration parameters */
  auto SetUpAccelerationParameters(dealii::ParameterHandler&) const -> void;
  /*! \brief Set up solver parameters */
  auto SetUpSolverParameters(dealii::ParameterHandler&) const -> void;
  /*! \brief Set up angular quadrature parameters */
  auto SetUpAngularQuadratureParameters(dealii::ParameterHandler&) const -> void;

  /*! \brief Parses a ParameterHandler entry of type dealii::Patterns::List with
   * doubles into a vector.
   */
  auto ParseDealiiList(std::string to_parse) const -> std::vector<double>;
  auto ParseDealiiIntList(std::string to_parse) const -> std::vector<int>;

  /*! \brief Parses the Material Filename mapping which is of the following
   * deal.II pattern: Map(Integer, Anything)
   */
  auto ParseMap(std::string to_parse) const -> std::unordered_map<int, std::string>;

  /*! \brief Parses a ParameterHandler entry of type
   * dealii::Patterns::MultipleSelection returning a map of one type to another
   */
  template<typename Key>
  auto ParseDealiiMultiple(std::string to_parse,
                           std::unordered_map<std::string, Key> enum_map) const -> std::map<Key, bool>;

  /*! \brief Returns a string formed by combining the key strings in a mapping.
   *
   * Entries will be separated by `|`. Used to generate valid option strings for
   * ParameterHandler entries. Optional parameter allows a list of options to
   * ignore.
   */
  template<typename T>
  auto GetOptionString(std::unordered_map<std::string, T> enum_map, std::vector<T> to_ignore) const -> std::string;
  template<typename T>
  auto GetOptionString(std::unordered_map<std::string, T> enum_map, T to_ignore) const -> std::string;
  template<typename T>
  auto GetOptionString(std::unordered_map<std::string, T> enum_map) const -> std::string;
};

} // namespace bart::problem

#endif // BART_SRC_PROBLEM_PARAMETERS_DEALII_HANDLER_HPP_


/* ----------------------------------------------------------------------------

* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)

* See LICENSE for the license information

* -------------------------------------------------------------------------- */

/**
* @file   testSymmetricBlockMatrix.cpp
* @brief  Unit tests for SymmetricBlockMatrix class
* @author Richard Roberts
**/

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <boost/assign/list_of.hpp>

using namespace std;
using namespace gtsam;
using boost::assign::list_of;

static SymmetricBlockMatrix testBlockMatrix(
  list_of(3)(2)(1),
  (Matrix(6, 6) <<
  1, 2, 3, 4, 5, 6,
  2, 8, 9, 10, 11, 12,
  3, 9, 15, 16, 17, 18,
  4, 10, 16, 22, 23, 24,
  5, 11, 17, 23, 29, 30,
  6, 12, 18, 24, 30, 36));

/* ************************************************************************* */
TEST(SymmetricBlockMatrix, ReadBlocks)
{
  // On the diagonal
  Matrix expected1 = (Matrix(2, 2) <<
    22, 23,
    23, 29);
  Matrix actual1 = testBlockMatrix(1, 1);
  // Test only writing the upper triangle for efficiency
  Matrix actual1t = Matrix::Zero(2, 2);
  actual1t.triangularView<Eigen::Upper>() = testBlockMatrix(1, 1).triangularView();
  EXPECT(assert_equal(expected1, actual1));
  EXPECT(assert_equal(Matrix(expected1.triangularView<Eigen::Upper>()), actual1t));

  // Above the diagonal
  Matrix expected2 = (Matrix(3, 2) <<
    4, 5,
    10, 11,
    16, 17);
  Matrix actual2 = testBlockMatrix(0, 1);
  EXPECT(assert_equal(expected2, actual2));

  // Below the diagonal
  Matrix expected3 = (Matrix(2, 3) <<
    4, 10, 16,
    5, 11, 17);
  Matrix actual3 = testBlockMatrix(1, 0);
  EXPECT(assert_equal(expected3, actual3));
}

/* ************************************************************************* */
TEST(SymmetricBlockMatrix, WriteBlocks)
{
  // On the diagonal
  Matrix expected1 = testBlockMatrix(1, 1);
  SymmetricBlockMatrix bm1 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
  bm1(1, 1) = expected1.selfadjointView<Eigen::Upper>(); // Verified with debugger that this only writes the upper triangle
  Matrix actual1 = bm1(1, 1);
  EXPECT(assert_equal(expected1, actual1));

  // On the diagonal
  Matrix expected1p = testBlockMatrix(1, 1);
  SymmetricBlockMatrix bm1p = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
  bm1p(1, 1) = expected1p; // Verified with debugger that this only writes the upper triangle
  Matrix actual1p = bm1p(1, 1);
  EXPECT(assert_equal(expected1p, actual1p));

  // Above the diagonal
  Matrix expected2 = testBlockMatrix(0, 1);
  SymmetricBlockMatrix bm2 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
  bm2(0, 1) = expected2;
  Matrix actual2 = bm2(0, 1);
  EXPECT(assert_equal(expected2, actual2));

  // Below the diagonal
  Matrix expected3 = testBlockMatrix(1, 0);
  SymmetricBlockMatrix bm3 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
  bm3(1, 0) = expected3;
  Matrix actual3 = bm3(1, 0);
  EXPECT(assert_equal(expected3, actual3));
}

/* ************************************************************************* */
TEST(SymmetricBlockMatrix, Ranges)
{
  // On the diagonal
  Matrix expected1 = (Matrix(3, 3) <<
    22, 23, 24,
    23, 29, 30,
    24, 30, 36);
  Matrix actual1 = testBlockMatrix.range(1, 3, 1, 3).selfadjointView();
  Matrix actual1a = testBlockMatrix.range(1, 3, 1, 3);
  EXPECT(assert_equal(expected1, actual1));
  EXPECT(assert_equal(expected1, actual1a));

  // Above the diagonal
  Matrix expected2 = (Matrix(3, 1) <<
    24,
    30,
    36);
  Matrix actual2 = testBlockMatrix.range(1, 3, 2, 3).knownOffDiagonal();
  Matrix actual2a = testBlockMatrix.range(1, 3, 2, 3);
  EXPECT(assert_equal(expected2, actual2));
  EXPECT(assert_equal(expected2, actual2a));

  // Below the diagonal
  Matrix expected3 = (Matrix(3, 3) <<
    4, 10, 16,
    5, 11, 17,
    6, 12, 18);
  Matrix actual3 = testBlockMatrix.range(1, 3, 0, 1).knownOffDiagonal();
  Matrix actual3a = testBlockMatrix.range(1, 3, 0, 1);
  EXPECT(assert_equal(expected3, actual3));
  EXPECT(assert_equal(expected3, actual3a));
}

/* ************************************************************************* */
TEST(SymmetricBlockMatrix, expressions)
{
  SymmetricBlockMatrix expected1(list_of(2)(3)(1), (Matrix(6, 6) <<
    0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0,
    0, 0, 4, 6, 8, 0,
    0, 0, 0, 9, 12, 0,
    0, 0, 0, 0, 16, 0,
    0, 0, 0, 0, 0, 0));

  SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Matrix(6, 6) <<
    0, 0, 10, 15, 20, 0,
    0, 0, 12, 18, 24, 0,
    0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0));

  Matrix a = (Matrix(1, 3) << 2, 3, 4);
  Matrix b = (Matrix(1, 2) << 5, 6);

  SymmetricBlockMatrix bm1(list_of(2)(3)(1));
  bm1.full().triangularView().setZero();
  bm1(1, 1).selfadjointView().rankUpdate(a.transpose());
  EXPECT(assert_equal(Matrix(expected1.full().selfadjointView()), bm1.full().selfadjointView()));

  SymmetricBlockMatrix bm2(list_of(2)(3)(1));
  bm2.full().triangularView().setZero();
  bm2(0, 1).knownOffDiagonal() += b.transpose() * a;
  EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm2.full().selfadjointView()));

  SymmetricBlockMatrix bm3(list_of(2)(3)(1));
  bm3.full().triangularView().setZero();
  bm3(1, 0).knownOffDiagonal() += a.transpose() * b;
  EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm3.full().selfadjointView()));

  SymmetricBlockMatrix bm4(list_of(2)(3)(1));
  bm4.full().triangularView().setZero();
  bm4(1, 1) += expected1(1, 1);
  EXPECT(assert_equal(Matrix(expected1.full().selfadjointView()), bm4.full().selfadjointView()));

  SymmetricBlockMatrix bm5(list_of(2)(3)(1));
  bm5.full().triangularView().setZero();
  bm5(0, 1) += expected2(0, 1);
  EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm5.full().selfadjointView()));

  SymmetricBlockMatrix bm6(list_of(2)(3)(1));
  bm6.full().triangularView().setZero();
  bm6(1, 0) += expected2(1, 0);
  EXPECT(assert_equal(Matrix(expected2.full().selfadjointView()), bm6.full().selfadjointView()));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


/*
 * convergence_report.hpp
 *
 *  Created on: Mar 15, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   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.
 */

#pragma once
//stdlib
#include <iostream>

//libraries
#include <Eigen/Eigen>

//local
#include "../math/typedefs.hpp"
#include "../telemetry/tsdf_difference_statistics.hpp"
#include "../telemetry/warp_delta_statistics.hpp"

namespace eig = Eigen;

namespace telemetry {
/**
 * A structure for logging characteristics of convergence (warp-threshold-based optimization) after a full optimization run
 */
template<typename Coordinates>
struct ConvergenceReport {
	int iteration_count = 0;
	bool iteration_limit_reached = false;
	WarpDeltaStatistics<Coordinates> warp_delta_statistics;
	TsdfDifferenceStatistics<Coordinates> tsdf_difference_statistics;

	ConvergenceReport() = default;
	ConvergenceReport(
			int iteration_count,
			bool iteration_limit_reached,
			WarpDeltaStatistics<Coordinates> warp_delta_statistics,
			TsdfDifferenceStatistics<Coordinates> tsdf_difference_statistics
			);

	bool operator==(const ConvergenceReport& rhs) {
		return this->iteration_count == rhs.iteration_count &&
				this->iteration_limit_reached == rhs.iteration_limit_reached &&
				this->warp_delta_statistics == rhs.warp_delta_statistics &&
				this->tsdf_difference_statistics == rhs.tsdf_difference_statistics;
	}
	bool operator!=(const ConvergenceReport& rhs) {
		return !(*this == rhs);
	}

};
template<typename Coordinates>
std::ostream &operator<<(std::ostream &ostr, const ConvergenceReport<Coordinates> &ts);

typedef ConvergenceReport<math::Vector2i> ConvergenceReport2d;
typedef ConvergenceReport<math::Vector3i> ConvergenceReport3d;

} //namespace telemetry


#ifndef STAN_MATH_PRIM_PROB_VON_MISES_RNG_HPP
#define STAN_MATH_PRIM_PROB_VON_MISES_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/constants.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a von Mises random variate for the given location and concentration
 * using the specified random number generator.
 *
 * mu and kappa can each be a scalar or a vector. Any non-scalar
 * inputs must be the same length.
 *
 * The algorithm used in von_mises_rng is a modified version of the
 * algorithm in:
 *
 * Efficient Simulation of the von Mises Distribution
 * D. J. Best and N. I. Fisher
 * Journal of the Royal Statistical Society. Series C (Applied Statistics),
 * Vol. 28, No. 2 (1979), pp. 152-157
 *
 * For kappa < 1.4e-8, this reduces to a uniform distribution.
 *
 * @tparam T_loc type of location parameter
 * @tparam T_conc type of scale (concentration) parameter
 * @tparam RNG type of random number generator
 *
 * @param mu (Sequence of) location parameter(s)
 * @param kappa (Sequence of) non-negative scale (concentration) parameter(s)
 * @param rng random number generator
 * @return (Sequence of) von Mises random variate(s)
 * @throw std::domain_error if mu is infinite or kappa is negative
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_loc, typename T_conc, class RNG>
inline typename VectorBuilder<true, double, T_loc, T_conc>::type von_mises_rng(
    const T_loc& mu, const T_conc& kappa, RNG& rng) {
  using boost::random::uniform_real_distribution;
  using boost::variate_generator;
  static const char* function = "von_mises_rng";

  check_finite(function, "Location parameter", mu);
  check_nonnegative(function, "Scale parameter", kappa);
  check_finite(function, "Scale parameter", kappa);
  check_consistent_sizes(function, "Location parameter", mu, "Scale parameter",
                         kappa);

  scalar_seq_view<T_loc> mu_vec(mu);
  scalar_seq_view<T_conc> kappa_vec(kappa);
  size_t N = max_size(mu, kappa);
  VectorBuilder<true, double, T_loc, T_conc> output(N);

  variate_generator<RNG&, uniform_real_distribution<> > uniform_rng(
      rng, uniform_real_distribution<>(0.0, 1.0));

  for (size_t n = 0; n < N; ++n) {
    // for kappa sufficiently close to zero, it reduces to a
    // circular uniform distribution centered at mu
    if (kappa_vec[n] < 1.4e-8) {
      output[n] = (uniform_rng() - 0.5) * TWO_PI
                  + std::fmod(std::fmod(mu_vec[n], TWO_PI) + TWO_PI, TWO_PI);
      continue;
    }

    double r = 1 + std::pow((1 + 4 * kappa_vec[n] * kappa_vec[n]), 0.5);
    double rho = 0.5 * (r - std::pow(2 * r, 0.5)) / kappa_vec[n];
    double s = 0.5 * (1 + rho * rho) / rho;

    bool done = false;
    double W;
    while (!done) {
      double Z = std::cos(pi() * uniform_rng());
      W = (1 + s * Z) / (s + Z);
      double Y = kappa_vec[n] * (s - W);
      double U2 = uniform_rng();
      done = Y * (2 - Y) - U2 > 0;

      if (!done) {
        done = std::log(Y / U2) + 1 - Y >= 0;
      }
    }

    double U3 = uniform_rng() - 0.5;
    double sign = ((U3 >= 0) - (U3 <= 0));

    //  it's really an fmod() with a positivity constraint
    output[n] = sign * std::acos(W)
                + std::fmod(std::fmod(mu_vec[n], TWO_PI) + TWO_PI, TWO_PI);
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


/***************************************************************************************************************************
 * landpad_det.cpp
 * Author: Jario
 * Update Time: 2020.1.12
 *
 * 说明: 降落目标识别程序，降落板的尺寸为60cmX60cm
 *      1. 【订阅】图像话题 (默认来自web_cam)
 *         /prometheus/camera/rgb/image_raw
 *      2. 【发布】目标位置，发布话题见 Prometheus/Modules/msgs/msg/DetectionInfo.msg
 *         /prometheus/object_detection/landpad_det
 *      3. 【发布】检测结果的可视化图像话题
 *         /prometheus/camera/rgb/image_landpad_det
***************************************************************************************************************************/

#include <time.h>
#include <fstream>
#include <iostream>
#include <math.h>
#include <pthread.h>
#include <thread>
#include <chrono>
#include <numeric>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

// ros头文件
#include <ros/ros.h>
#include <yaml-cpp/yaml.h>
#include <ros/package.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

// opencv头文件
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>

// topic 头文件
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Bool.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/Message.h>

#include "message_utils.h"

using namespace std;
using namespace cv;

double threshold_error=0.4;

//---------------------------variables---------------------------------------
//------------ROS TOPIC---------
//【订阅】无人机位置
ros::Subscriber drone_pose_sub;
//【订阅】小车位置
ros::Subscriber vehicle_pose_sub;
//【订阅】输入图像
image_transport::Subscriber image_subscriber;
//【订阅】输入开关量
ros::Subscriber switch_subscriber;
//【发布】无人机和小车相对位置
ros::Publisher position_pub;
//【发布】识别后的图像
image_transport::Publisher landpad_pub;
//【发布】调试消息
ros::Publisher message_pub;
std::string msg_node_name;

//-------------VISION-----------
Mat img;
prometheus_msgs::DetectionInfo pose_now;

//-------------TIME-------------
ros::Time begin_time;
float photo_time;
double calculation_time;

// 相机话题中的图像同步相关变量
int frame_width, frame_height;
std_msgs::Header image_header;
cv::Mat cam_image_copy;
boost::shared_mutex mutex_image_callback;
bool image_status = false;
boost::shared_mutex mutex_image_status;

// 无人机位姿message
geometry_msgs::Pose pos_drone_optitrack;
Eigen::Vector3d euler_drone_optitrack;
Eigen::Quaterniond q_drone;
// 小车位姿message
geometry_msgs::Pose pos_vehicle_optitrack;
Eigen::Vector3d euler_vehicle_optitrack;
Eigen::Quaterniond q_vehicle;

// 保存的上次观测的位置 用于cluster算法使用
Eigen::Vector3d last_position;
bool bool_last_position = false;
// 接收消息，允许暂停检测
bool is_suspanded = false;
bool local_print = false;
bool message_print = true;

void printf_result();

//-----------------利用Euler角进行三次旋转得到无人机相对目标的位置------------------
void CodeRotateByZ(double x, double y, double thetaz, double& outx, double& outy)
{
    double x1 = x;  // 将变量拷贝一次，保证&x == &outx这种情况下也能计算正确
    double y1 = y;
    double rz = thetaz * CV_PI / 180;
    outx = cos(rz) * x1 - sin(rz) * y1;
    outy = sin(rz) * x1 + cos(rz) * y1;
}
void CodeRotateByY(double x, double z, double thetay, double& outx, double& outz)
{
    double x1 = x;
    double z1 = z;
    double ry = thetay * CV_PI / 180;
    outx = cos(ry) * x1 + sin(ry) * z1;
    outz = cos(ry) * z1 - sin(ry) * x1;
}
void CodeRotateByX(double y, double z, double thetax, double& outy, double& outz)
{
    double y1 = y;  // 将变量拷贝一次，保证&y == &y这种情况下也能计算正确
    double z1 = z;
    double rx = thetax * CV_PI / 180;
    outy = cos(rx) * y1 - sin(rx) * z1;
    outz = cos(rx) * z1 + sin(rx) * y1;
}
// 四元数转Euler
// q0 q1 q2 q3
// w x y z
void quaternion_2_euler(Eigen::Quaterniond quat, Eigen::Vector3d &angle)
{
    angle(0) = atan2(2.0 * (quat.z() * quat.y() + quat.w() * quat.x()), 1.0 - 2.0 * (quat.x() * quat.x() + quat.y() * quat.y()));
    angle(1) = asin(2.0 * (quat.y() * quat.w() - quat.z() * quat.x()));
    // angle[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), -1.0 + 2.0 * (quat[0] * quat[0] + quat[1] * quat[1]));
    angle(2) = atan2(2.0 * (quat.z() * quat.w() + quat.x() * quat.y()), 1.0 - 2.0 * (quat.y() * quat.y() + quat.z() * quat.z()));
}

//--------------------------利用optitrack获取真值-------------------------------

// 获取系统时间
float get_dt(ros::Time last)
{
    ros::Time time_now = ros::Time::now();
    float currTimeSec = time_now.sec-last.sec;
    float currTimenSec = time_now.nsec / 1e9 - last.nsec / 1e9;
    return (currTimeSec + currTimenSec);
}

// 图像接收回调函数，接收web_cam的话题，并将图像保存在cam_image_copy中
void cameraCallback(const sensor_msgs::ImageConstPtr& msg)
{
    if (local_print)
        ROS_DEBUG("[LandpadDetector] USB image received.");

    cv_bridge::CvImagePtr cam_image;

    try {
        cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        image_header = msg->header;
    } catch (cv_bridge::Exception& e) {
        if (local_print)
            ROS_ERROR("cv_bridge exception: %s", e.what());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::ERROR, msg_node_name, "cv_bridge exception");
        return;
    }

    if (cam_image) {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            cam_image_copy = cam_image->image.clone();
        }
        {
            boost::unique_lock<boost::shared_mutex> lockImageStatus(mutex_image_status);
            image_status = true;
        }
        frame_width = cam_image->image.size().width;
        frame_height = cam_image->image.size().height;
    }
    return;
}

// 用此函数查看是否收到图像话题
bool getImageStatus(void)
{
    boost::shared_lock<boost::shared_mutex> lock(mutex_image_status);
    return image_status;
}

void switchCallback(const std_msgs::Bool::ConstPtr& msg)
{
    is_suspanded = !(bool)msg->data;
    // cout << is_suspanded << endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "landpad_det");
    ros::NodeHandle nh("~");
    image_transport::ImageTransport it(nh);

    // 发布调试消息
    msg_node_name = "/prometheus/message/landpad_det";
    message_pub = nh.advertise<prometheus_msgs::Message>(msg_node_name, 10);

    std::string camera_topic, camera_info;
    if (nh.getParam("camera_topic", camera_topic)) {
        if (local_print)
            ROS_INFO("camera_topic is %s", camera_topic.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_topic is" + camera_topic);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_topic");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_topic");
        camera_topic = "/prometheus/camera/rgb/image_raw";
    }

    if (nh.getParam("camera_info", camera_info)) {
        if (local_print)
            ROS_INFO("camera_info is %s", camera_info.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_info is" + camera_info);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_info");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_info");
        camera_info = "camera_param.yaml";
    }

    position_pub = nh.advertise<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/landpad_det", 10);

    // 接收开关话题
    switch_subscriber = nh.subscribe("/prometheus/switch/landpad_det", 10, switchCallback);

    // 接收图像的话题
    image_subscriber = it.subscribe(camera_topic.c_str(), 1, cameraCallback);
    // 发布ArUco检测结果的话题
    landpad_pub = it.advertise("/prometheus/camera/rgb/image_landpad_det", 1);

    sensor_msgs::ImagePtr msg_ellipse;

    std::string ros_path = ros::package::getPath("prometheus_detection");

    if (local_print)
        cout << "DETECTION_PATH: " << ros_path << endl;
    if (message_print)
        pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "DETECTION_PATH: " + ros_path);

    // 读取参数文档camera_param.yaml中的参数值；
    YAML::Node camera_config = YAML::LoadFile(camera_info);
    // 相机内部参数
    double fx = camera_config["fx"].as<double>();
    double fy = camera_config["fy"].as<double>();
    double cx = camera_config["x0"].as<double>();
    double cy = camera_config["y0"].as<double>();
    // 相机畸变系数
    double k1 = camera_config["k1"].as<double>();
    double k2 = camera_config["k2"].as<double>();
    double p1 = camera_config["p1"].as<double>();
    double p2 = camera_config["p2"].as<double>();
    double k3 = camera_config["k3"].as<double>();

    double landpad_det_len = camera_config["landpad_det_len"].as<double>();
    // DEBUG
    // cout << fx << " " << fy << " " << cx << " " << cy << " " << k1 << " " << k2 << " ";

    //--------------------------相机参数赋值---------------------
    // 相机内参
    Mat camera_matrix;
    camera_matrix = cv::Mat(3,3,CV_64FC1,cv::Scalar::all(0));
    camera_matrix.ptr<double>(0)[0] = fx;
    camera_matrix.ptr<double>(0)[2] = cx;
    camera_matrix.ptr<double>(1)[1] = fy;
    camera_matrix.ptr<double>(1)[2] = cy;
    camera_matrix.ptr<double>(2)[2] = 1.0f;
    // 相机畸变参数k1 k2 p1 p2 k3
    Mat distortion_coefficients;
    distortion_coefficients = cv::Mat(5,1,CV_64FC1,cv::Scalar::all(0));
    distortion_coefficients.ptr<double>(0)[0] = k1;
    distortion_coefficients.ptr<double>(1)[0] = k2;
    distortion_coefficients.ptr<double>(2)[0] = p1;
    distortion_coefficients.ptr<double>(3)[0] = p2;
    distortion_coefficients.ptr<double>(4)[0] = k3;

    // ArUco Marker字典选择以及旋转向量和评议向量初始化
    Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(10);

    vector<double> rv(3), tv(3);
    cv::Mat rvec(rv), tvec(tv);
    // cv::VideoCapture capture(0);
    float last_x(0), last_y(0), last_z(0), last_yaw(0), last_az(0), last_ay(0), last_ax(0), last_qx(0), last_qy(0), last_qz(0), last_qw(0);
    bool switch_state = is_suspanded;

    // 节点运行频率： 20hz 【视觉端解算频率大概为20HZ】
    ros::Rate loopRate(20);
    ros::Rate loopRate_1Hz(1);
    //----------------------------------------主循环------------------------------------
    // const auto wait_duration = std::chrono::milliseconds(2000);
    while (ros::ok())
    {
        while (!getImageStatus() && ros::ok())
        {
            if (local_print)
                cout << "Waiting for image." << endl;
            if (message_print)
                pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Waiting for image.");

            // std::this_thread::sleep_for(wait_duration);
            ros::spinOnce();
            loopRate_1Hz.sleep();
        }

        if (switch_state != is_suspanded)
        {
            switch_state = is_suspanded;
            if (!is_suspanded)
            {
                if (local_print)
                    cout << "Start Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Start Detection.");
            }
            else
            {
                if (local_print)
                    cout << "Stop Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Stop Detection.");
            }
        }

        if (!is_suspanded)
        {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            img = cam_image_copy.clone();
        }

        clock_t start = clock();

        //------------------调用ArUco Marker库对图像进行识别--------------
        // markerids存储每个识别到二维码的编号  markerCorners每个二维码对应的四个角点的像素坐标
        std::vector<int> markerids, markerids_deted;
        vector<vector<Point2f> > markerCorners, markerCorners_deted, rejectedCandidate;

        Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
        cv::aruco::detectMarkers(img, dictionary, markerCorners_deted, markerids_deted, parameters, rejectedCandidate);

        if (markerids_deted.size() > 0)
        {
            for (int tt=0; tt<markerids_deted.size(); tt++) {
                if (19 == markerids_deted[tt]) {
                    markerids.push_back(markerids_deted[tt]);
                    markerCorners.push_back(markerCorners_deted[tt]);
                }
            }
            if (markerids.size() == 0) {
                for (int tt=0; tt<markerids_deted.size(); tt++) {
                    if (43 == markerids_deted[tt]) {
                        markerids.push_back(markerids_deted[tt]);
                        markerCorners.push_back(markerCorners_deted[tt]);
                    }
                }
            }
            if (markerids.size() == 0) {
                for (int tt=0; tt<markerids_deted.size(); tt++) {
                    if (1 == markerids_deted[tt]) {
                        markerids.push_back(markerids_deted[tt]);
                        markerCorners.push_back(markerCorners_deted[tt]);
                    }
                }
            }
            if (markerids.size() == 0) {
                for (int tt=0; tt<markerids_deted.size(); tt++) {
                    if (2 == markerids_deted[tt]) {
                        markerids.push_back(markerids_deted[tt]);
                        markerCorners.push_back(markerCorners_deted[tt]);
                    }
                }
            }
            if (markerids.size() == 0) {
                for (int tt=0; tt<markerids_deted.size(); tt++) {
                    if (3 == markerids_deted[tt]) {
                        markerids.push_back(markerids_deted[tt]);
                        markerCorners.push_back(markerCorners_deted[tt]);
                    }
                }
            }
            if (markerids.size() == 0) {
                for (int tt=0; tt<markerids_deted.size(); tt++) {
                    if (4 == markerids_deted[tt]) {
                        markerids.push_back(markerids_deted[tt]);
                        markerCorners.push_back(markerCorners_deted[tt]);
                    }
                }
            }
        }

        //-------------------多于一个目标被识别到，进入算法-----------------
        if (markerids.size() > 0)
        {
            aruco::drawDetectedMarkers(img, markerCorners, markerids);

            std::vector<float> collected_tx, collected_ty, collected_tz;
            std::vector<float> collected_qx, collected_qy, collected_qz, collected_qw;

            int tt=0;
            vector< Vec3d > rvecs, tvecs;
            if (19 == markerids[tt])
            {
                vector<vector<Point2f> > markerCornersONE;
                markerCornersONE.push_back(markerCorners[tt]);
                aruco::estimatePoseSingleMarkers(markerCornersONE, landpad_det_len*0.666667, camera_matrix, distortion_coefficients, rvecs, tvecs);
                aruco::drawAxis(img, camera_matrix, distortion_coefficients, rvecs[0], tvecs[0],landpad_det_len*0.666667*0.5f);
            }
            else if (43 == markerids[tt])
            {
                vector<vector<Point2f> > markerCornersONE;
                markerCornersONE.push_back(markerCorners[tt]);
                aruco::estimatePoseSingleMarkers(markerCornersONE, landpad_det_len*0.066667, camera_matrix, distortion_coefficients, rvecs, tvecs);
                aruco::drawAxis(img, camera_matrix, distortion_coefficients, rvecs[0], tvecs[0],landpad_det_len*0.066667*0.5f);
            }
            else if (1 == markerids[tt] || 2 == markerids[tt] || 3 == markerids[tt] || 4 == markerids[tt])
            {
                vector<vector<Point2f> > markerCornersONE;
                markerCornersONE.push_back(markerCorners[tt]);
                aruco::estimatePoseSingleMarkers(markerCornersONE, landpad_det_len*0.133334, camera_matrix, distortion_coefficients, rvecs, tvecs);
                aruco::drawAxis(img, camera_matrix, distortion_coefficients, rvecs[0], tvecs[0],landpad_det_len*0.133334*0.5f);
            }

            cv::Mat rotation_matrix;
            cv::Rodrigues(rvecs[0], rotation_matrix);
            Eigen::Matrix3d rotation_matrix_eigen;
            cv::cv2eigen(rotation_matrix, rotation_matrix_eigen);
            Eigen::Quaterniond q = Eigen::Quaterniond(rotation_matrix_eigen);
            q.normalize();

            /*if (43 != markerids[tt] && 19 != markerids[tt]) {
                static tf::TransformBroadcaster br;
                tf::Transform world2camera = tf::Transform(tf::Quaternion(q.x(), q.y(), q.z(), q.w()), tf::Vector3(tvecs[0][0], tvecs[0][1], tvecs[0][2]));
                char obj_str[16];
                sprintf(obj_str, "object-%d", markerids[tt]);
                tf::StampedTransform trans_world2camera = tf::StampedTransform(world2camera, ros::Time(), "camera", obj_str);
                br.sendTransform(trans_world2camera);
            }*/

            std::vector<double> vec_t{tvecs[0][0], tvecs[0][1], tvecs[0][2]};
            cv::Mat vec_t_mat{vec_t};
            vec_t_mat = vec_t_mat;
            vec_t_mat.convertTo(vec_t_mat, CV_32FC1);
            // cout << "vec_t_mat.size():" << vec_t_mat.size() << endl;
            // cout << "vec_t_mat.type():" << vec_t_mat.type() <<endl;
            std::vector<double> id_to8_t(3);

            if (19 == markerids[tt] || 43 == markerids[tt])
            {
                id_to8_t[0] = 0.;
                id_to8_t[1] = 0.;
                id_to8_t[2] = 0.;
            }
            else if (1 == markerids[tt])
            {
                id_to8_t[0] = -(landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[1] = (landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[2] = 0.;
            }
            else if (2 == markerids[tt])
            {
                id_to8_t[0] = -(landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[1] = -(landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[2] = 0.;
            }
            else if (3 == markerids[tt])
            {
                id_to8_t[0] = (landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[1] = -(landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[2] = 0.;
            }
            else if (4 == markerids[tt])
            {
                id_to8_t[0] = (landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[1] = (landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[2] = 0.;
            }

            cv::Mat id_to8_t_mat{id_to8_t};
            id_to8_t_mat.convertTo(id_to8_t_mat, CV_32FC1);

            rotation_matrix.convertTo(rotation_matrix, CV_32FC1);
            // cv::invert(rotation_matrix, rotation_matrix);
            cv::Mat id_8_t = rotation_matrix * id_to8_t_mat + vec_t_mat;
            // cout << id_8_t << endl;

            float o_tx = id_8_t.at<float>(0);
            float o_ty = id_8_t.at<float>(1);
            float o_tz = id_8_t.at<float>(2);

            float o_qx = q.x();
            float o_qy = q.y();
            float o_qz = q.z();
            float o_qw = q.w();

            // 将解算的位置转化成旋转矩阵 并旋转计算无人机相对于目标的位置
            float r11 = rotation_matrix.ptr<float>(0)[0];
            float r12 = rotation_matrix.ptr<float>(0)[1];
            float r13 = rotation_matrix.ptr<float>(0)[2];
            float r21 = rotation_matrix.ptr<float>(1)[0];
            float r22 = rotation_matrix.ptr<float>(1)[1];
            float r23 = rotation_matrix.ptr<float>(1)[2];
            float r31 = rotation_matrix.ptr<float>(2)[0];
            float r32 = rotation_matrix.ptr<float>(2)[1];
            float r33 = rotation_matrix.ptr<float>(2)[2];

            // 计算欧拉角
            float thetaz = atan2(r21, r11) / CV_PI * 180;
            float thetay = atan2(-1 * r31, sqrt(r32*r32 + r33*r33)) / CV_PI * 180;
            float thetax = atan2(r32, r33) / CV_PI * 180;

            // C2W代表 相机坐标系转换到世界坐标系  W2C代表 世界坐标系转换到相机坐标系 Theta为欧拉角
            cv::Point3f Theta_C2W;
            cv::Point3f Theta_W2C;
            cv::Point3f Position_OcInW;

            Theta_C2W.z = thetaz;
            Theta_C2W.y = thetay;
            Theta_C2W.x = thetax;

            Theta_W2C.x = -1 * thetax;
            Theta_W2C.y = -1 * thetay;
            Theta_W2C.z = -1 * thetaz;

            Position_OcInW.x = id_8_t.at<float>(0);
            Position_OcInW.y = id_8_t.at<float>(1);
            Position_OcInW.z = id_8_t.at<float>(2);

            Eigen::Vector3d eulerVec;
            eulerVec(0) = (Theta_C2W.z) / 180 * CV_PI;
            double A1_yaw = eulerVec(0);

            // 将解算后的位置发给控制端
            pose_now.header.stamp = ros::Time::now();
            pose_now.detected = true;
            pose_now.frame = 0;
            pose_now.position[0] = o_tx;
            pose_now.position[1] = o_ty;
            pose_now.position[2] = o_tz;
            pose_now.attitude[0] = thetaz;
            pose_now.attitude[1] = thetay;
            pose_now.attitude[2] = thetax;
            pose_now.attitude_q[0] = o_qx;
            pose_now.attitude_q[1] = o_qy;
            pose_now.attitude_q[2] = o_qz;
            pose_now.attitude_q[3] = o_qw;
            pose_now.sight_angle[0] = atan(o_tx / o_tz);
            pose_now.sight_angle[1] = atan(o_ty / o_tz);
            pose_now.yaw_error = A1_yaw;

            last_x = pose_now.position[0];
            last_y = pose_now.position[1];
            last_z = pose_now.position[2];
            last_az = pose_now.attitude[0];
            last_ay = pose_now.attitude[1];
            last_ax = pose_now.attitude[2];
            last_qx = pose_now.attitude_q[0];
            last_qy = pose_now.attitude_q[1];
            last_qz = pose_now.attitude_q[2];
            last_qw = pose_now.attitude_q[3];
            last_yaw = pose_now.yaw_error;
        }
        else
        {
            pose_now.header.stamp = ros::Time::now();
            pose_now.detected = false;
            pose_now.frame = 0;
            pose_now.position[0] = last_x;
            pose_now.position[1] = last_y;
            pose_now.position[2] = last_z;
            pose_now.attitude[0] = last_az;
            pose_now.attitude[1] = last_ay;
            pose_now.attitude[2] = last_ax;
            pose_now.attitude_q[0] = last_qx;
            pose_now.attitude_q[1] = last_qy;
            pose_now.attitude_q[2] = last_qz;
            pose_now.attitude_q[3] = last_qw;
            pose_now.sight_angle[0] = atan(last_x / last_z);
            pose_now.sight_angle[1] = atan(last_y / last_z);
            pose_now.yaw_error = last_yaw;
        }
        position_pub.publish(pose_now);

        // 计算算法运行时间
        clock_t finish=clock();
        calculation_time=(finish-start)/1000;

        // 打印
        // printf_result();

        // 画出识别到的二维码
        // cv::aruco::drawDetectedMarkers(img, markerCorners, markerids);

        msg_ellipse = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
        landpad_pub.publish(msg_ellipse);
        }

        // cv::imshow("test",img);
        ros::spinOnce();
        cv::waitKey(1);
        loopRate.sleep();
    }
}

void printf_result()
{
    // 固定的浮点显示
    cout.setf(ios::fixed);
    // setprecision(n) 设显示小数精度为n位
    cout<<setprecision(4);
    // 左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Landpad Detection<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    if(pose_now.detected)
    {
        cout << "is_detected: ture" <<endl;
    }else
    {
        cout << "is_detected: false" <<endl;
    }
    cout << "pos_target: [X Y Z] : " << pose_now.position[0]  << " [m] "<< pose_now.position[1] <<" [m] " << pose_now.position[2] << " [m] "<<endl;
    cout << "pos_target: [Yaw] :   " << pose_now.yaw_error/3.1415926 *180    << " [du] "<<endl;
    cout << "calculation_time =    " << time << " [ms] " << endl;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2021, LAAS-CNRS, University of Edinburgh, University of Trento
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include <boost/make_shared.hpp>

namespace crocoddyl {

template <typename Scalar>
ControlParametrizationModelAbstractTpl<Scalar>::ControlParametrizationModelAbstractTpl(const std::size_t nw,
                                                                                const std::size_t nu)
    : nw_(nw), nu_(nu) {}

template <typename Scalar>
ControlParametrizationModelAbstractTpl<Scalar>::~ControlParametrizationModelAbstractTpl() {}

template <typename Scalar>
boost::shared_ptr<ControlParametrizationDataAbstractTpl<Scalar> >
ControlParametrizationModelAbstractTpl<Scalar>::createData() {
  return boost::allocate_shared<ControlParametrizationDataAbstract>(
      Eigen::aligned_allocator<ControlParametrizationDataAbstract>(), this);
}

template <typename Scalar>
bool ControlParametrizationModelAbstractTpl<Scalar>::checkData(
    const boost::shared_ptr<ControlParametrizationDataAbstract>&) {
  return false;
}

template <typename Scalar>
typename MathBaseTpl<Scalar>::MatrixXs ControlParametrizationModelAbstractTpl<Scalar>::multiplyByJacobian_J(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Eigen::Ref<const MatrixXs>& A) const {
  MatrixXs AJ(A.rows(), nu_);
  multiplyByJacobian(data, A, AJ);
  return AJ;
}

template <typename Scalar>
typename MathBaseTpl<Scalar>::MatrixXs ControlParametrizationModelAbstractTpl<Scalar>::multiplyJacobianTransposeBy_J(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Eigen::Ref<const MatrixXs>& A) const {
  MatrixXs JTA(nu_, A.cols());
  multiplyJacobianTransposeBy(data, A, JTA);
  return JTA;
}

template <typename Scalar>
std::size_t ControlParametrizationModelAbstractTpl<Scalar>::get_nw() const {
  return nw_;
}

template <typename Scalar>
std::size_t ControlParametrizationModelAbstractTpl<Scalar>::get_nu() const {
  return nu_;
}

}  // namespace crocoddyl


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

#include <boost/foreach.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/tail_variate.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

template<typename Range>
void check_tail(Range const &rng, char const *expected)
{
    BOOST_FOREACH(int i, rng)
    {
        if(!*expected)
        {
            BOOST_CHECK(false);
            return;
        }
        BOOST_CHECK_EQUAL(i, *expected++);
    }
    BOOST_CHECK(!*expected);
}

///////////////////////////////////////////////////////////////////////////////
// test_right_tail
//
void test_right_tail()
{
    accumulator_set<int, stats<tag::tail_weights<right>, tag::tail_variate<int, tag::covariate1, right> >, int > acc(
        right_tail_cache_size = 4
    );

    acc(010, weight = 2, covariate1 = 3);
    check_tail(tail(acc), "\10");
    check_tail(tail_variate(acc), "\3");
    check_tail(tail_weights(acc), "\2");

    acc(020, weight = 7, covariate1 = 1);
    check_tail(tail(acc), "\20\10");
    check_tail(tail_variate(acc), "\1\3");
    check_tail(tail_weights(acc), "\7\2");

    acc(014, weight = 6, covariate1 = 4);
    check_tail(tail(acc), "\20\14\10");
    check_tail(tail_variate(acc), "\1\4\3");
    check_tail(tail_weights(acc), "\7\6\2");

    acc(030, weight = 4, covariate1 = 5);
    check_tail(tail(acc), "\30\20\14\10");
    check_tail(tail_variate(acc), "\5\1\4\3");
    check_tail(tail_weights(acc), "\4\7\6\2");

    acc(001, weight = 1, covariate1 = 9);
    check_tail(tail(acc), "\30\20\14\10");
    check_tail(tail_variate(acc), "\5\1\4\3");
    check_tail(tail_weights(acc), "\4\7\6\2");

    acc(011, weight = 3, covariate1 = 7);
    check_tail(tail(acc), "\30\20\14\11");
    check_tail(tail_variate(acc), "\5\1\4\7");
    check_tail(tail_weights(acc), "\4\7\6\3");

}

///////////////////////////////////////////////////////////////////////////////
// test_left_tail
//
void test_left_tail()
{
    accumulator_set<int, stats<tag::tail_weights<left>, tag::tail_variate<int, tag::covariate1, left> >, int > acc(
        left_tail_cache_size = 4
    );

    acc(010, weight = 2, covariate1 = 3);
    check_tail(tail(acc), "\10");
    check_tail(tail_variate(acc), "\3");
    check_tail(tail_weights(acc), "\2");

    acc(020, weight = 7, covariate1 = 1);
    check_tail(tail(acc), "\10\20");
    check_tail(tail_variate(acc), "\3\1");
    check_tail(tail_weights(acc), "\2\7");

    acc(014, weight = 6, covariate1 = 4);
    check_tail(tail(acc), "\10\14\20");
    check_tail(tail_variate(acc), "\3\4\1");
    check_tail(tail_weights(acc), "\2\6\7");

    acc(030, weight = 4, covariate1 = 5);
    check_tail(tail(acc), "\10\14\20\30");
    check_tail(tail_variate(acc), "\3\4\1\5");
    check_tail(tail_weights(acc), "\2\6\7\4");

    acc(001, weight = 1, covariate1 = 9);
    check_tail(tail(acc), "\1\10\14\20");
    check_tail(tail_variate(acc), "\x9\3\4\1");
    check_tail(tail_weights(acc), "\1\2\6\7");

    acc(011, weight = 3, covariate1 = 7);
    check_tail(tail(acc), "\1\10\11\14");
    check_tail(tail_variate(acc), "\x9\3\7\4");
    check_tail(tail_weights(acc), "\1\2\3\6");
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("tail test");

    test->add(BOOST_TEST_CASE(&test_right_tail));
    test->add(BOOST_TEST_CASE(&test_left_tail));

    return test;
}


/*!
  \file SiPixel2DTemplateDBObject_PayloadInspector
  \Payload Inspector Plugin for SiPixel2DTemplateDBObject
  \author M. Musich
  \version $Revision: 1.0 $
  \date $Date: 2020/04/16 18:00:00 $
*/

#include "FWCore/MessageLogger/interface/MessageLogger.h"

#include "CondCore/Utilities/interface/PayloadInspectorModule.h"
#include "CondCore/Utilities/interface/PayloadInspector.h"
#include "CondCore/CondDB/interface/Time.h"
#include "CondCore/SiPixelPlugins/interface/SiPixelPayloadInspectorHelper.h"
#include "CondCore/SiPixelPlugins/interface/SiPixelTemplateHelper.h"

#include "CalibTracker/StandaloneTrackerTopology/interface/StandaloneTrackerTopology.h"

// the data format of the condition to be inspected
#include "CondFormats/SiPixelObjects/interface/SiPixel2DTemplateDBObject.h"
#include "CondFormats/SiPixelTransient/interface/SiPixelTemplate2D.h"
#include "DataFormats/SiPixelDetId/interface/PixelSubdetector.h"
#include "DataFormats/DetId/interface/DetId.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"

#include <memory>
#include <map>
#include <sstream>
#include <iostream>
#include <algorithm>
#include <boost/range/adaptor/indexed.hpp>

// include ROOT
#include "TH2.h"
#include "TProfile2D.h"
#include "TH2Poly.h"
#include "TGraph.h"
#include "TH2F.h"
#include "TLegend.h"
#include "TCanvas.h"
#include "TLine.h"
#include "TGraph.h"
#include "TStyle.h"
#include "TLatex.h"
#include "TPave.h"
#include "TPaveStats.h"
#include "TGaxis.h"

namespace {

  //***********************************************
  // Display of Template Titles
  // **********************************************/
  using namespace templateHelper;
  using SiPixel2DTemplateTitles_Display =
      SiPixelTitles_Display<SiPixel2DTemplateDBObject, SiPixelTemplateStore2D, SiPixelTemplate2D>;

  //***********************************************
  // Display of 2DTemplate Header
  // **********************************************/
  using SiPixel2DTemplateHeaderTable =
      SiPixelHeaderTable<SiPixel2DTemplateDBObject, SiPixelTemplateStore2D, SiPixelTemplate2D>;

  //***********************************************
  // TH2Poly Map of IDs
  //***********************************************/
  using SiPixel2DTemplateIDsBPixMap = SiPixelIDs<SiPixel2DTemplateDBObject, SiPixelPI::t_barrel>;
  using SiPixel2DTemplateIDsFPixMap = SiPixelIDs<SiPixel2DTemplateDBObject, SiPixelPI::t_forward>;

  //************************************************
  // Full Pixel Tracker Map of Template IDs
  // ***********************************************/
  using SiPixel2DTemplateIDsFullPixelMap =
      SiPixelFullPixelIDMap<SiPixel2DTemplateDBObject, SiPixelTemplateStore2D, SiPixelTemplate2D>;

}  // namespace

// Register the classes as boost python plugin
PAYLOAD_INSPECTOR_MODULE(SiPixel2DTemplateDBObject) {
  PAYLOAD_INSPECTOR_CLASS(SiPixel2DTemplateTitles_Display);
  PAYLOAD_INSPECTOR_CLASS(SiPixel2DTemplateHeaderTable);
  PAYLOAD_INSPECTOR_CLASS(SiPixel2DTemplateIDsBPixMap);
  PAYLOAD_INSPECTOR_CLASS(SiPixel2DTemplateIDsFPixMap);
  PAYLOAD_INSPECTOR_CLASS(SiPixel2DTemplateIDsFullPixelMap);
}


#pragma once

#include <crab/common/types.hpp>
#include <crab/common/debug.hpp>
#include <crab/common/stats.hpp>

#include <crab/cfg/cfg.hpp>
#include <crab/iterators/killgen_fixpoint_iterator.hpp>
#include <crab/domains/separate_domains.hpp>
#include <crab/analysis/graphs/cdg.hpp>

#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
#include <boost/range/iterator_range.hpp>

/**
 *  Dataflow analysis that for each block b it computes facts <i,V>
 *  meaning that there exists a path emanating from b that will reach
 *  assertion with id=i and its evaluation depends on the set of
 *  variables V.
 *
 *  These dataflow facts are useful at least for:
 *
 *  - Computing the set of unjustified assumptions when proving a
 *    particular assertion, and
 *
 *  - Performing slicing by havoc'ing statements that cannot affect
 *    any assertion.
 *
 *  TODO:
 *  - Consider only integer instructions ignoring the heap.
 **/

namespace crab {

  namespace analyzer {

    using namespace crab::iterators;
    using namespace crab::domains;
    using namespace crab::cfg;

    // A wrapper to an assert statement
    template<typename CFG>
    struct assert_wrapper: public ikos::writeable {

      typedef typename statement_visitor<typename CFG::number_t,
					 typename CFG::varname_t>::assert_t assert_t;
      typedef typename CFG::basic_block_label_t basic_block_label_t;
      typedef assert_wrapper<CFG> this_type;

      // unique identifier for the assert statement needed for being
      // used as key
      index_t id;
      // basic block where the assert statement is located
      basic_block_label_t bbl;
      // the assert statement
      assert_t* a;

      /// pointers to some global datastructures
      typedef boost::unordered_map<assert_t*, this_type > assert_map_t;
      typedef boost::unordered_map <basic_block_label_t,
				    std::vector<basic_block_label_t> > cdg_t;
      // map assertions to their wrappers
      assert_map_t* assert_map_ptr;
      // control-dependency graph
      const cdg_t* cdg_ptr;

      assert_wrapper (index_t _id, basic_block_label_t _bbl, assert_t* _a,
		      assert_map_t *am, const cdg_t *cdg)
	: id(_id), bbl(_bbl), a(_a), assert_map_ptr(am), cdg_ptr(cdg) {}

      assert_t* get() { return a;}
      const assert_t* get() const { return a;}
      index_t index() const { return id;}
      bool operator==(this_type o) const { return id == o.id;}
      bool operator< (this_type o) const { return id < o.id; }
      void write (crab::crab_os& o) { o << "\"" << *a << "\""; }
    };

    template<typename CFG> class assertion_crawler;

    // Define the operations of the dataflow analysis
    template<class CFG>
    class assertion_crawler_operations:
      public killgen_operations_api<CFG,
	     separate_killgen_domain<assert_wrapper<CFG>,
				     flat_killgen_domain<typename CFG::variable_t> > > {

      friend class assertion_crawler<CFG>;

    public:

       typedef assert_wrapper<CFG> assert_wrapper_t;
       typedef flat_killgen_domain<typename CFG::variable_t> var_dom_t;
       // -- key type: map an assertion to a set of variables
       typedef separate_killgen_domain<assert_wrapper_t, var_dom_t> separate_domain_t;

    private:

       typedef killgen_operations_api<CFG, separate_domain_t> killgen_operations_api_t;
       typedef typename CFG::basic_block_label_t basic_block_label_t;
       typedef typename CFG::basic_block_t basic_block_t;
       typedef typename CFG::varname_t V;
       typedef typename CFG::number_t N;

       // map each stmt assertion to a unique identifier
       typedef typename assert_wrapper_t::assert_t assert_t;
       typedef boost::unordered_map<assert_t*, assert_wrapper_t > assert_map_t;

       // control-dependency graph
       // map a CFG block to the set of blocks which control-dependent on it.
       typedef boost::unordered_map <basic_block_label_t,
				     std::vector<basic_block_label_t> > cdg_t;

       // set of uses and definitions of an instruction
       typedef crab::cfg::Live<N,V> live_t;

       class transfer_function: public statement_visitor<N,V> {

         typedef typename statement_visitor<N,V>::bin_op_t  bin_op_t;
         typedef typename statement_visitor<N,V>::assign_t  assign_t;
         typedef typename statement_visitor<N,V>::assume_t  assume_t;
         typedef typename statement_visitor<N,V>::select_t  select_t;
         typedef typename statement_visitor<N,V>::assert_t  assert_t;
         typedef typename statement_visitor<N,V>::int_cast_t int_cast_t;
         typedef typename statement_visitor<N,V>::havoc_t   havoc_t;
         typedef typename statement_visitor<N,V>::unreach_t unreach_t;
	 typedef typename statement_visitor<N,V>::bool_bin_op_t bool_bin_op_t;
	 typedef typename statement_visitor<N,V>::bool_assign_cst_t bool_assign_cst_t;
	 typedef typename statement_visitor<N,V>::bool_assign_var_t bool_assign_var_t;
	 typedef typename CFG::variable_t variable_t;

	 // Helper that applies function F to each pair's value of the
	 // separate domain_t.
       	 template<typename F>
       	 struct apply_separate:
	   public std::unary_function<separate_domain_t, separate_domain_t>{
	   typedef apply_separate<F> this_type;
	   typedef std::binary_function<assert_wrapper_t, var_dom_t,
					std::pair<var_dom_t, bool> > function_type;
	   static_assert (std::is_base_of<function_type, F>::value,
			  "Function must be subclass of type F");
       	   F f;

	 public:

       	   apply_separate(F _f): f (_f) {}
	   apply_separate(const this_type& o): f (o.f) {}

       	   separate_domain_t operator()(separate_domain_t inv)
       	   { // XXX: separate_domain_t cannot be modified in-place
	     typedef std::pair<typename separate_domain_t::key_type,
			       typename separate_domain_t::value_type> value_type;

	     if (inv.is_bottom ()) return inv;

       	     std::vector<value_type> kvs;
       	     kvs.reserve (std::distance (inv.begin(), inv.end ()));
       	     for (auto const &kv: inv)
       	       kvs.push_back (value_type(kv.first, kv.second));

       	     for (auto &kv: kvs) {
       	       auto p = f(kv.first, kv.second);
       	       if (p.second) inv.set (kv.first, p.first);
       	     }
       	     return inv;
       	   }
       	 };

	 /** Add data-dependencies **/
       	 class add_data_deps:
       	   public std::binary_function<assert_wrapper_t, var_dom_t,
				       std::pair<var_dom_t,bool> > {
	   var_dom_t uses;
	   var_dom_t defs;

	 public:

       	   add_data_deps(const live_t& l)
	     : uses(var_dom_t::bottom()), defs (var_dom_t::bottom()) {
	     for (auto v: boost::make_iterator_range(l.uses_begin(), l.uses_end()))
	       uses += v;
	     for (auto v: boost::make_iterator_range(l.defs_begin(), l.defs_end()))
	       defs += v;
	   }

	   add_data_deps(const add_data_deps &o): uses (o.uses), defs (o.defs) {}

       	   std::pair<var_dom_t,bool> operator()(assert_wrapper_t /*w*/,var_dom_t d) {
       	     bool change = false;

	     if (defs.is_bottom() && !uses.is_bottom() && !(uses & d).is_bottom ()) {
	       d += uses;
	       change = true;
	     }
       	     if (!(d & defs).is_bottom()) {
       	       d -= defs;
	       d += uses;
       	       change = true;
       	     }
       	     return std::make_pair(d, change);
       	   }
       	 };

	 /** Add control-dependencies **/
       	 class add_control_deps:
       	   public std::binary_function<assert_wrapper_t, var_dom_t,
				       std::pair<var_dom_t,bool> > {
	   const cdg_t &cdg;
	   const std::vector<basic_block_label_t> &roots;

	   var_dom_t uses;

	   // return true if we find a path in cdg from root to target
	   // FIXME: do caching for the queries
	   bool reach (basic_block_label_t root, basic_block_label_t target,
		       boost::unordered_set<basic_block_label_t> &visited) {
	     if (root == target)
	       return true;

	     // break cycles
	     if (visited.find (root) != visited.end ())
	       return false;

	     visited.insert (root);
	     auto it = cdg.find (root);
	     if (it == cdg.end ()) return false;

	     for (auto child: it->second) {
	       if (reach (child, target, visited))
		 return true;
	     }
	     return false;
	   }

	   bool reach (basic_block_label_t target) {
	     boost::unordered_set <basic_block_label_t> visited;
	     for (auto r: roots)
	       if (reach(r, target,visited))
		 return true;
	     return false;
	   }

	 public:

       	   add_control_deps (const cdg_t & _cdg,
			     const std::vector<basic_block_label_t>& _roots,
			     const live_t& l)
	     : cdg(_cdg), roots (_roots), uses(var_dom_t::bottom()) {
	     for (auto v: boost::make_iterator_range(l.uses_begin(), l.uses_end()))
	       uses += v;
	   }

	   add_control_deps (const add_data_deps &o)
	     : cdg (o.cdg), roots (o.roots), uses (o.uses) {}

       	   std::pair<var_dom_t,bool> operator()(assert_wrapper_t w, var_dom_t d) {
       	     bool change = false;
	     if (reach (w.bbl)) {
	       d += uses;
	       change = true;
	     }

       	     return std::make_pair(d, change);
       	   }
       	 };

	 /** Remove data-dependencies **/
       	 class remove_deps:
       	   public std::binary_function<assert_wrapper_t, var_dom_t,
				       std::pair<var_dom_t,bool> > {
	   var_dom_t vars;

	 public:

       	   remove_deps(variable_t v): vars(var_dom_t::bottom()) { vars += v; }

	   remove_deps(const std::vector<variable_t> &vs)
	     : vars(var_dom_t::bottom()) { for (auto v: vs) {vars += v; } }

	   remove_deps(const remove_deps& o): vars (o.vars) {}

       	   std::pair<var_dom_t,bool> operator()(assert_wrapper_t /*w*/, var_dom_t d) {
       	     bool change = false;
       	     if (!(d & vars).is_bottom()) {
       	       d -= vars;
       	       change = true;
       	     }
       	     return std::make_pair(d, change);
       	   }
       	 };

	 typedef apply_separate<add_data_deps> apply_add_data_t;
	 typedef apply_separate<add_control_deps> apply_add_control_t;
	 typedef apply_separate<remove_deps> apply_remove_t;

	 // dataflow solution: map blocks to pairs of assertion id and
	 //                    set of variables.
         separate_domain_t _inv;
	 // map each assertion to a unique identifier
	 assert_map_t &_assert_map;
	 // control-dependence graph
	 const cdg_t &_cdg;
	 // parent basic block (XXX: needed because crab statements do
	 // not have a back pointer to their basic blocks)
	 basic_block_t &_bb;

       public:

         transfer_function (separate_domain_t inv, const cdg_t &g,
			    assert_map_t &am, basic_block_t &bb)
	   : _inv(inv), _assert_map (am), _cdg (g), _bb (bb) { }

	 separate_domain_t inv () {return _inv;}

         void visit(bin_op_t &s){
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "*** " << s << "\n"
	 	                  << "\tBEFORE: " << _inv << "\n");
       	   apply_add_data_t f(add_data_deps(s.get_live ()));
       	   _inv = f(_inv);
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "\tAFTER " << _inv << "\n";);
       	 }

         void visit(assign_t &s) {
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "*** " << s << "\n"
	 	                  << "\tBEFORE: " << _inv << "\n");
       	   apply_add_data_t f(add_data_deps (s.get_live ()));
       	   _inv = f(_inv);
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "\tAFTER " << _inv << "\n";);

         }

         void visit(assume_t &s) {
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "*** " << s << "\n"
	 	                  << "\tBEFORE: " << _inv << "\n");
	   // -- add data dependencies
       	   apply_add_data_t df (add_data_deps (s.get_live ()));
       	   _inv = df(_inv);
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "\tAFTER data-dep " << _inv << "\n";);

	   // -- add control dependencies
	   for (auto const &pred: boost::make_iterator_range(_bb.prev_blocks ())) {
	     // it->second is the set of basic blocks that control
	     // dependent on s' block
	     auto it = _cdg.find (/*_bb*/ pred);
	     if (it != _cdg.end ()) {
	       auto const &children =  it->second;
	       CRAB_LOG("assertion-crawler-step-control",
			crab::outs () << "{";
			for(auto &c: children) { crab::outs () << c << ";"; }
			crab::outs () << "} control-dependent on " << pred << "\n";);
	       apply_add_control_t cf(add_control_deps(_cdg, children, s.get_live()));
	       _inv = cf(_inv);
	       CRAB_LOG("assertion-crawler-step-control",
			crab::outs () << "\tAFTER control-dep " << _inv << "\n";);
	     }
	   }
         }

         void visit(select_t &s) {
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "*** " << s << "\n"
	 	                  << "\tBEFORE: " << _inv << "\n");
       	   apply_add_data_t f(add_data_deps (s.get_live ()));
       	   _inv = f(_inv);
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "\tAFTER " << _inv << "\n";);
         }

         void visit(assert_t &s) {
	   auto it = _assert_map.find (&s);
	   if (it != _assert_map.end ()) return;

	   var_dom_t vdom = var_dom_t::bottom ();
	   auto const& l = s.get_live ();
	   for (auto v: boost::make_iterator_range(l.uses_begin(), l.uses_end())) {
	     vdom += v;
	   }

	   unsigned id = _assert_map.size ();
	   assert_wrapper_t val(id, _bb.label(), &s, &_assert_map, &_cdg);
	   _assert_map.insert(typename assert_map_t::value_type (&s, val));
	   _inv.set (val, vdom);
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "*** " << s << "\n" << "\tAdded " << vdom << "\n";);

       	 }

         void visit(unreach_t&) {
	   _inv = separate_domain_t::bottom();
	 }

         void visit(havoc_t& s) {
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "*** " << s << "\n"
	 	                  << "\tBEFORE: " << _inv << "\n");
       	   apply_remove_t f(remove_deps (s.variable ()));
       	   _inv = f(_inv);
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "\tAFTER " << _inv << "\n";);
	 }

         void visit(int_cast_t &s){
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "*** " << s << "\n"
	 	                  << "\tBEFORE: " << _inv << "\n");
       	   apply_add_data_t f(add_data_deps (s.get_live ()));
       	   _inv = f(_inv);
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "\tAFTER " << _inv << "\n";);
       	 }

         void visit(bool_bin_op_t &s){
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "*** " << s << "\n"
	 	                  << "\tBEFORE: " << _inv << "\n");
       	   apply_add_data_t f(add_data_deps(s.get_live ()));
       	   _inv = f(_inv);
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "\tAFTER " << _inv << "\n";);
       	 }

         void visit(bool_assign_var_t &s){
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "*** " << s << "\n"
	 	                  << "\tBEFORE: " << _inv << "\n");
       	   apply_add_data_t f(add_data_deps(s.get_live ()));
       	   _inv = f(_inv);
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "\tAFTER " << _inv << "\n";);
       	 }

         void visit(bool_assign_cst_t &s){
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "*** " << s << "\n"
	 	                  << "\tBEFORE: " << _inv << "\n");
       	   apply_add_data_t f(add_data_deps(s.get_live ()));
       	   _inv = f(_inv);
	   CRAB_LOG("assertion-crawler-step",
		    crab::outs () << "\tAFTER " << _inv << "\n";);
       	 }

       };

      private:

       // global datastructure for the whole analysis of the CFG
       assert_map_t _assert_map;
       cdg_t _cdg;

      public:

       assertion_crawler_operations(CFG cfg) : killgen_operations_api_t(cfg) { }

       virtual bool is_forward() override { return false;}

       virtual std::string name() override { return "assertion-crawler";}

       virtual void init_fixpoint() override {
	 crab::ScopedCrabStats __st__("Control-Dependency Graph");
	 crab::analyzer::graph_algo::control_dep_graph (this->_cfg, _cdg);
       }

       virtual separate_domain_t entry() override {
	 return separate_domain_t::bottom();
       }

       virtual separate_domain_t merge(separate_domain_t d1, separate_domain_t d2) override {
	 return d1 | d2;
       }

       virtual separate_domain_t
       analyze(basic_block_label_t bb_id, separate_domain_t out) override {
         auto &bb = this->_cfg.get_node(bb_id);
         transfer_function vis(out,_cdg, _assert_map, bb);
	 for (auto &s: boost::make_iterator_range(bb.rbegin(),bb.rend())) {
	   s.accept (&vis);
	 }
         return vis.inv();
       }
     };

     /**
      * The assertion crawler dataflow analysis
      *
      * Compute for each basic block b a set of facts (i,V) such that
      * there exists a path from b that will check assertion i and its
      * evaluation depends on the set of variables V.
      **/
     template<class CFG>
     class assertion_crawler:
       public boost::noncopyable,
       public crab::iterators::
       killgen_fixpoint_iterator<CFG,assertion_crawler_operations<CFG> >{

      public:

       typedef typename CFG::basic_block_label_t basic_block_label_t;
       typedef typename CFG::varname_t varname_t;

      private:

       typedef crab::iterators::
       killgen_fixpoint_iterator<CFG,
				 assertion_crawler_operations<CFG> > fixpo_t;

      public:

       // map assertions to a set of variables
       typedef typename
       assertion_crawler_operations<CFG>::separate_domain_t separate_domain_t;

      private:

       boost::unordered_map<basic_block_label_t, separate_domain_t> m_map;

      public:

       assertion_crawler(CFG cfg): fixpo_t(cfg) {}

       void exec() {
         this->run();
	 for (auto p : boost::make_iterator_range(this->out_begin(), this->out_end())) {
	   m_map.insert(std::make_pair(p.first, p.second));
	 }
         this->release_memory();
       }

       // return the dataflow facts that hold at the exit of block bb
       separate_domain_t& get_assertions(basic_block_label_t bb) {
	 auto it = m_map.find(bb);
	 if (it == m_map.end())
	   CRAB_ERROR("Basic block ", bb, " not found");
	 return it->second;
       }

       // return the dataflow facts of the pre-state at each program point in bb
       void get_assertions(basic_block_label_t b,
			   std::map<typename CFG::statement_t*, separate_domain_t> &res) {
	 auto it = m_map.find(b);
	 if (it != m_map.end()) {
	   if (!it->second.is_bottom()) {
	     auto kv = *(it->second.begin());
	     auto &bb = this->_cfg.get_node(b);
	     typename assertion_crawler_operations<CFG>::transfer_function
	       vis(it->second,
		   *(kv.first.cdg_ptr), *(kv.first.assert_map_ptr),
		   bb);
	     for (auto &s: boost::make_iterator_range(bb.rbegin(),bb.rend())) {
	       // -- post-state
	       // auto out = vis.inv ();
	       // res.insert(std::make_pair(&s,out));
	       // s.accept (&vis);
	       // -- pre-state
	       s.accept (&vis);
	       auto in = vis.inv ();
	       res.insert(std::make_pair(&s,in));
	     }
	   }
	 }
       }

       void write (crab_os &o) {
	 o << "Assertion Crawler Analysis \n";
	 for (auto &kv: m_map) {
	   o << "Block " << kv.first << "\n";
	   #if 1
	   o << "\t" << kv.second << "\n";
	   #else
	   std::map<typename CFG::statement_t*, separate_domain_t> pp_map;
	   get_assertions(kv.first, pp_map);
	   for (auto &kv: pp_map) {
	     o << "\t" << *(kv.first) << " --> " << kv.second << "\n";
	   }
	   #endif
	 }
       }

     };

   template <typename CFG>
   crab_os& operator<<(crab_os& o, assertion_crawler<CFG> &ac) {
     ac.write (o);
     return o;
   }

  } // end namespace
} // end namespace