// File: geometry.hpp
// Project: core
// Created Date: 14/04/2021
// Author: Shun Suzuki
// -----
// Last Modified: 22/09/2021
// Modified By: Shun Suzuki (suzuki@hapis.k.u-tokyo.ac.jp)
// -----
// Copyright (c) 2021 Hapis Lab. All rights reserved.
//

#pragma once

#if _MSC_VER
#pragma warning(push)
#pragma warning(disable : 6031 6255 6294 26450 26451 26454 26495 26812)
#endif
#if defined(__GNUC__) && !defined(__llvm__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif
#include <Eigen/Dense>
#if _MSC_VER
#pragma warning(pop)
#endif
#if defined(__GNUC__) && !defined(__llvm__)
#pragma GCC diagnostic pop
#endif

#include <map>
#include <memory>
#include <utility>
#include <vector>

#include "hardware_defined.hpp"

namespace autd {
namespace core {

class Geometry;
using GeometryPtr = std::unique_ptr<Geometry>;

using Vector3 = Eigen::Matrix<double, 3, 1>;
using Vector4 = Eigen::Matrix<double, 4, 1>;
using Matrix4X4 = Eigen::Matrix<double, 4, 4>;
using Quaternion = Eigen::Quaternion<double>;

/**
 * \brief Device contains an AUTD device geometry.
 */
struct Device {
  Device(const Vector3& position, const Quaternion& quaternion)
      : x_direction(quaternion * Vector3(1, 0, 0)),
        y_direction(quaternion * Vector3(0, 1, 0)),
        z_direction(quaternion * Vector3(0, 0, 1)),
        global_trans_positions(std::make_unique<Vector3[]>(NUM_TRANS_IN_UNIT)) {
    const Eigen::Transform<double, 3, Eigen::Affine> transform_matrix = Eigen::Translation<double, 3>(position) * quaternion;
    auto index = 0;
    for (size_t y = 0; y < NUM_TRANS_Y; y++)
      for (size_t x = 0; x < NUM_TRANS_X; x++) {
        if (is_missing_transducer(x, y)) continue;
        const Vector4 local_pos = Vector4(static_cast<double>(x) * TRANS_SPACING_MM, static_cast<double>(y) * TRANS_SPACING_MM, 0, 1);
        const Vector4 global_pos = transform_matrix * local_pos;
        global_trans_positions[index++] = Vector3(global_pos[0], global_pos[1], global_pos[2]);
      }
    g2l = transform_matrix.inverse();
  }

  Device(const Vector3& position, const Vector3& euler_angles)
      : Device(position, Eigen::AngleAxis<double>(euler_angles.x(), Vector3::UnitZ()) * Eigen::AngleAxis<double>(euler_angles.y(), Vector3::UnitY()) *
                             Eigen::AngleAxis<double>(euler_angles.z(), Vector3::UnitZ())) {}

  Vector3 x_direction;
  Vector3 y_direction;
  Vector3 z_direction;
  std::unique_ptr<Vector3[]> global_trans_positions;
  Eigen::Transform<double, 3, Eigen::Affine> g2l;
};

/**
 * @brief Geometry of all devices
 */
class Geometry {
 public:
  Geometry() : _wavelength(8.5), _attenuation(0) {}
  ~Geometry() = default;
  Geometry(const Geometry& v) noexcept = default;
  Geometry& operator=(const Geometry& obj) = default;
  Geometry(Geometry&& obj) = default;
  Geometry& operator=(Geometry&& obj) = default;

  /**
   * @brief  Add new device with position and rotation. Note that the transform is done with order: Translate -> Rotate
   * @param position Position of transducer #0, which is the one at the lower-left corner.
   * (The lower-left corner is the one with the two missing transducers.)
   * @param euler_angles ZYZ convention euler angle of the device
   * @param group Grouping ID of the device
   * @return an id of added device
   */
  size_t add_device(const Vector3& position, const Vector3& euler_angles, const size_t group = 0) {
    const auto device_id = this->_devices.size();
    this->_devices.emplace_back(position, euler_angles);
    this->_group_map[device_id] = group;
    return device_id;
  }

  /**
   * @brief Same as add_device(const Vector3&, const Vector3&, const size_t), but using quaternion rather than zyz euler angles.
   * @param position Position of transducer #0, which is the one at the lower-left corner.
   * @param quaternion rotation quaternion of the device.
   * @param group Grouping ID
   * @return an id of added device
   */
  size_t add_device(const Vector3& position, const Quaternion& quaternion, const size_t group = 0) {
    const auto device_id = this->_devices.size();
    this->_devices.emplace_back(position, quaternion);
    this->_group_map[device_id] = group;
    return device_id;
  }

  /**
   * @brief Delete device
   * @param idx Index of the device to delete
   * @return an index of deleted device
   */
  size_t del_device(const size_t idx) {
    this->_devices.erase(this->_devices.begin() + idx);
    return idx;
  }

  /**
   * @brief Clear all devices
   */
  void clear_devices() { std::vector<Device>().swap(this->_devices); }

  /**
   * @brief ultrasound wavelength
   */
  double& wavelength() noexcept { return this->_wavelength; }

  /**
   * @brief attenuation coefficient
   */
  double& attenuation_coefficient() noexcept { return this->_attenuation; }

  /**
   * @brief Number of devices
   */
  [[nodiscard]] size_t num_devices() const noexcept { return this->_devices.size(); }

  /**
   * @brief Number of transducers
   */
  [[nodiscard]] size_t num_transducers() const noexcept { return this->num_devices() * NUM_TRANS_IN_UNIT; }

  /**
   * @brief Convert device ID into group ID
   */
  [[nodiscard]] size_t group_id_for_device_idx(const size_t device_idx) const { return this->_group_map.at(device_idx); }

  /**
   * @brief Position of a transducer specified by id
   */
  [[nodiscard]] const Vector3& position(const size_t global_transducer_idx) const {
    const auto local_trans_id = global_transducer_idx % NUM_TRANS_IN_UNIT;
    return position(device_idx_for_trans_idx(global_transducer_idx), local_trans_id);
  }

  /**
   * @brief Position of a transducer specified by id
   */
  [[nodiscard]] const Vector3& position(const size_t device_idx, const size_t local_transducer_idx) const {
    return this->_devices[device_idx].global_trans_positions[local_transducer_idx];
  }

  /**
   * @brief Convert a global position to a local position
   */
  [[nodiscard]] Vector3 to_local_position(const size_t device_idx, const Vector3& global_position) const {
    const Vector4 homo = Vector4(global_position[0], global_position[1], global_position[2], 1);
    const Vector4 local_position = this->_devices[device_idx].g2l * homo;
    return Vector3(local_position[0], local_position[1], local_position[2]);
  }

  /**
   * @brief Normalized direction of a device
   */
  [[nodiscard]] const Vector3& direction(const size_t device_idx) const { return this->_devices[device_idx].z_direction; }

  /**
   * @brief Normalized long-axis direction of a device
   */
  [[nodiscard]] const Vector3& x_direction(const size_t device_idx) const { return this->_devices[device_idx].x_direction; }

  /**
   * @brief Normalized short-axis direction of a device
   */
  [[nodiscard]] const Vector3& y_direction(const size_t device_idx) const { return this->_devices[device_idx].y_direction; }

  /**
   * @brief Same as the direction()
   */
  [[nodiscard]] const Vector3& z_direction(const size_t device_idx) const { return this->_devices[device_idx].z_direction; }

  /**
   * @brief Convert transducer index into device index
   */
  [[nodiscard]] static size_t device_idx_for_trans_idx(const size_t transducer_idx) { return transducer_idx / NUM_TRANS_IN_UNIT; }

 private:
  std::vector<Device> _devices;
  std::map<size_t, size_t> _group_map;
  double _wavelength;
  double _attenuation;
};
}  // namespace core
}  // namespace autd


#include "SRKSpinTracker.h"
#include <iostream>
#include <algorithm>
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/numeric/odeint.hpp>

#include "SRKODEState.h"

using namespace boost::numeric::odeint;
using namespace std;

//typedef runge_kutta_cash_karp54< SRKMotionState > error_stepper_type;
typedef runge_kutta_dopri5<SRKODEState, SRKSpinFloat> error_stepper_type;

typedef controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

//#define SRKSPINTRACKERDEBUG 1

SRKSpinTracker::SRKSpinTracker()
{
	eps_abs = 1.e-7;
	eps_rel = 1.e-7;
	initialStepSize = 0.01;
	stepsTaken = 0;
	constStepper = false;
}
SRKSpinTracker::SRKSpinTracker(SRKGlobalField* theField):
	SRKSpinTracker()
{
	theEquationOfMotion.setGlobalField(theField);
}

SRKSpinTracker::~SRKSpinTracker()
{

}

void SRKSpinTracker::trackSpin(SRKODEState& theState, double timeToTrack, std::vector<SRKODEState>* stepRecord, std::vector<double>* stepTimes)
{
	runge_kutta4<SRKODEState, SRKSpinFloat> stepper;
	controlled_stepper_type controlled_stepper(default_error_checker<SRKSpinFloat, range_algebra, default_operations>(SRKSpinFloat(eps_abs), SRKSpinFloat(eps_rel), SRKSpinFloat(1.0), SRKSpinFloat(1.0)));

#ifdef SRKSPINTRACKERDEBUG
	cout <<"PRESPIN TRACKING";
	printMotionState(theState);
#endif

	if(stepRecord != nullptr)
	{
		if(constStepper)
		{
			integrate_const(stepper, theEquationOfMotion, theState, SRKSpinFloat(0.0), SRKSpinFloat(timeToTrack), SRKSpinFloat(initialStepSize), pushBackStateAndTime(stepRecord, stepTimes));
		}
		else
		{
			integrate_adaptive(controlled_stepper, theEquationOfMotion, theState, theState[8], theState[8] + SRKSpinFloat(timeToTrack), SRKSpinFloat(initialStepSize), pushBackStateAndTime(stepRecord, stepTimes));
		}

	}
	else
	{
		if(constStepper)
		{
			integrate_const(stepper, theEquationOfMotion, theState, SRKSpinFloat(0.0), SRKSpinFloat(timeToTrack), SRKSpinFloat(initialStepSize));
		}
		else
		{
			integrate_adaptive(controlled_stepper, theEquationOfMotion, theState, theState[8], theState[8] + SRKSpinFloat(timeToTrack), SRKSpinFloat(initialStepSize));
		}
	}
	theState[8] += timeToTrack;

#ifdef SRKSPINTRACKERDEBUG
	cout <<"PostSpin TRACKING";
	printMotionState(theState);
#endif

}

void SRKSpinTracker::trackSpinAltA(SRKODEState& theState, double timeToTrack, std::vector<SRKODEState>* stepRecord, std::vector<double>* stepTimes)
{
	SRKSpinFloat timeToTrackConv(timeToTrack);
	error_stepper_type theStepper;
	SRKSpinFloat dt = SRKSpinFloat(initialStepSize);
	SRKSpinFloat t0 = theState[8];  //Initial time simulation started at
	SRKSpinFloat deltaPhi;
	SRKSpinFloat val;
	SRKODEState previousState(9);
	SRKODEState stateError(9);
	bool potentialLastStep = false;
	for (;;)
	{

		previousState = theState;
		theStepper.do_step(theEquationOfMotion, theState, theState[8], dt, stateError);
		deltaPhi = theState[6] - previousState[6];
		if(deltaPhi > SRKSpinFloat(.785)) //if it rotates greater than pi slow it down!
		{
			dt = dt * SRKSpinFloat(.5) / deltaPhi;
			theState = previousState;
			potentialLastStep = false;
		}
		else
		{
			val = abs(stateError[6]);
//			val/=(eps_abs+eps_rel*abs(deltaPhi));
			val /= (SRKSpinFloat(eps_rel) * abs(deltaPhi));
			if(val > SRKSpinFloat(1.)) //too much error, slow down and repeat
			{
//				SRKSpinFloat a=(SRKSpinFloat(0.9)*SRKSpinFloat(pow(val,SRKSpinFloat(-1.)/(SRKSpinFloat(theStepper.error_order_value) - SRKSpinFloat(1.)))));
				SRKSpinFloat a = SRKSpinFloat(0);
				SRKSpinFloat b = SRKSpinFloat(0.2);
				dt = dt * max(a, b);
				theState = previousState;
				potentialLastStep = false;
			}
			else if(val < SRKSpinFloat(0.5))
			{
				theState[8] += dt;
				stepsTaken++;
				if(stepRecord != nullptr && stepTimes != nullptr)
				{
					stepRecord->push_back(theState);
					stepTimes->push_back(static_cast<double>(theState[8]));
				}
				if(potentialLastStep) //If potential last step worked we break!
				{
					break;
				}

//				SRKSpinFloat a=SRKSpinFloat(0.9)*SRKSpinFloat(pow(val,SRKSpinFloat(-1.)/SRKSpinFloat(theStepper.order_value )));
				SRKSpinFloat a = SRKSpinFloat(0);
				SRKSpinFloat b = SRKSpinFloat(5);
				dt = dt * max(a, b);

			}
			else
			{
				theState[8] += dt;
				stepsTaken++;
				if(stepRecord != nullptr && stepTimes != nullptr)
				{
					stepRecord->push_back(theState);
					stepTimes->push_back(static_cast<double>(theState[8]));
				}
				if(potentialLastStep) //If potential last step worked we break!
				{
					break;
				}
			}

		}

		if(theState[8] + dt > t0 + timeToTrackConv)
		{
			dt = t0 + timeToTrackConv - theState[8];
			potentialLastStep = true;
		}

	}

}


/*
 * This file is a part of the TChecker project.
 *
 * See files AUTHORS and LICENSE for copyright details.
 *
 */

#include <boost/dynamic_bitset.hpp>

#include "tchecker/dbm/refdbm.hh"
#include "tchecker/refzg/semantics.hh"

namespace tchecker {

namespace refzg {

/* Semantics functions */

/*!
  \brief Compute initial zone with reference clocks w.r.t. standard semantics
  \note see tchecker::refzg::standard_semantics_t::initial
*/
tchecker::state_status_t standard_initial(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
                                          boost::dynamic_bitset<> const & delay_allowed,
                                          tchecker::clock_constraint_container_t const & invariant, tchecker::integer_t spread)
{
  tchecker::refdbm::zero(rdbm, r);

  if (tchecker::refdbm::bound_spread(rdbm, r, spread) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SPREAD;

  if (tchecker::refdbm::constrain(rdbm, r, invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;

  return tchecker::STATE_OK;
}

/*!
  \brief Compute next zone with reference clocks w.r.t. standard semantics
  \note see tchecker::refzg::standard_semantics_t::next
*/
tchecker::state_status_t
standard_next(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
              boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
              boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
              tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
              tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  if (src_delay_allowed.any()) {
    tchecker::refdbm::asynchronous_open_up(rdbm, r, src_delay_allowed);

    if (tchecker::refdbm::constrain(rdbm, r, src_invariant) == tchecker::dbm::EMPTY)
      return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED; // should never occur
  }

  if (tchecker::refdbm::bound_spread(rdbm, r, spread) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SPREAD;

  if (tchecker::refdbm::synchronize(rdbm, r, sync_ref_clocks) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SYNC;

  if (tchecker::refdbm::constrain(rdbm, r, guard) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_GUARD_VIOLATED;

  tchecker::refdbm::reset(rdbm, r, clkreset);

  if (tchecker::refdbm::constrain(rdbm, r, tgt_invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_TGT_INVARIANT_VIOLATED;

  return tchecker::STATE_OK;
}

/*!
  \brief Compute initial zone with reference clocks w.r.t. elapsed semantics
  \note see tchecker::refzg::elapsed_semantics_t::initial
*/
tchecker::state_status_t elapsed_initial(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
                                         boost::dynamic_bitset<> const & delay_allowed,
                                         tchecker::clock_constraint_container_t const & invariant, tchecker::integer_t spread)
{
  tchecker::refdbm::zero(rdbm, r);

  if (tchecker::refdbm::constrain(rdbm, r, invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;

  if (delay_allowed.any()) {
    tchecker::refdbm::asynchronous_open_up(rdbm, r, delay_allowed);

    if (tchecker::refdbm::constrain(rdbm, r, invariant) == tchecker::dbm::EMPTY)
      return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;
  }

  if (tchecker::refdbm::bound_spread(rdbm, r, spread) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SPREAD;

  return tchecker::STATE_OK;
}

/*!
  \brief Compute next zone with reference clocks w.r.t. elapsed semantics
  \note see tchecker::refzg::elapsed_semantics_t::next
*/
tchecker::state_status_t
elapsed_next(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
             boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
             boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
             tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
             tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  if (tchecker::refdbm::constrain(rdbm, r, src_invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;

  if (tchecker::refdbm::synchronize(rdbm, r, sync_ref_clocks) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SYNC;

  if (tchecker::refdbm::constrain(rdbm, r, guard) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_GUARD_VIOLATED;

  tchecker::refdbm::reset(rdbm, r, clkreset);

  if (tchecker::refdbm::constrain(rdbm, r, tgt_invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_TGT_INVARIANT_VIOLATED;

  if (tgt_delay_allowed.any()) {
    tchecker::refdbm::asynchronous_open_up(rdbm, r, tgt_delay_allowed);

    if (tchecker::refdbm::constrain(rdbm, r, tgt_invariant) == tchecker::dbm::EMPTY)
      return tchecker::STATE_CLOCKS_TGT_INVARIANT_VIOLATED;
  }

  if (tchecker::refdbm::bound_spread(rdbm, r, spread) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SPREAD;

  return tchecker::STATE_OK;
}

/* standard_semantics_t */

tchecker::state_status_t standard_semantics_t::initial(tchecker::dbm::db_t * rdbm,
                                                       tchecker::reference_clock_variables_t const & r,
                                                       boost::dynamic_bitset<> const & delay_allowed,
                                                       tchecker::clock_constraint_container_t const & invariant,
                                                       tchecker::integer_t spread)
{
  return tchecker::refzg::standard_initial(rdbm, r, delay_allowed, invariant, spread);
}

tchecker::state_status_t standard_semantics_t::next(
    tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
    boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
    boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
    tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
    tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  return tchecker::refzg::standard_next(rdbm, r, src_delay_allowed, src_invariant, sync_ref_clocks, guard, clkreset,
                                        tgt_delay_allowed, tgt_invariant, spread);
}

/* elapsed_semantics_t */

tchecker::state_status_t elapsed_semantics_t::initial(tchecker::dbm::db_t * rdbm,
                                                      tchecker::reference_clock_variables_t const & r,
                                                      boost::dynamic_bitset<> const & delay_allowed,
                                                      tchecker::clock_constraint_container_t const & invariant,
                                                      tchecker::integer_t spread)
{
  return tchecker::refzg::elapsed_initial(rdbm, r, delay_allowed, invariant, spread);
}

tchecker::state_status_t
elapsed_semantics_t::next(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
                          boost::dynamic_bitset<> const & src_delay_allowed,
                          tchecker::clock_constraint_container_t const & src_invariant,
                          boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
                          tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
                          tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  return tchecker::refzg::elapsed_next(rdbm, r, src_delay_allowed, src_invariant, sync_ref_clocks, guard, clkreset,
                                       tgt_delay_allowed, tgt_invariant, spread);
}

/* synchronizable_standard_semantics_t */

tchecker::state_status_t synchronizable_standard_semantics_t::initial(tchecker::dbm::db_t * rdbm,
                                                                      tchecker::reference_clock_variables_t const & r,
                                                                      boost::dynamic_bitset<> const & delay_allowed,
                                                                      tchecker::clock_constraint_container_t const & invariant,
                                                                      tchecker::integer_t spread)
{
  tchecker::state_status_t status = tchecker::refzg::standard_initial(rdbm, r, delay_allowed, invariant, spread);
  if (status != tchecker::STATE_OK)
    return status;

  if (!tchecker::refdbm::is_synchronizable(rdbm, r))
    return tchecker::STATE_ZONE_EMPTY_SYNC;

  return tchecker::STATE_OK;
}

tchecker::state_status_t synchronizable_standard_semantics_t::next(
    tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
    boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
    boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
    tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
    tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  tchecker::state_status_t status = tchecker::refzg::standard_next(rdbm, r, src_delay_allowed, src_invariant, sync_ref_clocks,
                                                                   guard, clkreset, tgt_delay_allowed, tgt_invariant, spread);
  if (status != tchecker::STATE_OK)
    return status;

  if (!tchecker::refdbm::is_synchronizable(rdbm, r))
    return tchecker::STATE_ZONE_EMPTY_SYNC;

  return tchecker::STATE_OK;
}

/* synchronizable_elapsed_semantics_t */

tchecker::state_status_t synchronizable_elapsed_semantics_t::initial(tchecker::dbm::db_t * rdbm,
                                                                     tchecker::reference_clock_variables_t const & r,
                                                                     boost::dynamic_bitset<> const & delay_allowed,
                                                                     tchecker::clock_constraint_container_t const & invariant,
                                                                     tchecker::integer_t spread)
{
  tchecker::state_status_t status = tchecker::refzg::elapsed_initial(rdbm, r, delay_allowed, invariant, spread);
  if (status != tchecker::STATE_OK)
    return status;

  if (!tchecker::refdbm::is_synchronizable(rdbm, r))
    return tchecker::STATE_ZONE_EMPTY_SYNC;

  return tchecker::STATE_OK;
}

tchecker::state_status_t synchronizable_elapsed_semantics_t::next(
    tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
    boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
    boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
    tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
    tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  tchecker::state_status_t status = tchecker::refzg::elapsed_next(rdbm, r, src_delay_allowed, src_invariant, sync_ref_clocks,
                                                                  guard, clkreset, tgt_delay_allowed, tgt_invariant, spread);
  if (status != tchecker::STATE_OK)
    return status;

  if (!tchecker::refdbm::is_synchronizable(rdbm, r))
    return tchecker::STATE_ZONE_EMPTY_SYNC;

  return tchecker::STATE_OK;
}

/* factory */

tchecker::refzg::semantics_t * semantics_factory(enum tchecker::refzg::semantics_type_t semantics)
{
  switch (semantics) {
  case tchecker::refzg::STANDARD_SEMANTICS:
    return new tchecker::refzg::standard_semantics_t{};
  case tchecker::refzg::ELAPSED_SEMANTICS:
    return new tchecker::refzg::elapsed_semantics_t{};
  case tchecker::refzg::SYNC_STANDARD_SEMANTICS:
    return new tchecker::refzg::synchronizable_standard_semantics_t{};
  case tchecker::refzg::SYNC_ELAPSED_SEMANTICS:
    return new tchecker::refzg::synchronizable_elapsed_semantics_t{};
  default:
    throw std::invalid_argument("Unknown semantics over zones with reference clocks");
  }
}

} // end of namespace refzg

} // end of namespace tchecker


/*
 * Copyright (c) 2011 Adrian Michel
 * http://www.amichel.com
 *
 * Permission to use, copy, modify, distribute and sell this
 * software and its documentation for any purpose is hereby
 * granted without fee, provided that both the above copyright
 * notice and this permission notice appear in all copies and in
 * the supporting documentation.
 *
 * This library is distributed in the hope that it will be
 * useful. However, Adrian Michel makes no representations about
 * the suitability of this software for any purpose.  It is
 * provided "as is" without any express or implied warranty.
 *
 * Should you find this library useful, please email
 * info@amichel.com with a link or other reference
 * to your work.
*/

#ifndef DE_MUTATION_STRATEGY_HPP_INCLUDED
#define DE_MUTATION_STRATEGY_HPP_INCLUDED

// MS compatible compilers support #pragma once

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

#include "population.hpp"

#define URN_DEPTH 5

#include <boost/tuple/tuple.hpp>

namespace de
{

/**
 * Parameters used by mutation strategies
 * weight factor, crossover factor and dither factor which is
 * calculated from the previous two
 *
 * @author adrian (12/1/2011)
 */
class mutation_strategy_arguments
{
private:
	const double m_weight;
	const double m_crossover;
	const double m_dither;

public:
	/**
	 * constructs a mutation_strategy_arguments object.
	 *
	 * Besides the weight and crossover factors, which are supplied
	 * by the calling code, this object holds a dither factor, which
	 * is calculated upon construction, and is used by some mutation
	 * strtegies.
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param weight weight factor which is a double between 0-2 as
	 *  			 defined by the differential evolution algorithm
	 * @param crossover crossover factor which is a double between
	 *  				0-1 as defined by the differential evolution
	 *  				algorithm
	 */
	mutation_strategy_arguments( double weight, double crossover )
	: m_weight( weight ), m_crossover( crossover ), m_dither(  weight + genrand() * ( 1.0 - weight ) )
	{
		// todo: test or assert that weight and crossover are within bounds (0-1, 0-2 or something)
	}

	/**
	 * returns the weight factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double weight() const { return m_weight; }
	/**
	 * returns the crossover factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double crossover() const { return m_crossover; }
	/**
	 * returns the dither factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double dither() const { return m_dither; }
};

/**
 * A an abstract based class for mutation strategies
 *
 * A mutation strategy defines how varaibles values are adjusted
 * during the optimization process
 *
 * @author adrian (12/1/2011)
 */
class mutation_strategy
{
private:
	mutation_strategy_arguments m_args;
	size_t m_varCount;

protected:
	/**
	 * Used to generate a set of 4 random size_t numbers
	 * by the mutation strategy as indexes
	 *
	 * The numbers must all be different, and also different from an
	 * index supplied externally
	 *
	 * @author adrian (12/1/2011)
	 */
	class Urn
	{
		size_t m_urn[ URN_DEPTH ];

	public:
		/**
		 * Constructs an urn object
		 *
		 * @author adrian (12/4/2011)
		 *
		 * @param NP upper limit (exclusive) for the generated random
		 *  		 numbers
		 * @param avoid value to avoid when generating the random
		 *  			numbers
		 */
		Urn( size_t NP, size_t avoid )
		{
			do m_urn[ 0 ] = genintrand( 0, NP, true ) ; while( m_urn[ 0 ] == avoid ) ;
			do m_urn[ 1 ] = genintrand( 0, NP, true ) ; while( m_urn[ 1 ] == m_urn[ 0 ] || m_urn[ 1 ] == avoid );
			do m_urn[ 2 ] = genintrand( 0, NP, true ) ; while( m_urn[ 2 ] == m_urn[ 1 ] || m_urn[ 2 ] == m_urn[ 0 ] || m_urn[ 2  ] == avoid );
			do m_urn[ 3 ] = genintrand( 0, NP, true ) ; while( m_urn[ 3 ] == m_urn[ 2 ] || m_urn[ 3 ] == m_urn[ 1 ] || m_urn[ 3 ] == m_urn[ 0 ] || m_urn[ 3 ] == avoid );
		}

		/**
		 * returns one of the four generated random numbers
		 *
		 * @author adrian (12/4/2011)
		 *
		 * @param index the index of the random number to return, can be
		 *  			between 0-3
		 *
		 * @return size_t
		 */
		size_t operator[]( size_t index ) const { assert( index < 4 ); return m_urn[ index ]; }
	};

public:
	virtual ~mutation_strategy()
	{
	}

	/**
	 * constructs a mutation strategy
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy( size_t varCount, const mutation_strategy_arguments& args )
	: m_args( args ), m_varCount( varCount )
	{
	}

	/**
	 * type for the tuple returned by the operator() member.
	 */
	typedef boost::tuple< individual_ptr, de::DVectorPtr > mutation_info;

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	virtual mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i ) = 0;

	/**
	 * returns the number of variables
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return size_t
	 */
	size_t varCount() const { return m_varCount; }

	/**
	 * returns the weight factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double weight() const { return m_args.weight(); }

	/**
	 * returns the crossover factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double crossover() const { return m_args.crossover(); }

	/**
	 * returns the dither factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double dither() const { return m_args.dither(); }
};

typedef boost::shared_ptr< mutation_strategy > mutation_strategy_ptr;

/**
 * Mutation strategy #1
 *
 * @author adrian (12/1/2011)
 */
class mutation_strategy_1 : public mutation_strategy
{
public:
	/**
	 * constructs a mutation strategy # 1
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy_1( size_t varCount, const mutation_strategy_arguments& args )
	: mutation_strategy( varCount, args )
	{
	}

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i )
	{
		assert( bestIt );

		de::DVectorPtr origin( boost::make_shared< de::DVector >( varCount() ) );
		individual_ptr tmpInd( boost::make_shared< individual >( *pop[ i ]->vars() ) );
		Urn urn( pop.size(), i );

		// make sure j is within bounds
		size_t j = genintrand( 0, varCount(), true );
		size_t k = 0;

		do
		{
			(*tmpInd->vars())[ j ] = (*pop[ urn[ 0 ] ]->vars() )[ j ] + weight() * ( (*pop[ urn[ 1 ] ]->vars() )[ j ] - (*pop[ urn[ 2 ] ]->vars())[ j ] );

			j = ++j % varCount();
			++k;
		} while( genrand() < crossover() && k < varCount() );

		origin = pop[ urn[ 0 ] ]->vars();

		return mutation_info( tmpInd, origin );
	}

};

/**
 * mutation strategy # 2
 *
 * @author adrian (12/4/2011)
 */
class mutation_strategy_2 : public mutation_strategy
{
public:
	/**
	 * constructs a mutation strategy # 2
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy_2( size_t varCount, const mutation_strategy_arguments& args )
	: mutation_strategy( varCount, args )
	{
	}

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i )
	{
		assert( bestIt );

		de::DVectorPtr origin( boost::make_shared< de::DVector >( varCount() ) );
		individual_ptr tmpInd( boost::make_shared< individual >( *pop[ i ]->vars() ) );
		Urn urn( pop.size(), i );

		// make sure j is within bounds
		size_t j = genintrand( 0, varCount(), true );
		size_t k = 0;

		do
		{
			(*tmpInd->vars())[ j ] = (*tmpInd->vars())[ j ] +
				weight() * ( (*bestIt->vars() )[ j ] - (*tmpInd->vars())[ j ] ) +
				weight() * ( (*pop[ urn[ 1 ] ]->vars() )[ j ] - (*pop[ urn[ 2 ] ]->vars())[ j ] );

			j = ++j % varCount();
			++k;
		} while( genrand() < crossover() && k < varCount() );

		origin = pop[ urn[ 0 ] ]->vars();

		return mutation_info( tmpInd, origin );
	}

};

/**
 * mutation strategy # 3
 *
 * @author adrian (12/4/2011)
 */
class mutation_strategy_3 : public mutation_strategy
{
public:
	/**
	 * constructs a mutation strategy # 3
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy_3( size_t varCount, const mutation_strategy_arguments& args )
	: mutation_strategy( varCount, args )
	{
	}

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i )
	{
		assert( bestIt );

		de::DVectorPtr origin( boost::make_shared< de::DVector >( varCount() ) );
		individual_ptr tmpInd( boost::make_shared< individual >( *pop[ i ]->vars() ) );
		Urn urn( pop.size(), i );

		// make sure j is within bounds
		size_t j = genintrand( 0, varCount(), true );
		size_t k = 0;

		do
		{
			double jitter = (0.0001* genrand() + weight() );

			(*tmpInd->vars())[ j ] = (*bestIt->vars() )[ j ] + jitter * ( (*pop[ urn[ 1 ] ]->vars() )[ j ] - (*pop[ urn[ 2 ] ]->vars())[ j ] );

			j = ++j % varCount();
			++k;
		} while( genrand() < crossover() && k < varCount() );

		origin = pop[ urn[ 0 ] ]->vars();

		return mutation_info( tmpInd, origin );
	}
};

/**
 * mutation strategy # 4
 *
 * @author adrian (12/4/2011)
 */
class mutation_strategy_4 : public mutation_strategy
{
public:
	/**
	 * constructs a mutation strategy # 4
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy_4( size_t varCount, const mutation_strategy_arguments& args )
	: mutation_strategy( varCount, args )
	{
	}

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i )
	{
		assert( bestIt );

		de::DVectorPtr origin( boost::make_shared< de::DVector >( varCount() ) );
		individual_ptr tmpInd( boost::make_shared< individual >( *pop[ i ]->vars() ) );
		Urn urn( pop.size(), i );

		// make sure j is within bounds
		size_t j = genintrand( 0, varCount(), true );
		size_t k = 0;

		double factor( weight() + genrand() * ( 1.0 - weight() ) );

		do
		{
			double jitter = (0.0001* genrand() + weight() );

			(*tmpInd->vars())[ j ] = (*pop[ urn[ 0 ] ]->vars() )[ j ] +
				factor * ( (*pop[ urn[ 1 ] ]->vars() )[ j ] - (*pop[ urn[ 2 ] ]->vars())[ j ] );

			j = ++j % varCount();
			++k;
		} while( genrand() < crossover() && k < varCount() );

		origin = pop[ urn[ 0 ] ]->vars();

		return mutation_info( tmpInd, origin );
	}

};

/**
 * Mutation strategy # 5
 *
 * @author adrian (12/1/2011)
 */
class mutation_strategy_5 : public mutation_strategy
{

public:
	/**
	 * constructs a mutation strategy # 5
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy_5( size_t varCount, const mutation_strategy_arguments& args )
	: mutation_strategy( varCount, args )
	{
	}

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i )
	{
		assert( bestIt );

		de::DVectorPtr origin( boost::make_shared< de::DVector >( varCount() ) );
		individual_ptr tmpInd( boost::make_shared< individual >( *pop[ i ]->vars() ) );
		Urn urn( pop.size(), i );

		// make sure j is within bounds
		size_t j = genintrand( 0, varCount(), true );
		size_t k = 0;

		do
		{
			(*tmpInd->vars())[ j ] = (*pop[ urn[ 0 ] ]->vars() )[ j ] + dither() * ( (*pop[ urn[ 1 ] ]->vars() )[ j ] - (*pop[ urn[ 2 ] ]->vars() )[ j ] );

			j = ++j % varCount();
			++k;
		} while( genrand() < crossover() && k < varCount() );

		origin = pop[ urn[ 0 ] ]->vars();
		return mutation_info( tmpInd, origin );
	}

};

}

#endif //DE_MUTATION_STRATEGY_HPP_INCLUDED


// Copyright 2021 Apex.AI, Inc.
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef MEASUREMENT_CONVERSION__EIGEN_UTILS_HPP_
#define MEASUREMENT_CONVERSION__EIGEN_UTILS_HPP_

#include <Eigen/Geometry>

namespace autoware
{
namespace common
{
namespace state_estimation
{

///
/// @brief      This class describes a data storage order.
///
enum class DataStorageOrder
{
  kRowMajor,
  kColumnMajor
};

namespace detail
{
///
/// @brief      A functor to compute an index in the data array.
///
class Index
{
public:
  ///
  /// @brief      Create an index functor.
  ///
  /// @param[in]  start_index    The start index at which to start the count.
  /// @param[in]  stride         The stride of the data, i.e., step to the next row or column.
  /// @param[in]  storage_order  How the data should be stored in memory.
  ///
  Index(
    const std::int32_t start_index,
    const std::int32_t stride,
    const DataStorageOrder storage_order)
  : m_start_index{start_index}, m_stride{stride}, m_storage_order{storage_order} {}

  ///
  /// @brief      Compute an index using the stored starting index, stride and storage order.
  ///
  /// @param[in]  row   Query row
  /// @param[in]  col   Query column
  ///
  /// @return     A 1D index in the underlying data array
  ///
  std::size_t operator()(const Eigen::Index row, const Eigen::Index col) const
  {
    switch (m_storage_order) {
      case DataStorageOrder::kRowMajor:
        return static_cast<std::size_t>(m_start_index + row * m_stride + col);
      case DataStorageOrder::kColumnMajor:
        return static_cast<std::size_t>(m_start_index + col * m_stride + row);
    }
    throw std::runtime_error("Unexpected storage order");
  }

private:
  std::int32_t m_start_index{};
  std::int32_t m_stride{};
  DataStorageOrder m_storage_order{};
};
}  // namespace detail

///
/// @brief      Downscale the isometry to a lower dimension if needed.
///
/// @param[in]  isometry              The isometry transform
///
/// @tparam     kStateDimensionality  Dimensionality of the space.
/// @tparam     FloatT                Type of scalar.
///
/// @return     Downscaled isometry.
///
template<std::int32_t kStateDimensionality, typename FloatT>
static constexpr Eigen::Transform<
  FloatT, kStateDimensionality, Eigen::TransformTraits::Isometry> downscale_isometry(
  const Eigen::Transform<FloatT, 3, Eigen::TransformTraits::Isometry> & isometry)
{
  static_assert(kStateDimensionality <= 3, "We only handle scaling the isometry down.");
  using Isometry = Eigen::Transform<
    FloatT, kStateDimensionality, Eigen::TransformTraits::Isometry>;
  Isometry result{Isometry::Identity()};
  result.linear() = isometry.rotation()
    .template block<kStateDimensionality, kStateDimensionality>(0, 0);
  result.translation() = isometry.translation().topRows(kStateDimensionality);
  return result;
}

///
/// @brief      Transform a given array to an Eigen matrix using the specified stride and a starting
///             index within this array.
///
/// @details    The function converts a given array into an Eigen matrix using the given starting
///             index and stride. The function will take the number of rows and columns from the
///             provided parameters and will iterate the given array as many times as needed to fill
///             all elements of the resulting matrix. It is assumed that there are no gaps between
///             the elements in a single row but that the columns are spaced `stride` apart.
///
/// @note       The storage order only refers to the storage order expected from the array, not from
///             the Eigen matrix. The Eigen matrices are expected to be column-major as per default.
///
/// @throws     std::runtime_error  if there is not enough elements in the array to perform a full
///                                 conversion of all the asked elements.
///
/// @param[in]  array          The given array (e.g. a covariance array from a message)
/// @param[in]  start_index    The start index of the first element in an array to be copied
/// @param[in]  stride         How big the step to the next row is in terms of indices in the array
/// @param[in]  storage_order  The storage order of the data in the input array (row-major for ROS)
///
/// @tparam     kRows          Number of rows in the resulting matrix
/// @tparam     kCols          Number of columns in the resulting matrix
/// @tparam     ScalarT        Scalar type (inferred)
/// @tparam     kSize          Size of the input array (inferred)
///
/// @return     An Eigen matrix that stores the requested data.
///
template<std::int32_t kRows, std::int32_t kCols, typename ScalarT, std::size_t kSize>
Eigen::Matrix<ScalarT, kRows, kCols> array_to_matrix(
  const std::array<ScalarT, kSize> & array,
  const std::int32_t start_index,
  const std::int32_t stride,
  const DataStorageOrder storage_order)
{
  using Mat = Eigen::Matrix<ScalarT, kRows, kCols>;
  const detail::Index index{start_index, stride, storage_order};
  Mat res{Mat::Zero()};
  const auto max_index = index(res.rows() - 1, res.cols() - 1);
  if (max_index >= array.size()) {
    throw std::runtime_error(
            "Trying to access out of bound memory at index " +
            std::to_string(max_index) + " of an array with size: " + std::to_string(array.size()));
  }

  for (auto col = 0; col < kCols; ++col) {
    for (auto row = 0; row < kRows; ++row) {
      res(row, col) = array[index(row, col)];
    }
  }
  return res;
}

///
/// @brief      Sets data in an array from a given Eigen matrix.
///
/// @note       The storage order only refers to the storage order expected from the array, not from
///             the Eigen matrix. The Eigen matrices are expected to be column-major as per default.
///
/// @param      array          The array for which the data is set
/// @param[in]  matrix         The matrix that has the data that is to be copied
/// @param[in]  start_index    The start index in the output data array
/// @param[in]  stride         The step that brings the index to the next row/column
/// @param[in]  storage_order  The storage order of the output data
///
/// @tparam     kRows          Number of rows in the input matrix, inferred by the compiler.
/// @tparam     kCols          Number of columns in the input matrix, inferred by the compiler.
/// @tparam     ScalarT        Type of underlying data elements, inferred by the compiler.
/// @tparam     kSize          Size of the input matrix, inferred by the compiler.
///
template<std::int32_t kRows, std::int32_t kCols, typename ScalarT, std::size_t kSize>
void set_from_matrix(
  std::array<ScalarT, kSize> & array,
  const Eigen::Matrix<ScalarT, kRows, kCols> & matrix,
  const std::int32_t start_index,
  const std::int32_t stride,
  const DataStorageOrder storage_order)
{
  const detail::Index index{start_index, stride, storage_order};
  const auto max_index = index(matrix.rows() - 1, matrix.cols() - 1);
  if (max_index >= array.size()) {
    throw std::runtime_error(
            "Trying to access out of bound memory at index " +
            std::to_string(max_index) + " of an array with size: " + std::to_string(array.size()));
  }
  for (auto col = 0; col < kCols; ++col) {
    for (auto row = 0; row < kRows; ++row) {
      array[index(row, col)] = matrix(row, col);
    }
  }
}

///
/// @brief      Get a slice of a given matrix.
///
/// @details    Given two sequences for rows and for columns generate a new matrix that contains
///             only rows and columns in those sequences.
///
/// @param[in]  matrix         The given matrix
/// @param[in]  sequence_rows  The sequence of rows to include in the slice
/// @param[in]  sequence_cols  The sequence of columns to include in the slice
///
/// @tparam     ScalarT        Type of underlying data
/// @tparam     kRows          Number of rows in a query matrix
/// @tparam     kCols          Number of columns in a query matrix
/// @tparam     kSeqRowsSize   Number of rows in the slice
/// @tparam     kSeqColsSize   Number of columns in the slice
///
/// @return     A matrix representing a slice of the original matrix.
///
template<
  typename ScalarT,
  std::int32_t kRows,
  std::int32_t kCols,
  std::size_t kSeqRowsSize,
  std::size_t kSeqColsSize>
Eigen::Matrix<
  ScalarT, static_cast<std::int32_t>(kSeqRowsSize), static_cast<std::int32_t>(kSeqColsSize)>
slice(
  const Eigen::Matrix<ScalarT, kRows, kCols> matrix,
  const std::array<Eigen::Index, kSeqRowsSize> & sequence_rows,
  const std::array<Eigen::Index, kSeqColsSize> & sequence_cols)
{
  using Mat = Eigen::Matrix<
    ScalarT, static_cast<std::int32_t>(kSeqRowsSize), static_cast<std::int32_t>(kSeqColsSize)>;
  Mat res{Mat::Zero()};
  for (auto col = 0UL; col < kSeqColsSize; ++col) {
    for (auto row = 0UL; row < kSeqRowsSize; ++row) {
      res(static_cast<Eigen::Index>(row), static_cast<Eigen::Index>(col)) =
        matrix(sequence_rows[row], sequence_cols[col]);
    }
  }
  return res;
}

}  // namespace state_estimation
}  // namespace common
}  // namespace autoware

#endif  // MEASUREMENT_CONVERSION__EIGEN_UTILS_HPP_