//@HEADER
// ************************************************************************
//
//                        miniTri v. 1.0
//              Copyright (2016) Sandia Corporation
//
// Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
// the U.S. Government retains certain rights in this software.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the Corporation nor the names of the
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY SANDIA CORPORATION "AS IS" AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL SANDIA CORPORATION OR THE
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Questions? Contact  Jon Berry (jberry@sandia.gov)
//                     Michael Wolf (mmwolf@sandia.gov)
//
// ************************************************************************
//@HEADER

//////////////////////////////////////////////////////////////////////////////
//                                                                          //
// File:      CSRmatrix.h                                                   //
// Project:   miniTri                                                       //
// Author:    Michael Wolf                                                  //
//                                                                          //
// Description:                                                             //
//              Header file for CSR matrix class.                           //
//                                                                          //
//////////////////////////////////////////////////////////////////////////////
#ifndef CSRMATRIX_H
#define CSRMATRIX_H

typedef enum {UNDEFINED,LOWERTRI,UPPERTRI,INCIDENCE} matrixtype;

#include <list>
#include <vector>
#include <map>

#include <boost/shared_array.hpp>

class Vector;

//////////////////////////////////////////////////////////////////////////////
// Compressed Sparse Row storage format Matrix
//////////////////////////////////////////////////////////////////////////////
class CSRMat
{

 private:
  matrixtype type;
  int m;   //number of rows
  int n;   //number of cols
  int nnz; //number of nonzeros

  boost::shared_array<int> nnzInRow;                             // nnz in each row
  boost::shared_array<boost::shared_array<int> > cols;           //columns of nonzeros
  boost::shared_array<boost::shared_array<int> > vals; //values of nonzeros
  boost::shared_array<boost::shared_array<int> > vals2; //values of nonzeros

 public:
  //////////////////////////////////////////////////////////////////////////
  // default constructor -- builds empty matrix
  //////////////////////////////////////////////////////////////////////////
  CSRMat()
    :type(UNDEFINED),m(0),n(0),nnz(0),nnzInRow(),cols(),vals(),vals2()
  {
  };
  //////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////
  // Constructor that accepts matrix type as an argument
  //////////////////////////////////////////////////////////////////////////
  CSRMat(matrixtype _type)
    :type(_type),m(0),n(0),nnz(0),nnzInRow(),cols(),vals(),vals2()
  {
  };
  //////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////
  // constructor -- allocates memory for CSR sparse matrix
  //////////////////////////////////////////////////////////////////////////
  CSRMat(int _m, int _n,bool allocateVals2=false)
    :type(UNDEFINED),m(_m),n(_n),
     nnzInRow(new int[m]),cols(new boost::shared_array<int> [m]),
     vals(new boost::shared_array<int> [m])
  {
    if(allocateVals2==true)
    {
      vals2 = boost::shared_array<boost::shared_array<int> >(new boost::shared_array<int> [m]);
    }
  };
  //////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////
  // destructor -- deletes matrix
  //////////////////////////////////////////////////////////////////////////
  ~CSRMat()
  {
  };
  //////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////
  // additional functions and prototypes for additional functions
  // defined in CSRmatrix.cc
  //////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////
  // print -- prints matrix to stdio
  //////////////////////////////////////////////////////////////////
  void print() const;
  //////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////
  // Sums matrix elements
  //////////////////////////////////////////////////////////////////
  std::list<int> getSumElements() const;
  //////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////
  // additional accessors and accessor prototypes
  //////////////////////////////////////////////////////////////////
  // returns the number of rows
  int getM() const { return m;};

  // returns the number of cols
  int getN() const { return n;};

  // returns the number of cols
  int getNNZ() const { return nnz;};

  // returns NNZ in row rnum
  inline int getNNZInRow(int rnum) const {return nnzInRow[rnum];};

  // returns column # for nonzero in row rowi at index nzindx
  inline int getCol(int rowi, int nzindx) const {return cols[rowi][nzindx];};

  // returns value for nonzero at inddex nzindx
  inline int & getVal(int rowi, int nzindx) const
    {return vals[rowi][nzindx];};

  //////////////////////////////////////////////////////////////////

  void SpMV1(bool trans, Vector &y);

  //////////////////////////////////////////////////////////////////
  // level 3 basic linear algebra subroutines
  //////////////////////////////////////////////////////////////////
  void matmat(const CSRMat &A, const CSRMat &B);
  //////////////////////////////////////////////////////////////////

  void computeKCounts(const Vector &vTriDegrees, const Vector &eTriDegrees,
                      const std::map<int,std::map<int,int> > & edgeInds,
                      std::vector<int> &kCounts);

  void readMMMatrix(const char* fname);
  void readBinMatrix(const char* fname);

  void createTriMatrix(const CSRMat &matrix, matrixtype mtype);
  void createIncidentMatrix(const CSRMat &matrix, std::map<int,std::map<int,int> > & eIndices);

  void permute();

  //////////////////////////////////////////////////////////////////////////

};
//////////////////////////////////////////////////////////////////////////////

#endif


#include <boost/test/unit_test.hpp>

#include "PeriodVal.h"
#include "PeriodValSegment.h"
#include "DoubleBottomScanner.h"
#include "TestHelper.h"
#include "PatternShapeGenerator.h"
#include "MultiPatternScanner.h"
#include "PatternMatchFilter.h"
#include "PatternShape.h"

using namespace boost::posix_time;
using namespace boost::gregorian;
using namespace testHelper;

BOOST_AUTO_TEST_CASE( PatternShape_DoubleBottom_QCOR_20130819 )
{
    // Test a full double-bottom scan for QCOR in 2013. This unit test is built up from
    // VScanner_QCOR_20130819_LHSofDoubleBottom and VScanner_QCOR_20130819_RHSofDoubleBottom, which
    // test the LHS and RHS of the double-bottom individually. The number of pattern matches
    // should therefore be the number of matches from VScanner_QCOR_20130819_LHSofDoubleBottom
    // multiplied times the number of matches from VScanner_QCOR_20130819_RHSofDoubleBottom.

    PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternScan/QCOR_DoubleBottom_Weekly.csv");
    genPeriodValSegmentInfo("Double bottom segment data",*chartData);

    DoubleBottomScanner scanner(DoubleRange(7.0,40.0)); // allow for a deeper depth than the default
    PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

    verifyMatchList("Double bottom match",patternMatches,1);

    verifyPatternMatch("Double bottom match",
            ptime(date(2013,8,26)),ptime(date(2014,2,10)),4,patternMatches->front());

    PatternMatchPtr doubleBottomMatch = patternMatches->front();

    PatternShapeGenerator shapeGen;
    PatternShapePtr patternShape = shapeGen.generateShape(*doubleBottomMatch);
    BOOST_CHECK_EQUAL(patternShape->numLineShapes(),2);

    PatternShapePointVectorVectorPtr lineShapes = patternShape->lineShapes();
    PatternShapePointVectorPtr firstShape = (*lineShapes)[0];

    BOOST_TEST_MESSAGE("PatternShape_DoubleBottom_QCOR_20130819: num points in shape: " << firstShape->size());
    BOOST_CHECK_EQUAL(firstShape->size(),3);

}

BOOST_AUTO_TEST_CASE( PatternShape_DoubleBottom_MultiScan_QCOR_2013_2014_Weekly )
{

    PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternShape/QCOR_2013_2014_Weekly.csv");

    PatternScannerPtr doubleBottomScanner(new DoubleBottomScanner(DoubleRange(7.0,40.0)));
    MultiPatternScanner multiScanner(doubleBottomScanner);

    PatternMatchListPtr doubleBottoms = multiScanner.scanPatternMatches(chartData);

    // TODO - The scanning seems to be working fine, but the actual pattern matches for a double-bottom don't appear to
    // be correct. In particular, the start dates for some of the matches occur below the RHS of the first dip in the
    // double bottom.
    verifyMatchList("PatternShape_DoubleBottom_MultiScan_QCOR_2013_2014_Weekly: unfiltered matches",doubleBottoms,1);

    PatternMatchListPtr  uniqueMatches = patternMatchFilter::filterUniqueLongestPatternSameEndDate(doubleBottoms);

    genPatternMatchListInfo("Unique matches",*uniqueMatches);

    verifyMatchList("PatternShape_DoubleBottom_MultiScan_QCOR_2013_2014_Weekly: filtered matches",uniqueMatches,1);
    verifyPatternMatch("PatternShape_DoubleBottom_MultiScan_QCOR_2013_2014_Weekly match",
            ptime(date(2013,8,26)),ptime(date(2014,2,10)),4,uniqueMatches,0);

}


#include <boost/test/unit_test.hpp>
#include "algorithms/data_structures/array/num_is_sum_of_square_numbers.hpp"

BOOST_AUTO_TEST_SUITE(TestNumIsSumOfSquareNumbers)

BOOST_AUTO_TEST_CASE(check)
{
    BOOST_CHECK(true == Algo::DS::Array::SumOfSquares::check(0));
    BOOST_CHECK(true == Algo::DS::Array::SumOfSquares::check(1));
    BOOST_CHECK(true == Algo::DS::Array::SumOfSquares::check(2));
    BOOST_CHECK(false == Algo::DS::Array::SumOfSquares::check(3));
    BOOST_CHECK(true == Algo::DS::Array::SumOfSquares::check(4));
    BOOST_CHECK(true == Algo::DS::Array::SumOfSquares::check(5));
    BOOST_CHECK(false == Algo::DS::Array::SumOfSquares::check(7));
    BOOST_CHECK(true == Algo::DS::Array::SumOfSquares::check(8));
    BOOST_CHECK(true == Algo::DS::Array::SumOfSquares::check(9));
    BOOST_CHECK(true == Algo::DS::Array::SumOfSquares::check(10));
    BOOST_CHECK(true == Algo::DS::Array::SumOfSquares::check(13));
    BOOST_CHECK(true == Algo::DS::Array::SumOfSquares::check(25));
    BOOST_CHECK(true == Algo::DS::Array::SumOfSquares::check(257));
}

BOOST_AUTO_TEST_SUITE_END()


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_TRAIT_SIZE_HPP__
#define __OPTIMISATION2_TRAIT_SIZE_HPP__

#include <cstddef>
#include <utility>
#include <libv/core/tag.hpp>
#include <type_traits>
#include <boost/mpl/bool.hpp>
#include <array>

namespace lma
{
  template<class T, class = void> struct Size { enum {value = -1}; };

  template<class T, size_t N> struct Size< T[N] > { enum { value = N }; };

  template<class T> struct Size<T&> : Size<T> {};
  template<class T> struct Size<const T> : Size<T> {};
  template<class T> struct Size<T*> : Size<T> {};
  template<class T> struct Size<const T*> : Size<T> {};
  template<class T> struct Size<const T&> : Size<T> {};

  template<class T, size_t N> struct Size< std::array<T,N> > { enum { value = N } ; };

  template<class T> struct Size< std::pair<T,bool> > : Size<T> {};

  template<> struct Size< float >          { enum { value = 1 }; };
  template<> struct Size< int >            { enum { value = 1 }; };
  template<> struct Size< double >         { enum { value = 1 }; };
  template<int I> struct Size<v::numeric_tag<I>>  { enum { value = I }; };

  template<class X, class R = void> struct SizeIsDefined : boost::mpl::false_ {};
  template<class X> struct SizeIsDefined< X, typename std::enable_if<Size<X>::value!=-1>::type > : boost::mpl::true_ {};
}

#endif


/***************************************************************************************************************************
 * object_tracking.cpp
 *
 * Author: Qyp
 *
 * Update Time: 2020.2.11
 *
 * 说明: 目标追踪示例程序
 *      1. 订阅目标位置(来自视觉的ros节点)
 *      2. 追踪算法及追踪策略
 *      3. 发布上层控制指令 (prometheus_msgs::ControlCommand)
***************************************************************************************************************************/
//ros头文件
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <iostream>
#include <mission_utils.h>

//topic 头文件
#include <prometheus_msgs/ControlCommand.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/DetectionInfo.h>
#include "message_utils.h"

using namespace std;
using namespace Eigen;
# define NODE_NAME "object_tracking"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
prometheus_msgs::DroneState _DroneState;
Eigen::Vector3f drone_pos;
//---------------------------------------Vision---------------------------------------------
prometheus_msgs::DetectionInfo Detection_raw;          //目标位置[机体系下：前方x为正，右方y为正，下方z为正]
Eigen::Vector3f pos_body_frame;
Eigen::Vector3f pos_body_enu_frame;     //原点位于质心，x轴指向前方，y轴指向左，z轴指向上的坐标系
Eigen::Vector3f pos_des_prev;
float kpx_track,kpy_track,kpz_track;                                                 //控制参数 - 比例参数
float start_point_x,start_point_y,start_point_z,start_yaw;
bool is_detected = false;                                          // 是否检测到目标标志
int num_count_vision_lost = 0;                                                      //视觉丢失计数器
int num_count_vision_regain = 0;                                                      //视觉丢失计数器
int Thres_vision = 0;                                                          //视觉丢失计数器阈值
Eigen::Vector3f camera_offset;
//---------------------------------------Track---------------------------------------------
float distance_to_setpoint;
Eigen::Vector3f tracking_delta;
//---------------------------------------Output---------------------------------------------
prometheus_msgs::ControlCommand Command_Now;                               //发送给控制模块 [px4_pos_controller.cpp]的命令
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param();                                                                 //打印各项参数以供检查
void printf_result();                                                                 //打印函数
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void vision_cb(const prometheus_msgs::DetectionInfo::ConstPtr &msg)
{
    Detection_raw = *msg;

    pos_body_frame[0] =   Detection_raw.position[2] + camera_offset[0];
    pos_body_frame[1] = - Detection_raw.position[0] + camera_offset[1];
    pos_body_frame[2] = - Detection_raw.position[1] + camera_offset[2];

    Eigen::Matrix3f R_Body_to_ENU;

    R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]);

    pos_body_enu_frame = R_Body_to_ENU * pos_body_frame;

    if(Detection_raw.detected)
    {
        num_count_vision_regain++;
        num_count_vision_lost = 0;
    }else
    {
        num_count_vision_regain = 0;
        num_count_vision_lost++;
    }

    // 当连续一段时间无法检测到目标时，认定目标丢失
    if(num_count_vision_lost > Thres_vision)
    {
        is_detected = false;
    }

    // 当连续一段时间检测到目标时，认定目标得到
    if(num_count_vision_regain > Thres_vision)
    {
        is_detected = true;
    }
}
void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr& msg)
{
    _DroneState = *msg;

    drone_pos[0] = _DroneState.position[0];
    drone_pos[1] = _DroneState.position[1];
    drone_pos[2] = _DroneState.position[2];
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "object_tracking");
    ros::NodeHandle nh("~");

    //节点运行频率： 20hz 【视觉端解算频率大概为20HZ】
    ros::Rate rate(20.0);

    // 【订阅】视觉消息 来自视觉节点
    //  方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
    //  标志位：   detected 用作标志位 ture代表识别到目标 false代表丢失目标
    // 注意这里为了复用程序使用了/prometheus/object_detection/kcf_tracker作为话题名字，适用于椭圆、二维码、yolo等视觉算法
    // 故同时只能运行一种视觉识别程序，如果想同时追踪多个目标，这里请修改接口话题的名字
    ros::Subscriber vision_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/kcf_tracker", 10, vision_cb);

    ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);

    // 【发布】发送给控制模块 [px4_pos_controller.cpp]的命令
    ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);

    // 【发布】用于地面站显示的提示消息
    ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
    //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    //视觉丢失次数阈值
    nh.param<int>("Thres_vision", Thres_vision, 10);

    //追踪的前后间隔
    nh.param<float>("tracking_delta_x", tracking_delta[0], 0.0);
    nh.param<float>("tracking_delta_y", tracking_delta[1], 0.0);
    nh.param<float>("tracking_delta_z", tracking_delta[2], 0.0);

    nh.param<float>("camera_offset_x", camera_offset[0], 0.0);
    nh.param<float>("camera_offset_y", camera_offset[1], 0.0);
    nh.param<float>("camera_offset_z", camera_offset[2], 0.0);

    //追踪控制参数
    nh.param<float>("kpx_track", kpx_track, 0.1);
    nh.param<float>("kpy_track", kpy_track, 0.1);
    nh.param<float>("kpz_track", kpz_track, 0.1);

    nh.param<float>("start_point_x", start_point_x, 0.0);
    nh.param<float>("start_point_y", start_point_y, 0.0);
    nh.param<float>("start_point_z", start_point_z, 2.0);
    nh.param<float>("start_yaw", start_yaw, 0.0);

    //打印现实检查参数
    printf_param();
    //固定的浮点显示
    cout.setf(ios::fixed);
    //setprecision(n) 设显示小数精度为n位
    cout<<setprecision(2);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    // Waiting for input
    int start_flag = 0;
    while(start_flag == 0)
    {
        cout << ">>>>>>>>>>>>>>>>>>>>>>>>>Object Tracking Mission<<<<<<<<<<<<<<<<<<<<<< "<< endl;
        cout << "Please check the parameter and setting，enter 1 to continue， else for quit: "<<endl;
        cin >> start_flag;
    }

    // 起飞
    cout<<"[object tracking]: "<<"Takeoff to predefined position."<<endl;
    Command_Now.Command_ID = 1;
    Command_Now.source = NODE_NAME;
    while( _DroneState.position[2] < 0.3)
    {
        Command_Now.header.stamp = ros::Time::now();
        Command_Now.Mode  = prometheus_msgs::ControlCommand::Idle;
        Command_Now.Command_ID = Command_Now.Command_ID + 1;
        Command_Now.source = NODE_NAME;
        Command_Now.Reference_State.yaw_ref = 999;
        command_pub.publish(Command_Now);
        cout << "Switch to OFFBOARD and arm ..."<<endl;
        ros::Duration(3.0).sleep();

        Command_Now.header.stamp                    = ros::Time::now();
        Command_Now.Mode                                = prometheus_msgs::ControlCommand::Move;
        Command_Now.Command_ID = Command_Now.Command_ID + 1;
        Command_Now.source = NODE_NAME;
        Command_Now.Reference_State.Move_mode           = prometheus_msgs::PositionReference::XYZ_POS;
        Command_Now.Reference_State.Move_frame          = prometheus_msgs::PositionReference::ENU_FRAME;
        Command_Now.Reference_State.position_ref[0]     = start_point_x;
        Command_Now.Reference_State.position_ref[1]     = start_point_y;
        Command_Now.Reference_State.position_ref[2]     = start_point_z;
        Command_Now.Reference_State.yaw_ref             = start_yaw/180*3.1415926;
        command_pub.publish(Command_Now);
        cout << "Takeoff ..."<<endl;
        ros::Duration(3.0).sleep();

        ros::spinOnce();
    }

    // 先读取一些飞控的数据
    for(int i=0;i<10;i++)
    {
        ros::spinOnce();
        rate.sleep();
    }

    pos_des_prev[0] = drone_pos[0];
    pos_des_prev[1] = drone_pos[1];
    pos_des_prev[2] = drone_pos[2];

    ros::Duration(3.0).sleep();

    while (ros::ok())
    {
        //回调
        ros::spinOnce();

        Command_Now.header.stamp                    = ros::Time::now();
        Command_Now.Command_ID                      = Command_Now.Command_ID + 1;

        printf_result();

        distance_to_setpoint = pos_body_frame.norm();
        if(!is_detected)
        {
            Command_Now.Mode = prometheus_msgs::ControlCommand::Hold;
            pos_des_prev[0] = drone_pos[0];
            pos_des_prev[1] = drone_pos[1];
            pos_des_prev[2] = drone_pos[2];
            cout <<"[object_tracking]: Lost the Target "<< endl;
            pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Lost the Target.");
        }else
        {
            cout <<"[object_tracking]: Tracking the Target, distance_to_setpoint : "<< distance_to_setpoint << " [m] " << endl;
            Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
            Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
            Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;   //xy velocity z position

            Eigen::Vector3f vel_command;
            vel_command[0] = kpx_track * (pos_body_enu_frame[0] - tracking_delta[0]);
            vel_command[1] = kpy_track * (pos_body_enu_frame[1] - tracking_delta[1]);
            vel_command[2] = kpz_track * (pos_body_enu_frame[2] - tracking_delta[2]);

            for (int i=0; i<3; i++)
            {
                Command_Now.Reference_State.position_ref[i] = pos_des_prev[i] + vel_command[i]* 0.05;
            }
            Command_Now.Reference_State.yaw_ref             = start_yaw/180*3.1415926;

            for (int i=0; i<3; i++)
            {
                pos_des_prev[i] = Command_Now.Reference_State.position_ref[i];
            }
        }

        //Publish
        Command_Now.header.stamp = ros::Time::now();
        Command_Now.Command_ID   = Command_Now.Command_ID + 1;
        Command_Now.source = NODE_NAME;
        command_pub.publish(Command_Now);

        rate.sleep();
    }

    return 0;

}

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 <<">>>>>>>>>>>>>>>>>>>>>>>>>>>Obeject Tracking<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;

    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Vision State<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    if(is_detected)
    {
        cout << "is_detected: ture" <<endl;
    }else
    {
        cout << "is_detected: false" <<endl;
    }

    cout << "Detection_raw: " << Detection_raw.position[0] << " [m] "<< Detection_raw.position[1] << " [m] "<< Detection_raw.position[2] << " [m] "<<endl;
    cout << "Detection_raw: " << Detection_raw.attitude[2]/3.1415926 *180 << " [du] "<<endl;

    cout << "pos_body_frame: " << pos_body_frame[0] << " [m] "<< pos_body_frame[1] << " [m] "<< pos_body_frame[2] << " [m] "<<endl;

    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>Land Control State<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    cout << "pos_des: " << Command_Now.Reference_State.position_ref[0] << " [m] "<< Command_Now.Reference_State.position_ref[1] << " [m] "<< Command_Now.Reference_State.position_ref[2] << " [m] "<<endl;
}

void printf_param()
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    cout << "Thres_vision : "<< Thres_vision << endl;

    cout << "tracking_delta_x : "<< tracking_delta[0] << endl;
    cout << "tracking_delta_y : "<< tracking_delta[1] << endl;
    cout << "tracking_delta_z : "<< tracking_delta[2] << endl;

    cout << "kpx_track : "<< kpx_track << endl;
    cout << "kpy_track : "<< kpy_track << endl;
    cout << "kpz_track : "<< kpz_track << endl;
    cout << "start_point_x : "<< start_point_x << endl;
    cout << "start_point_y : "<< start_point_y << endl;
    cout << "start_point_z : "<< start_point_z << endl;
}


/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8
// test_iterators.cpp

// (C) Copyright 2002 Robert Ramey - http://www.rrsd.com .
// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <algorithm>
#include <list>

#if (defined _MSC_VER) && (_MSC_VER == 1200)
#  pragma warning (disable : 4786) // too long name, harmless warning
#endif

#include <cstdlib>
#include <cstddef> // size_t

#include <boost/config.hpp>
#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{
    using ::rand;
    using ::size_t;
}
#endif

#include <serialization/pfto.hpp>

#include <archive/iterators/binary_from_base64.hpp>
#include <archive/iterators/base64_from_binary.hpp>
#include <archive/iterators/insert_linebreaks.hpp>
#include <archive/iterators/remove_whitespace.hpp>
#include <archive/iterators/transform_width.hpp>

#include <I3Test.h>

#include <iostream>

template<typename CharType>
void test_base64(unsigned int size){
    CharType rawdata[150];
    CharType * rptr;
    for(rptr = rawdata + size; rptr-- > rawdata;)
        *rptr = static_cast<CharType>(std::rand()& 0xff);

    // convert to base64
    typedef std::list<CharType> text_base64_type;
    text_base64_type text_base64;

    typedef
        icecube::archive::iterators::insert_linebreaks<
            icecube::archive::iterators::base64_from_binary<
                icecube::archive::iterators::transform_width<
                    CharType *
                    ,6
                    ,sizeof(CharType) * 8
                >
            >
            ,76
        >
        translate_out;

    std::copy(
        translate_out(I3_MAKE_PFTO_WRAPPER(static_cast<CharType *>(rawdata))),
        translate_out(I3_MAKE_PFTO_WRAPPER(rawdata + size)),
        std::back_inserter(text_base64)
    );

    // convert from base64 to binary and compare with the original
    typedef
        icecube::archive::iterators::transform_width<
            icecube::archive::iterators::binary_from_base64<
                icecube::archive::iterators::remove_whitespace<
                    typename text_base64_type::iterator
                >
            >,
            sizeof(CharType) * 8,
            6
        > translate_in;

    ENSURE(
        std::equal(
            rawdata,
            rawdata + size,
            translate_in(I3_MAKE_PFTO_WRAPPER(text_base64.begin()))
        )
    );

}

TEST_GROUP(test_iterators_base64)

TEST(test_iterators_base64){
    test_base64<char>(1);
    test_base64<char>(2);
    test_base64<char>(3);
    test_base64<char>(4);
    test_base64<char>(150);
    #ifndef BOOST_NO_CWCHAR
    test_base64<wchar_t>(1);
    test_base64<wchar_t>(2);
    test_base64<wchar_t>(3);
    test_base64<wchar_t>(4);
    test_base64<wchar_t>(150);
    #endif
}


// Copyright 2015-2020 The ALMA Project Developers
//
// 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.

/// @file
/// Compute the thermal conductivity of Si under the RTA
/// after reading all the relevant data from an HDF5 file
/// and check the result.

#include <iostream>
#include <functional>
#include <boost/filesystem.hpp>
#include <boost/mpi.hpp>
#include "gtest/gtest.h"
#include <utilities.hpp>
#include <vasp_io.hpp>
#include <qpoint_grid.hpp>
#include <processes.hpp>
#include <bulk_properties.hpp>
#include <bulk_hdf5.hpp>

TEST(read_hdf5_case, read_hdf5_test) {
    boost::mpi::environment env;

    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto si_dir = basedir / boost::filesystem::path("Si");
    auto hdf5_path = si_dir / boost::filesystem::path("Si_threeph.h5");
    boost::mpi::communicator world;
    auto my_id = world.rank();

    auto hdf5_data = alma::load_bulk_hdf5(hdf5_path.string().c_str(), world);
    auto description = std::get<0>(hdf5_data);
    auto poscar = std::move(std::get<1>(hdf5_data));
    auto syms = std::move(std::get<2>(hdf5_data));
    auto grid = std::move(std::get<3>(hdf5_data));
    auto processes = std::move(std::get<4>(hdf5_data));

    double reference[] = {262.594,
                          188.086,
                          147.672,
                          122.172,
                          104.504,
                          91.4766,
                          81.4379,
                          73.4452,
                          66.9194,
                          61.4835};

    std::vector<double> Ts;

    for (auto i = 0; i < 10; ++i)
        Ts.emplace_back(200. + 50. * i);

    auto pos = 0;

    for (auto T : Ts) {
        Eigen::ArrayXXd total_w0(
            alma::calc_w0_threeph(*grid, *processes, T, world));

        if (my_id == 0) {
            auto kappa = alma::calc_kappa(*poscar, *grid, total_w0, T);
            EXPECT_TRUE(
                alma::almost_equal(reference[pos], kappa(0, 0), 1e-6, 5e-3));
            EXPECT_TRUE(
                alma::almost_equal(reference[pos], kappa(1, 1), 1e-6, 5e-3));
            EXPECT_TRUE(
                alma::almost_equal(reference[pos], kappa(2, 2), 1e-6, 5e-3));
            ++pos;
        }
    }
}


// This file is part of the dune-xt project:
//   https://zivgitlab.uni-muenster.de/ag-ohlberger/dune-community/dune-xt
// Copyright 2009-2021 dune-xt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Barbara Verfürth (2015)
//   Felix Schindler  (2014 - 2018, 2020)
//   René Fritze      (2014 - 2016, 2018 - 2020)
//   Tobias Leibner   (2014 - 2015, 2017 - 2020)

#ifndef DUNE_XT_LA_CONTAINER_EIGEN_SPARSE_HH
#define DUNE_XT_LA_CONTAINER_EIGEN_SPARSE_HH

#include <memory>
#include <type_traits>
#include <utility>
#include <vector>
#include <complex>
#include <mutex>

#include <boost/numeric/conversion/cast.hpp>

#if HAVE_EIGEN
#  include <dune/xt/common/disable_warnings.hh>
#  include <Eigen/SparseCore>
#  include <dune/xt/common/reenable_warnings.hh>
#endif

#include <dune/common/typetraits.hh>
#include <dune/common/ftraits.hh>

#include <dune/xt/common/crtp.hh>
#include <dune/xt/common/exceptions.hh>
#include <dune/xt/common/float_cmp.hh>
#include <dune/xt/common/math.hh>
#include <dune/xt/common/numeric_cast.hh>

#include "dune/xt/la/container/interfaces.hh"
#include "dune/xt/la/container/pattern.hh"

#include "dense.hh"

namespace Dune::XT {
namespace LA {

// forwards
template <class ScalarType>
class EigenRowMajorSparseMatrix;

class EigenMatrixInterfaceDynamic
{};

#if HAVE_EIGEN

namespace internal {

/**
 * \brief Traits for EigenRowMajorSparseMatrix.
 */
template <class ScalarImp = double>
class EigenRowMajorSparseMatrixTraits
  : public MatrixTraitsBase<ScalarImp,
                            EigenRowMajorSparseMatrix<ScalarImp>,
                            ::Eigen::SparseMatrix<ScalarImp, ::Eigen::RowMajor>,
                            Backends::eigen_sparse,
                            Backends::eigen_dense,
                            true>
{};

} // namespace internal

/**
 * \brief A sparse matrix implementation of the MatrixInterface with row major memory layout.
 */
template <class ScalarImp = double>
class EigenRowMajorSparseMatrix
  : public MatrixInterface<internal::EigenRowMajorSparseMatrixTraits<ScalarImp>, ScalarImp>
  , public ProvidesBackend<internal::EigenRowMajorSparseMatrixTraits<ScalarImp>>
{

  using ThisType = EigenRowMajorSparseMatrix;
  using InterfaceType = MatrixInterface<internal::EigenRowMajorSparseMatrixTraits<ScalarImp>, ScalarImp>;

public:
  using typename InterfaceType::RealType;
  using typename InterfaceType::ScalarType;
  using Traits = typename InterfaceType::Traits;
  using typename ProvidesBackend<Traits>::BackendType;

private:
  using MutexesType = typename Traits::MutexesType;
  using EIGEN_size_t = typename BackendType::Index;

public:
  /**
   * \brief This is the constructor of interest which creates a sparse matrix.
   */
  EigenRowMajorSparseMatrix(const size_t rr,
                            const size_t cc,
                            const SparsityPatternDefault& pattern_in,
                            const size_t num_mutexes = 1)
    : backend_(
        std::make_shared<BackendType>(Common::numeric_cast<EIGEN_size_t>(rr), Common::numeric_cast<EIGEN_size_t>(cc)))
    , mutexes_(std::make_unique<MutexesType>(num_mutexes))
  {
    if (rr > 0 && cc > 0) {
      if (size_t(pattern_in.size()) != rr)
        DUNE_THROW(Common::Exceptions::shapes_do_not_match,
                   "The size of the pattern (" << pattern_in.size() << ") does not match the number of rows of this ("
                                               << rr << ")!");
      for (size_t row = 0; row < size_t(pattern_in.size()); ++row) {
        backend_->startVec(static_cast<EIGEN_size_t>(row));
        const auto& columns = pattern_in.inner(row);
        for (const auto& column : columns) {
#  ifndef NDEBUG
          if (column >= cc)
            DUNE_THROW(Common::Exceptions::shapes_do_not_match,
                       "The size of row " << row << " of the pattern does not match the number of columns of this ("
                                          << cc << ")!");
#  endif // NDEBUG
          backend_->insertBackByOuterInner(static_cast<EIGEN_size_t>(row), static_cast<EIGEN_size_t>(column));
        }
        // create entry (insertBackByOuterInner() can not handle empty rows)
        if (columns.empty())
          backend_->insertBackByOuterInner(static_cast<EIGEN_size_t>(row), 0);
      }
      backend_->finalize();
      backend_->makeCompressed();
    }
  } // EigenRowMajorSparseMatrix(...)

  explicit EigenRowMajorSparseMatrix(const size_t rr = 0, const size_t cc = 0, const size_t num_mutexes = 1)
    : backend_(
        std::make_shared<BackendType>(Common::numeric_cast<EIGEN_size_t>(rr), Common::numeric_cast<EIGEN_size_t>(cc)))
    , mutexes_(std::make_unique<MutexesType>(num_mutexes))
  {}

  explicit EigenRowMajorSparseMatrix(const size_t rr,
                                     const size_t cc,
                                     const ScalarType& val,
                                     const size_t num_mutexes = 1)
    : backend_(
        std::make_shared<BackendType>(Common::numeric_cast<EIGEN_size_t>(rr), Common::numeric_cast<EIGEN_size_t>(cc)))
    , mutexes_(std::make_unique<MutexesType>(num_mutexes))
  {
    if (val != 0.) {
      for (size_t ii = 0; ii < rows(); ++ii)
        for (size_t jj = 0; jj < cols(); ++jj) {
          backend_->insert(ii, jj);
          set_entry(ii, jj, val);
        }
    }
  }

  EigenRowMajorSparseMatrix(const ThisType& other)
    : backend_(std::make_shared<BackendType>(*other.backend_))
    , mutexes_(std::make_unique<MutexesType>(other.mutexes_->size()))
  {}

  explicit EigenRowMajorSparseMatrix(const BackendType& mat,
                                     const bool prune = false,
                                     const typename Common::FloatCmp::DefaultEpsilon<ScalarType>::Type eps =
                                         Common::FloatCmp::DefaultEpsilon<ScalarType>::value(),
                                     const size_t num_mutexes = 1)
    : mutexes_(std::make_unique<MutexesType>(num_mutexes))
  {
    if (prune) {
      // we do this here instead of using pattern(true), since we can build the triplets along the way which is more
      // efficient
      using TripletType = ::Eigen::Triplet<ScalarType>;
      const ScalarType zero(0);
      std::vector<TripletType> triplets;
      triplets.reserve(mat.nonZeros());
      for (EIGEN_size_t row = 0; row < mat.outerSize(); ++row) {
        for (typename BackendType::InnerIterator row_it(mat, row); row_it; ++row_it) {
          const EIGEN_size_t col = row_it.col();
          const auto val = mat.coeff(row, col);
          if (Common::FloatCmp::ne<Common::FloatCmp::Style::absolute>(val, zero, eps))
            triplets.emplace_back(row, col, val);
        }
      }
      backend_ = std::make_shared<BackendType>(mat.rows(), mat.cols());
      backend_->setFromTriplets(triplets.begin(), triplets.end());
    } else
      backend_ = std::make_shared<BackendType>(mat);
  } // EigenRowMajorSparseMatrix(...)

  /**
   *  \note Takes ownership of backend_ptr in the sense that you must not delete it afterwards!
   */
  explicit EigenRowMajorSparseMatrix(BackendType* backend_ptr, const size_t num_mutexes = 1)
    : backend_(backend_ptr)
    , mutexes_(std::make_unique<MutexesType>(num_mutexes))
  {}

  explicit EigenRowMajorSparseMatrix(std::shared_ptr<BackendType> backend_ptr, const size_t num_mutexes = 1)
    : backend_(std::move(backend_ptr))
    , mutexes_(std::make_unique<MutexesType>(num_mutexes))
  {}

  ThisType& operator=(const ThisType& other)
  {
    if (this != &other) {
      *backend_ = *other.backend_;
      mutexes_ = std::make_unique<MutexesType>(other.mutexes_->size());
    }
    return *this;
  }

  /**
   *  \note Does a deep copy.
   */
  ThisType& operator=(const BackendType& other)
  {
    backend_ = std::make_shared<BackendType>(other);
    return *this;
  }

  /// \name Required by the ProvidesBackend interface.
  /// \{

  BackendType& backend()
  {
    return *backend_;
  }

  const BackendType& backend() const
  {
    return *backend_;
  }

  /// \}
  /// \name Required by ContainerInterface.
  /// \{

  ThisType copy() const
  {
    return ThisType(*backend_);
  }

  void scal(const ScalarType& alpha)
  {
    [[maybe_unused]] const internal::VectorLockGuard guard(*mutexes_);
    backend() *= alpha;
  }

  void set_to_zero()
  {
    backend() *= 0.;
  }

  void axpy(const ScalarType& alpha, const ThisType& xx)
  {
    if (!has_equal_shape(xx))
      DUNE_THROW(Common::Exceptions::shapes_do_not_match,
                 "The shape of xx (" << xx.rows() << "x" << xx.cols() << ") does not match the shape of this ("
                                     << rows() << "x" << cols() << ")!");
    [[maybe_unused]] const internal::VectorLockGuard guard(*mutexes_);
    backend() += alpha * xx.backend();
  } // ... axpy(...)

  bool has_equal_shape(const ThisType& other) const
  {
    return (rows() == other.rows()) && (cols() == other.cols());
  }

  /// \}
  /// \name Required by MatrixInterface.
  /// \{

  inline size_t rows() const
  {
    return backend_->rows();
  }

  inline size_t cols() const
  {
    return backend_->cols();
  }

  template <class T1, class T2>
  inline void mv(const EigenBaseVector<T1, ScalarType>& xx, EigenBaseVector<T2, ScalarType>& yy) const
  {
    yy.backend().transpose() = backend() * xx.backend();
  }

  template <class V1, class V2>
  inline std::enable_if_t<XT::Common::is_vector<V1>::value && XT::Common::is_vector<V2>::value
                              && (!is_eigen_vector<V1>::value || !is_eigen_vector<V2>::value),
                          void>
  mv(const V1& xx, V2& yy) const
  {
    EigenDenseVector<ScalarType> xx_eigen(xx.size()), yy_eigen(yy.size());
    for (size_t ii = 0; ii < xx.size(); ++ii)
      xx_eigen.set_entry(ii, XT::Common::VectorAbstraction<V1>::get_entry(xx, ii));
    mv(xx_eigen, yy_eigen);
    for (size_t ii = 0; ii < yy.size(); ++ii)
      XT::Common::VectorAbstraction<V2>::set_entry(yy, ii, yy_eigen[ii]);
  }

  template <class T1, class T2>
  inline void mtv(const EigenBaseVector<T1, ScalarType>& xx, EigenBaseVector<T2, ScalarType>& yy) const
  {
    yy.backend().transpose() = backend().transpose() * xx.backend();
  }

  template <class V1, class V2>
  inline std::enable_if_t<XT::Common::is_vector<V1>::value && XT::Common::is_vector<V2>::value
                              && !is_eigen_vector<V1>::value,
                          void>
  mtv(const V1& xx, V2& yy) const
  {
    EigenDenseVector<ScalarType> xx_eigen(xx.size()), yy_eigen(yy.size());
    for (size_t ii = 0; ii < xx.size(); ++ii)
      xx_eigen.set_entry(ii, XT::Common::VectorAbstraction<V1>::get_entry(xx, ii));
    mtv(xx_eigen, yy_eigen);
    for (size_t ii = 0; ii < yy.size(); ++ii)
      XT::Common::VectorAbstraction<V2>::set_entry(yy, ii, yy_eigen[ii]);
  }

  void add_to_entry(const size_t ii, const size_t jj, const ScalarType& value)
  {
    assert(these_are_valid_indices(ii, jj));
    [[maybe_unused]] const internal::LockGuard lock(*mutexes_, ii, rows());
    backend().coeffRef(static_cast<EIGEN_size_t>(ii), static_cast<EIGEN_size_t>(jj)) += value;
  }

  void set_entry(const size_t ii, const size_t jj, const ScalarType& value)
  {
    assert(these_are_valid_indices(ii, jj));
    backend().coeffRef(static_cast<EIGEN_size_t>(ii), static_cast<EIGEN_size_t>(jj)) = value;
  }

  ScalarType get_entry(const size_t ii, const size_t jj) const
  {
    assert(ii < rows());
    assert(jj < cols());
    return backend().coeff(static_cast<EIGEN_size_t>(ii), static_cast<EIGEN_size_t>(jj));
  }

  void clear_row(const size_t ii)
  {
    if (ii >= rows())
      DUNE_THROW(Common::Exceptions::index_out_of_range,
                 "Given ii (" << ii << ") is larger than the rows of this (" << rows() << ")!");
    backend().row(static_cast<EIGEN_size_t>(ii)) *= ScalarType(0);
  }

  void clear_col(const size_t jj)
  {
    if (jj >= cols())
      DUNE_THROW(Common::Exceptions::index_out_of_range,
                 "Given jj (" << jj << ") is larger than the cols of this (" << cols() << ")!");
    for (size_t row = 0; static_cast<EIGEN_size_t>(row) < backend().outerSize(); ++row) {
      for (typename BackendType::InnerIterator row_it(backend(), static_cast<EIGEN_size_t>(row)); row_it; ++row_it) {
        const size_t col = row_it.col();
        if (col == jj) {
          backend().coeffRef(static_cast<EIGEN_size_t>(row), static_cast<EIGEN_size_t>(jj)) = ScalarType(0);
          break;
        }
        if (col > jj)
          break;
      }
    }
  } // ... clear_col(...)

  void unit_row(const size_t ii)
  {
    if (ii >= cols())
      DUNE_THROW(Common::Exceptions::index_out_of_range,
                 "Given ii (" << ii << ") is larger than the cols of this (" << cols() << ")!");
    if (ii >= rows())
      DUNE_THROW(Common::Exceptions::index_out_of_range,
                 "Given ii (" << ii << ") is larger than the rows of this (" << rows() << ")!");
    if (!these_are_valid_indices(ii, ii))
      DUNE_THROW(Common::Exceptions::index_out_of_range,
                 "Diagonal entry (" << ii << ", " << ii << ") is not contained in the sparsity pattern!");
    backend().row(static_cast<EIGEN_size_t>(ii)) *= ScalarType(0);
    backend().coeffRef(static_cast<EIGEN_size_t>(ii), static_cast<EIGEN_size_t>(ii)) = ScalarType(1);
  } // ... unit_row(...)

  void unit_col(const size_t jj)
  {
    if (jj >= cols())
      DUNE_THROW(Common::Exceptions::index_out_of_range,
                 "Given jj (" << jj << ") is larger than the cols of this (" << cols() << ")!");
    if (jj >= rows())
      DUNE_THROW(Common::Exceptions::index_out_of_range,
                 "Given jj (" << jj << ") is larger than the rows of this (" << rows() << ")!");
    for (size_t row = 0; static_cast<EIGEN_size_t>(row) < backend().outerSize(); ++row) {
      for (typename BackendType::InnerIterator row_it(backend(), static_cast<EIGEN_size_t>(row)); row_it; ++row_it) {
        const size_t col = row_it.col();
        if (col == jj) {
          if (col == row)
            backend().coeffRef(static_cast<EIGEN_size_t>(row), static_cast<EIGEN_size_t>(col)) = ScalarType(1);
          else
            backend().coeffRef(static_cast<EIGEN_size_t>(row), static_cast<EIGEN_size_t>(jj)) = ScalarType(0);
          break;
        }
        if (col > jj)
          break;
      }
    }
  } // ... unit_col(...)

  bool valid() const
  {
    // iterate over non-zero entries
    using InnerIterator = typename BackendType::InnerIterator;
    for (EIGEN_size_t ii = 0; ii < backend().outerSize(); ++ii) {
      for (InnerIterator it(backend(), ii); it; ++it) {
        if (Common::isnan(it.value()) || Common::isinf(it.value()))
          return false;
      }
    }
    return true;
  }

  size_t non_zeros() const final
  {
    return backend_->nonZeros();
  }

  SparsityPatternDefault
  pattern(const bool prune = false,
          const ScalarType eps = Common::FloatCmp::DefaultEpsilon<ScalarType>::value()) const override
  {
    SparsityPatternDefault ret(rows());
    const auto zero = typename Common::FloatCmp::DefaultEpsilon<ScalarType>::Type(0);
    if (prune) {
      for (EIGEN_size_t row = 0; row < backend().outerSize(); ++row) {
        for (typename BackendType::InnerIterator row_it(backend(), row); row_it; ++row_it) {
          const EIGEN_size_t col = row_it.col();
          const auto val = backend().coeff(row, col);
          if (Common::FloatCmp::ne(val, zero, eps))
            ret.insert(static_cast<size_t>(row), static_cast<size_t>(col));
        }
      }
    } else {
      for (EIGEN_size_t row = 0; row < backend().outerSize(); ++row) {
        for (typename BackendType::InnerIterator row_it(backend(), row); row_it; ++row_it)
          ret.insert(static_cast<size_t>(row), static_cast<size_t>(row_it.col()));
      }
    }
    ret.sort();
    return ret;
  } // ... pattern(...)

  ThisType pruned(const ScalarType eps = Common::FloatCmp::DefaultEpsilon<ScalarType>::value()) const final
  {
    return ThisType(*backend_, true, eps);
  }

  /// \}

  ScalarType* entries()
  {
    backend_->makeCompressed();
    return backend().valuePtr();
  }

  const ScalarType* entries() const
  {
    backend_->makeCompressed();
    return backend().valuePtr();
  }

  int* outer_index_ptr()
  {
    backend_->makeCompressed();
    return backend().outerIndexPtr();
  }

  const int* outer_index_ptr() const
  {
    return backend().outerIndexPtr();
    backend_->makeCompressed();
  }

  int* inner_index_ptr()
  {
    backend_->makeCompressed();
    return backend().innerIndexPtr();
  }

  const int* inner_index_ptr() const
  {
    backend_->makeCompressed();
    return backend().innerIndexPtr();
  }

  void set_num_mutexes(const size_t num_mutexes)
  {
    mutexes_ = std::make_unique<MutexesType>(num_mutexes);
  }

  using InterfaceType::operator+;
  using InterfaceType::operator-;
  using InterfaceType::operator+=;
  using InterfaceType::operator-=;

private:
  bool these_are_valid_indices(const size_t ii, const size_t jj) const
  {
    if (ii >= rows())
      return false;
    if (jj >= cols())
      return false;
    for (size_t row = ii; static_cast<EIGEN_size_t>(row) < backend().outerSize(); ++row) {
      for (typename BackendType::InnerIterator row_it(backend(), static_cast<EIGEN_size_t>(row)); row_it; ++row_it) {
        const size_t col = row_it.col();
        if ((ii == row) && (jj == col))
          return true;
        if ((row > ii) && (col > jj))
          return false;
      }
    }
    return false;
  } // ... these_are_valid_indices(...)

  std::shared_ptr<BackendType> backend_;
  std::unique_ptr<MutexesType> mutexes_;
}; // class EigenRowMajorSparseMatrix

#else // HAVE_EIGEN

template <class ScalarImp>
class EigenRowMajorSparseMatrix
{
  static_assert(AlwaysFalse<ScalarImp>::value, "You are missing Eigen!");
};

#endif // HAVE_EIGEN

} // namespace LA
namespace Common {

#if HAVE_EIGEN

template <class T>
struct MatrixAbstraction<LA::EigenRowMajorSparseMatrix<T>>
  : public LA::internal::MatrixAbstractionBase<LA::EigenRowMajorSparseMatrix<T>>
{
  using BaseType = LA::internal::MatrixAbstractionBase<LA::EigenRowMajorSparseMatrix<T>>;

  static constexpr Common::StorageLayout storage_layout = Common::StorageLayout::csr;

  template <size_t rows = BaseType::static_rows, size_t cols = BaseType::static_cols, class FieldType = T>
  using MatrixTypeTemplate = LA::EigenRowMajorSparseMatrix<FieldType>;
};

#endif // HAVE_EIGEN

} // namespace Common
} // namespace Dune::XT

// begin: this is what we need for the lib
#if DUNE_XT_WITH_PYTHON_BINDINGS && HAVE_EIGEN

extern template class Dune::XT::LA::EigenRowMajorSparseMatrix<double>;
// extern template void Dune::XT::LA::EigenRowMajorSparseMatrix<double>::mv(const
// Dune::XT::LA::EigenDenseVector<double>&,
//                                                                         Dune::XT::LA::EigenDenseVector<double>&)
//                                                                         const;
// extern template void
// Dune::XT::LA::EigenRowMajorSparseMatrix<double>::mv(const Dune::XT::LA::EigenMappedDenseVector<double>&,
//                                                    Dune::XT::LA::EigenMappedDenseVector<double>&) const;
// extern template void
// Dune::XT::LA::EigenRowMajorSparseMatrix<double>::mv(const Dune::XT::LA::EigenDenseVector<double>&,
//                                                    Dune::XT::LA::EigenMappedDenseVector<double>&) const;
// extern template void
// Dune::XT::LA::EigenRowMajorSparseMatrix<double>::mv(const Dune::XT::LA::EigenMappedDenseVector<double>&,
//                                                    Dune::XT::LA::EigenDenseVector<double>&) const;

#endif // DUNE_XT_WITH_PYTHON_BINDINGS && HAVE_EIGEN
// end: this is what we need for the lib

#endif // DUNE_XT_LA_CONTAINER_EIGEN_SPARSE_HH


/**
* @ingroup idyntree_tutorials
*
* A tutorial on how to use the KinDynComputations class
* with Eigen data structures.
*
* \author Silvio Traversaro
*
* CopyPolicy: Released under the terms of LGPL 2.0+ or later
*/

#include <ros/ros.h>

// C headers
#include <cstdlib>

// Eigen headers
#include <Eigen/Core>

// iDynTree headers
#include <iDynTree/Model/FreeFloatingState.h>
#include <iDynTree/KinDynComputations.h>
#include <iDynTree/ModelIO/ModelLoader.h>

// Helpers function to convert between
// iDynTree datastructures
#include <iDynTree/Core/EigenHelpers.h>
#include <iDynTree/InverseKinematics.h>

/**
 * Struct containing the floating robot state
 * using Eigen data structures.
 */
struct EigenRobotState
{
    void resize(int nrOfInternalDOFs)
    {
        jointPos.resize(nrOfInternalDOFs);
        jointVel.resize(nrOfInternalDOFs);
    }

    void random()
    {
        world_H_base.setIdentity();
        jointPos.setRandom();
        baseVel.setRandom();
        jointVel.setRandom();
        gravity[0] = 0;
        gravity[1] = 0;
        gravity[2] = -9.8;
    }

    Eigen::Matrix4d world_H_base;
    Eigen::VectorXd jointPos;
    Eigen::Matrix<double,6,1> baseVel;
    Eigen::VectorXd jointVel;
    Eigen::Vector3d gravity;
};

/**
 * Struct containing the floating robot state
 * using iDynTree data structures.
 * For the semantics of this structures,
 * see KinDynComputation::setRobotState method.
 */
struct iDynTreeRobotState
{
    void resize(int nrOfInternalDOFs)
    {
        jointPos.resize(nrOfInternalDOFs);
        jointVel.resize(nrOfInternalDOFs);
    }

    iDynTree::Transform world_H_base;
    iDynTree::VectorDynSize jointPos;
    iDynTree::Twist         baseVel;
    iDynTree::VectorDynSize jointVel;
    iDynTree::Vector3       gravity;
};

struct EigenRobotAcceleration
{
    void resize(int nrOfInternalDOFs)
    {
        jointAcc.resize(nrOfInternalDOFs);
    }

    void random()
    {
        baseAcc.setRandom();
        jointAcc.setRandom();
    }

    Eigen::Matrix<double,6,1> baseAcc;
    Eigen::VectorXd jointAcc;
};

struct iDynTreeRobotAcceleration
{
    void resize(int nrOfInternalDOFs)
    {
        jointAcc.resize(nrOfInternalDOFs);
    }

    iDynTree::Vector6 baseAcc;
    iDynTree::VectorDynSize jointAcc;
};

/*****************************************************************/
int main(int argc, char *argv[])
{
    if( argc != 2 )
    {
        std::cerr << "KinDynComputationsWithEigen usage: KinDynComputationsWithEigen ./path/to/modelName.urdf" << std::endl;
        return EXIT_FAILURE;
    }

    std::string modelFile = argv[1];

    // Helper class to load the model from an external format
    iDynTree::ModelLoader mdlLoader;

    bool ok = mdlLoader.loadModelFromFile(modelFile);

    if( !ok )
    {
        std::cerr << "KinDynComputationsWithEigen: impossible to load model from " << modelFile << std::endl;
        return EXIT_FAILURE;
    }

    // Create a KinDynComputations class from the model
    iDynTree::KinDynComputations kinDynComp;
    ok = kinDynComp.loadRobotModel(mdlLoader.model());

    if( !ok )
    {
        std::cerr << "KinDynComputationsWithEigen: impossible to load the following model in a KinDynComputations class:" << std::endl
                  << mdlLoader.model().toString() << std::endl;
        return EXIT_FAILURE;
    }

    const iDynTree::Model & model = kinDynComp.model();

    // Now we create the robot state in Eigen datastructures
    EigenRobotState eigRobotState;
    eigRobotState.resize(model.getNrOfDOFs());
    eigRobotState.random();

    // Now we convert the Eigen data structures in iDynTree ones
    iDynTreeRobotState idynRobotState;
    idynRobotState.resize(model.getNrOfDOFs());
    iDynTree::fromEigen(idynRobotState.world_H_base,eigRobotState.world_H_base);
    iDynTree::toEigen(idynRobotState.jointPos) = eigRobotState.jointPos;
    iDynTree::fromEigen(idynRobotState.baseVel,eigRobotState.baseVel);
    toEigen(idynRobotState.jointVel) = eigRobotState.jointVel;
    toEigen(idynRobotState.gravity)  = eigRobotState.gravity;

    // Now we create the KinDynComputations class, so we can set the state
    kinDynComp.setRobotState(idynRobotState.world_H_base,idynRobotState.jointPos,
                             idynRobotState.baseVel,idynRobotState.jointVel,idynRobotState.gravity);

    // Once we called the setRobotState, we can call all the methods of KinDynComputations

    // For methods returning fixed size vector/matrices, the conversion to eigen types is trivial
    Eigen::Vector3d com = iDynTree::toEigen(kinDynComp.getCenterOfMassPosition());

    // If you are interested in a frame with a given name, you can obtain its associated index with
    // the appropriate method
    std::string arbitraryFrameName = model.getFrameName(model.getNrOfFrames()/2);

    iDynTree::FrameIndex arbitraryFrameIndex = model.getFrameIndex(arbitraryFrameName);

    Eigen::Matrix4d world_H_arbitraryFrame = iDynTree::toEigen(kinDynComp.getWorldTransform(arbitraryFrameIndex).asHomogeneousTransform());

    // You can also get quantities directly with their name, but clearly this is less efficient
    std::string anotherArbitraryFrameName = model.getFrameName(model.getNrOfFrames()/3);

    Eigen::Matrix4d arbitraryFrame_H_anotherArbitraryFrame = iDynTree::toEigen(kinDynComp.getRelativeTransform(arbitraryFrameName,anotherArbitraryFrameName).asHomogeneousTransform());

    // More complex quantities (such as jacobians and matrices) need to be handled in a different way for efficency reasons
    iDynTree::FreeFloatingMassMatrix idynMassMatrix(model);
    kinDynComp.getFreeFloatingMassMatrix(idynMassMatrix);
    Eigen::MatrixXd eigMassMatrix = iDynTree::toEigen(idynMassMatrix);

    ROS_INFO_STREAM("M:\n" <<eigMassMatrix);

    iDynTree::FrameFreeFloatingJacobian idynJacobian(model);
    kinDynComp.getFreeFloatingMassMatrix(idynMassMatrix);
    Eigen::MatrixXd eigJacobian = iDynTree::toEigen(idynJacobian);

    iDynTree::MatrixDynSize idynCOMJacobian(3,model.getNrOfDOFs()+6);
    kinDynComp.getCenterOfMassJacobian(idynCOMJacobian);
    Eigen::MatrixXd eigCOMJacobian = iDynTree::toEigen(idynCOMJacobian);

    std::cerr << "COM Jacobian: " << std::endl;
    std::cerr << eigCOMJacobian << std::endl;

    // For inverse dynamics, we need to pass the acceleration, of the robot
    // as it is not part of the "state"
    EigenRobotAcceleration eigRobotAcc;
    eigRobotAcc.resize(model.getNrOfDOFs());
    eigRobotAcc.random();

    iDynTreeRobotAcceleration idynRobotAcc;
    idynRobotAcc.resize(model.getNrOfDOFs());
    iDynTree::toEigen(idynRobotAcc.baseAcc) = eigRobotAcc.baseAcc;
    iDynTree::toEigen(idynRobotAcc.jointAcc) = eigRobotAcc.jointAcc;

    // In input to the inverse dynamics we also have external forces (that we assume set to zero)
    iDynTree::LinkNetExternalWrenches extForces(model);
    extForces.zero();

    // The output is a set of generalized torques (joint torques + base wrenches)
    iDynTree::FreeFloatingGeneralizedTorques invDynTrqs(model);

    kinDynComp.inverseDynamics(idynRobotAcc.baseAcc,idynRobotAcc.jointAcc,extForces,invDynTrqs);

    // The output of inv dynamics can be converted easily to eigen vectors
    Eigen::Matrix<double,6,1> baseWrench = iDynTree::toEigen(invDynTrqs.baseWrench());
    Eigen::VectorXd jntTorques = iDynTree::toEigen(invDynTrqs.jointTorques());

    std::cerr << "Inverse dynamics torques: " << std::endl;
    std::cerr << jntTorques << std::endl;

    iDynTree::InverseKinematics ik;
    ik.setModel(model);

    ROS_INFO_STREAM("rotation parameterization " << ik.rotationParametrization());
    Eigen::Quaterniond q(-0.8733046,-0.4871745,0,  0);
    Eigen::Matrix3d rot = q.matrix();
    iDynTree::Rotation r(rot(0,0),rot(0,1),rot(0,2),rot(1,0),rot(1,1),rot(1,2),rot(2,0),rot(2,1),rot(2,2));
    ROS_INFO_STREAM("base trasnform " << model.getFrameTransform(model.getFrameIndex("base")).toString());
    ik.addTarget("base",model.getFrameTransform(model.getFrameIndex("base")));
    ik.addRotationTarget("top",r);
//    model.getJointIndex()
//    ik.setVerbosity(6);
//    ik.getPoseForFrame()
    if(ik.solve()) {
        iDynTree::Transform base_solution;
        iDynTree::VectorDynSize q_star;
        ik.getFullJointsSolution(base_solution,q_star);
        ROS_INFO_STREAM("base solution:" << base_solution.toString() << "\njoint solution: " << q_star.toString());
    }else{
        ROS_FATAL("unable to solve ik");
    }

    return EXIT_SUCCESS;
}


/**
 * @file py_clipper.cpp
 * @brief Python bindings for CLIPPER
 * @author Parker Lusk <plusk@mit.edu>
 * @date 28 January 2021
 */

#include <cstdint>
#include <sstream>

#include <Eigen/Dense>

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>

#include "clipper/clipper.h"
#include "clipper/utils.h"
#include "clipper/invariants/builtins.h"

#include "trampolines.h"

namespace py = pybind11;
using namespace pybind11::literals;

void pybind_invariants(py::module& m)
{
  m.doc() = "Invariants are quantities that do not change under the"
            "transformation between two sets of objects. They are used to"
            "build a consistency graph. Some built-in invariants are provided.";

  using namespace clipper::invariants;

  //
  // Base Invariants
  //

  py::class_<Invariant, PyInvariant<>, std::shared_ptr<Invariant>>(m, "Invariant")
    .def(py::init<>());
  py::class_<PairwiseInvariant, Invariant, PyPairwiseInvariant<>, std::shared_ptr<PairwiseInvariant>>(m, "PairwiseInvariant")
    .def(py::init<>())
    .def("__call__", &clipper::invariants::PairwiseInvariant::operator());

  //
  // Euclidean Distance
  //

  py::class_<EuclideanDistance::Params>(m, "EuclideanDistanceParams")
    .def(py::init<>())
    .def("__repr__", [](const EuclideanDistance::Params &params) {
       std::ostringstream repr;
       repr << "<EuclideanDistanceParams : sigma=" << params.sigma;
       repr << " epsilon=" << params.epsilon;
       repr << " mindist=" << params.mindist << ">";
       return repr.str();
    })
    .def_readwrite("sigma", &clipper::invariants::EuclideanDistance::Params::sigma)
    .def_readwrite("epsilon", &clipper::invariants::EuclideanDistance::Params::epsilon)
    .def_readwrite("mindist", &clipper::invariants::EuclideanDistance::Params::mindist);

  py::class_<EuclideanDistance, PairwiseInvariant, PyPairwiseInvariant<EuclideanDistance>, std::shared_ptr<EuclideanDistance>>(m, "EuclideanDistance")
    .def(py::init<const EuclideanDistance::Params&>());

  //
  // Point-Normal Distance
  //

  py::class_<PointNormalDistance::Params>(m, "PointNormalDistanceParams")
    .def(py::init<>())
    .def("__repr__", [](const PointNormalDistance::Params &params) {
       std::ostringstream repr;
       repr << "<PointNormalDistanceParams : sigp=" << params.sigp;
       repr << " epsp=" << params.epsp << " sign=" << params.sign;
       repr << " epsn=" << params.epsn << ">";
       return repr.str();
    })
    .def_readwrite("sigp", &clipper::invariants::PointNormalDistance::Params::sigp)
    .def_readwrite("epsp", &clipper::invariants::PointNormalDistance::Params::epsp)
    .def_readwrite("sign", &clipper::invariants::PointNormalDistance::Params::sign)
    .def_readwrite("epsn", &clipper::invariants::PointNormalDistance::Params::epsn);

  py::class_<PointNormalDistance, PairwiseInvariant, PyPairwiseInvariant<PointNormalDistance>, std::shared_ptr<PointNormalDistance>>(m, "PointNormalDistance")
    .def(py::init<const PointNormalDistance::Params&>());
}

PYBIND11_MODULE(clipper, m)
{
  m.doc() = "CLIPPER is a graph-theoretic framework for robust data association";
  m.attr("__version__") = PROJECT_VERSION;

  py::module m_invariants = m.def_submodule("invariants");
  pybind_invariants(m_invariants);

  py::class_<clipper::Params>(m, "Params")
    .def(py::init<>())
    .def("__repr__", [](const clipper::Params &params) {
       std::ostringstream repr;
       repr << "<Parameters for CLIPPER dense cluster solver>";
       return repr.str();
    })
    .def_readwrite("tol_u", &clipper::Params::tol_u)
    .def_readwrite("tol_F", &clipper::Params::tol_F)
    .def_readwrite("tol_Fop", &clipper::Params::tol_Fop)
    .def_readwrite("maxiniters", &clipper::Params::maxiniters)
    .def_readwrite("maxoliters", &clipper::Params::maxoliters)
    .def_readwrite("beta", &clipper::Params::beta)
    .def_readwrite("maxlsiters", &clipper::Params::maxlsiters)
    .def_readwrite("eps", &clipper::Params::eps);

  py::class_<clipper::Solution>(m, "Solution")
    .def(py::init<>())
    .def("__repr__", [](const clipper::Solution &soln) {
       std::ostringstream repr;
       repr << "<CLIPPER dense cluster solution>";
       return repr.str();
    })
    .def_readwrite("t", &clipper::Solution::t)
    .def_readwrite("ifinal", &clipper::Solution::ifinal)
    .def_readwrite("nodes", &clipper::Solution::nodes)
    .def_readwrite("u", &clipper::Solution::u)
    .def_readwrite("score", &clipper::Solution::score);

  m.def("create_all_to_all", clipper::createAllToAll,
    "n1"_a, "n2"_a,
    "Create an all-to-all hypothesis for association. Useful for the case of"
    " no prior information or putative associations.");

  m.def("score_pairwise_consistency", [](const clipper::invariants::PairwiseInvariantPtr& invariant,
                        const clipper::invariants::Data& D1, const clipper::invariants::Data& D2,
                        clipper::Association& A) {

    // indicates if calls to invariant can be made in parallel. This is primarily to
    // prevent GIL-related resource deadlocking for derived classes in Python. See also
    // https://github.com/pybind/pybind11/issues/813
    // Python extended c++ classes will inherit from PyPairwiseInvariant
    bool parallelize = (std::dynamic_pointer_cast<PyPairwiseInvariant<>>(invariant)) ? false : true;
    auto ret = clipper::scorePairwiseConsistency(invariant, D1, D2, A, parallelize);
    return ret;

  }, py::call_guard<py::gil_scoped_release>(),
    "invariant"_a, "D1"_a.noconvert(), "D2"_a.noconvert(), "A"_a,
    "Scores consistency between pairs of associations in A");

  m.def(
      "score_sparse_pairwise_consistency",
      [](const clipper::invariants::PairwiseInvariantPtr &invariant,
         const clipper::invariants::Data &D1,
         const clipper::invariants::Data &D2, clipper::Association &A) {
        // indicates if calls to invariant can be made in parallel. This is
        // primarily to prevent GIL-related resource deadlocking for derived
        // classes in Python. See also
        // https://github.com/pybind/pybind11/issues/813
        // Python extended c++ classes will inherit from PyPairwiseInvariant
        bool parallelize =
            (std::dynamic_pointer_cast<PyPairwiseInvariant<>>(invariant))
                ? false
                : true;
        // hacky way
        auto ret = clipper::scoreSparsePairwiseConsistency(*invariant, D1, D2,
                                                           A, parallelize);
        return ret;
      },
      py::call_guard<py::gil_scoped_release>(), "invariant"_a,
      "D1"_a.noconvert(), "D2"_a.noconvert(), "A"_a,
      "Scores sparse consistency between pairs of associations in A");

  m.def("find_dense_cluster",
    py::overload_cast<const Eigen::MatrixXd&, const Eigen::MatrixXd&,
          const clipper::Params&>(clipper::findDenseCluster<Eigen::MatrixXd>),
    "M"_a.noconvert(), "C"_a.noconvert(), "params"_a);

  m.def("find_dense_cluster",
    py::overload_cast<const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::VectorXd&,
          const clipper::Params&>(clipper::findDenseCluster<Eigen::MatrixXd>),
    "M"_a.noconvert(), "C"_a.noconvert(), "u0"_a, "params"_a);

    m.def("find_dense_cluster_of_sparse_graph",
        py::overload_cast<const Eigen::SparseMatrix<double> &,
                          const Eigen::SparseMatrix<double> &,
                          const clipper::Params &>(
            clipper::findDenseClusterOfSparseGraph),
        "M"_a.noconvert(), "C"_a.noconvert(), "params"_a);

  m.def("select_inlier_associations", &clipper::selectInlierAssociations,
    "soln"_a, "A"_a);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex.arithmetic toolbox - floor/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.arithmetic components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 28/11/2010
///
#include <nt2/include/functions/bitwise_cast.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/include/functions/extract.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/floor.hpp>
#include <nt2/include/functions/cos.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/real.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>

#include <nt2/constant/constant.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/include/functions/splat.hpp>

#include <nt2/include/functions/load.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/complex/imaginary.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_imaginary.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>

NT2_TEST_CASE_TPL ( abs_cplx__1_0,  (float))
{
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef std::complex<T>                              cT;
  typedef native<T ,ext_t>                             vT;
  typedef native<cT ,ext_t>                           vcT;
  typedef typename nt2::meta::as_imaginary<T>::type   ciT;
  typedef native<ciT ,ext_t>                         vciT;
  typedef typename nt2::meta::as_dry<T>::type          dT;
  typedef native<dT ,ext_t>                           vdT;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  {
    typedef vcT r_t;
    NT2_TEST_ULP_EQUAL(floor(nt2::splat<vcT>(cT(-1.1, -1.6)))[0], cT(-2, -2),0);
    NT2_TEST_ULP_EQUAL(floor(nt2::splat<vcT>(cT(1.1, 1.6)))[0],  cT(1, 1),0);
  }
  {
    typedef vdT r_t;

    NT2_TEST_ULP_EQUAL(floor(nt2::Inf<vcT>())[0], nt2::Inf<cT>(),0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Minf<vcT>())[0], nt2::Minf<cT>(),0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Mone<vcT>())[0], nt2::Mone<cT>(),0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Nan<vcT>())[0], nt2::Nan<cT>(),0);
    NT2_TEST_ULP_EQUAL(floor(nt2::One<vcT>())[0], nt2::One<cT>(),0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Zero<vcT>())[0], nt2::Zero<cT>(),0);
  }
  {
    typedef vciT r_t;
    NT2_TEST_ULP_EQUAL(floor(nt2::splat<vciT>(ciT(-1.1)))[0], nt2::Mtwo<vciT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::splat<vciT>(ciT(1.1)))[0], nt2::One<vciT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Inf<vciT>())[0], nt2::Inf<vciT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Minf<vciT>())[0], nt2::Minf<vciT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Mone<vciT>())[0], nt2::Mone<vciT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Nan<vciT>())[0], nt2::Nan<vciT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::One<vciT>())[0], nt2::One<vciT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Zero<vciT>())[0], nt2::Zero<vciT>()[0],0);
  }
  {
    typedef vdT r_t;
    NT2_TEST_ULP_EQUAL(floor(nt2::splat<vdT>(dT(-1.1)))[0], nt2::Mtwo<vdT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::splat<vdT>(dT(1.1)))[0], nt2::One<vdT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Inf<vdT>())[0], nt2::Inf<vdT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Minf<vdT>())[0], nt2::Minf<vdT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Mone<vdT>())[0], nt2::Mone<vdT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Nan<vdT>())[0], nt2::Nan<vdT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::One<vdT>())[0], nt2::One<vdT>()[0],0);
    NT2_TEST_ULP_EQUAL(floor(nt2::Zero<vdT>())[0], nt2::Zero<vdT>()[0],0);
  }
} // end of test for floating_


/* Copyright (C) 2012,2013 IBM Corp.
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include <algorithm>   // defines count(...), min(...)

#include "PAlgebra.h"
#include "hypercube.h"
#include "timing.h"

#include <NTL/ZZXFactoring.h>
#include <NTL/GF2EXFactoring.h>
#include <NTL/lzz_pEXFactoring.h>

// polynomials are sorted lexicographically, with the
// constant term being the "most significant"

template<class RX> bool poly_comp(const RX& a, const RX& b)
{
  long na = deg(a) + 1;
  long nb = deg(b) + 1;

  long i = 0;
  while (i < na && i < nb && coeff(a, i) == coeff(b, i)) i++;

  if (i < na && i < nb)
    return coeff(a, i) < coeff(b, i);
  else
    return na < nb;
}

namespace NTL {

// for some weird reason, these need to be in either the std or NTL
// namespace; otherwise, the compiler won't find them...

bool operator<(GF2 a, GF2 b) { return rep(a) < rep(b); }
bool operator<(zz_p a, zz_p b) { return rep(a) < rep(b); }

bool operator<(const GF2X& a, const GF2X& b) { return poly_comp(a, b); }
bool operator<(const zz_pX& a, const zz_pX& b) { return poly_comp(a, b); }

bool operator<(const GF2E& a, const GF2E& b) { return rep(a) < rep(b); }
bool operator<(const zz_pE& a, const zz_pE& b) { return rep(a) < rep(b); }

bool operator<(const GF2EX& a, const GF2EX& b) { return poly_comp(a, b); }
bool operator<(const zz_pEX& a, const zz_pEX& b) { return poly_comp(a, b); }

}

bool PAlgebra::operator==(const PAlgebra& other) const
{
  if (m != other.m) return false;
  if (p != other.p) return false;

  return true;
}

bool PAlgebra::nextExpVector(vector<unsigned long>& buffer) const
{
  // increment the vector in lexicographic order
  if (!isDryRun()) for (long i=gens.size()-1; i>=0; i--) {
    if (i>=(long)buffer.size()) continue; // sanity check
    // increment current index, set all the ones after it to zero
    if (buffer[i] < OrderOf(i)-1) {
      buffer[i]++;
      for (unsigned long j=i+1; j<buffer.size(); j++) buffer[j] = 0;
      return true;  // succeeded in incrementing the vector
    }
    // if buffer[i] >= OrderOf(i)-1, mover to previous index i
  }
  return false;     // cannot increment the vector anymore
}

long PAlgebra::coordinate(long i, long k) const
{
  if (isDryRun()) return 0;
  long t = ith_rep(k); // element of Zm^* representing the k'th slot

  // dLog returns the representation of t along the generators, so the
  // i'th entry there is the coordinate relative to i'th geneator
  return dLog(t)[i];
}

long PAlgebra::addCoord(long i, long k, long offset) const
{
  if (isDryRun()) return 0;
  assert(k >= 0 && k < (long) nSlots);
  assert(i >= 0 && i < (long) gens.size());

  offset = offset % ((long) OrderOf(i));
  if (offset < 0) offset += OrderOf(i);

  long k_i = coordinate(i, k);
  long k_i1 = (k_i + offset) % OrderOf(i);

  long k1 = k + (k_i1 - k_i) * prods[i+1];

  return k1;
}

unsigned long PAlgebra::exponentiate(const vector<unsigned long>& exps,
				bool onlySameOrd) const
{
  if (isDryRun()) return 1;
  unsigned long t = 1;
  unsigned long n = min(exps.size(),gens.size());
  for (unsigned long i=0; i<n; i++) {
    if (onlySameOrd && !SameOrd(i)) continue;
    unsigned long g = PowerMod(gens[i] ,exps[i], m);
    t = MulMod(t, g, m);
  }
  return t;
}

void PAlgebra::printout() const
{
  cout << "m = " << m << ", p = " << p;
  if (isDryRun()) { cout << " (dry run)\n"; return; }
  cout << ", phi(m) = " << phiM << endl;
  cout << "  ord(p)=" << ordP << endl;

  unsigned long i;
  for (i=0; i<gens.size(); i++) if (gens[i]) {
      cout << "  generator " << gens[i] << " has order ("
           << (SameOrd(i)? "=":"!") << "= Z_m^*) of "
	   << OrderOf(i) << endl;
  }
  if (qGrpOrd()<100) {
    cout << "  T = [";
    for (i=0; i<T.size(); i++) cout << T[i] << " ";
    cout << "]\n";
  }
}

PAlgebra::PAlgebra(unsigned long mm, unsigned long pp,
                   const vector<long>& _gens, const vector<long>& _ords )
{
  assert( ProbPrime(pp) );
  assert( (mm % pp) != 0 );
  assert( mm < NTL_SP_BOUND );
  assert( mm > 1 );

  cM  = 1.0; // default value for the ring constant
  m = mm;
  p = pp;

  long k = NextPowerOfTwo(m);
  if (mm == (1UL << k))
    pow2 = k;
  else
    pow2 = 0;

  // For dry-run, use a tiny m value for the PAlgebra tables
  if (isDryRun()) mm = (p==3)? 4 : 3;

  // Compute the generators for (Z/mZ)^* (defined in NumbTh.cpp)

  if (_gens.size() == 0 || isDryRun())
      ordP = findGenerators(this->gens, this->ords, mm, pp);
  else {
    assert(_gens.size() == _ords.size());
    gens = _gens;
    ords = _ords;
    ordP = multOrd(pp, mm);
  }
  nSlots = qGrpOrd();
  phiM = ordP * nSlots;

  // Allocate space for the various arrays
  T.resize(nSlots);
  dLogT.resize(nSlots*gens.size());
  Tidx.assign(mm,-1);    // allocate m slots, initialize them to -1
  zmsIdx.assign(mm,-1);  // allocate m slots, initialize them to -1
  long i, idx;
  for (i=idx=0; i<(long)mm; i++) if (GCD(i,mm)==1) zmsIdx[i] = idx++;

  // Now fill the Tidx and dLogT translation tables. We identify an element
  // t\in T with its representation t = \prod_{i=0}^n gi^{ei} mod m (where
  // the gi's are the generators in gens[]) , represent t by the vector of
  // exponents *in reverse order* (en,...,e1,e0), and order these vectors
  // in lexicographic order.

  // FIXME: is the comment above about reverse order true? It doesn't
  // seem like it to me.  VJS.

  // buffer is initialized to all-zero, which represents 1=\prod_i gi^0
  vector<unsigned long> buffer(gens.size()); // temporaty holds exponents
  i = idx = 0;
  long ctr = 0;
  do {
    ctr++;
    unsigned long t = exponentiate(buffer);
    for (unsigned long j=0; j<buffer.size(); j++) dLogT[idx++] = buffer[j];

    assert(GCD(t,mm) == 1); // sanity check for user-supplied gens
    assert(Tidx[t] == -1);

    T[i] = t;       // The i'th element in T it t
    Tidx[t] = i++;  // the index of t in T is i

    // increment buffer by one (in lexigoraphic order)
  } while (nextExpVector(buffer)); // until we cover all the group

  assert(ctr == long(nSlots)); // sanity check for user-supplied gens

  PhimX = Cyclotomic(mm); // compute and store Phi_m(X)

  // initialize prods array
  long ndims = gens.size();
  prods.resize(ndims+1);
  prods[ndims] = 1;
  for (long j = ndims-1; j >= 0; j--) {
    prods[j] = OrderOf(j) * prods[j+1];
  }
  //  pp_factorize(mFactors,mm); // prime-power factorization from NumbTh.cpp
}

/***********************************************************************

  PAlgebraMod stuff....

************************************************************************/

PAlgebraModBase *buildPAlgebraMod(const PAlgebra& zMStar, long r)
{
  unsigned long p = zMStar.getP();
  assert(r > 0);

  if (p == 2 && r == 1)
    return new PAlgebraModDerived<PA_GF2>(zMStar, r);
  else
    return new  PAlgebraModDerived<PA_zz_p>(zMStar, r);
}

template<class T>
void PAlgebraLift(const ZZX& phimx, const T& lfactors, T& factors, T& crtc, long r);

// Missing NTL functionality

void EDF(vec_zz_pX& v, const zz_pX& f, long d)
{
   EDF(v, f, PowerXMod(zz_p::modulus(), f), d);
}

zz_pEX FrobeniusMap(const zz_pEXModulus& F)
{
  return PowerXMod(zz_pE::cardinality(), F);
}

template<class type>
PAlgebraModDerived<type>::PAlgebraModDerived(const PAlgebra& _zMStar, long _r)
  : zMStar(_zMStar), r(_r)

{
  long p = zMStar.getP();
  long m = zMStar.getM();

  // For dry-run, use a tiny m value for the PAlgebra tables
  if (isDryRun()) m = (p==3)? 4 : 3;

  assert(r > 0);

  ZZ BigPPowR = power_ZZ(p, r);
  assert(BigPPowR.SinglePrecision());
  pPowR = to_long(BigPPowR);

  long nSlots = zMStar.getNSlots();

  RBak bak; bak.save();
  SetModulus(p);

  // Compute the factors Ft of Phi_m(X) mod p, for all t \in T

  RX phimxmod;

  conv(phimxmod, zMStar.getPhimX()); // Phi_m(X) mod p

  vec_RX localFactors;

  EDF(localFactors, phimxmod, zMStar.getOrdP()); // equal-degree factorization

  RX* first = &localFactors[0];
  RX* last = first + localFactors.length();
  RX* smallest = min_element(first, last);
  swap(*first, *smallest);

  // We make the lexicographically smallest factor have index 0.
  // The remaining factors are ordered according to their representives.

  RXModulus F1(localFactors[0]);
  for (long i=1; i<nSlots; i++) {
    unsigned long t =zMStar.ith_rep(i); // Ft is minimal polynomial of x^{1/t} mod F1
    unsigned long tInv = InvMod(t, m);  // tInv = t^{-1} mod m
    RX X2tInv = PowerXMod(tInv,F1);     // X2tInv = X^{1/t} mod F1
    IrredPolyMod(localFactors[i], X2tInv, F1);
  }
  /* Debugging sanity-check #1: we should have Ft= GCD(F1(X^t),Phi_m(X))
  for (i=1; i<nSlots; i++) {
    unsigned long t = T[i];
    RX X2t = PowerXMod(t,phimxmod);  // X2t = X^t mod Phi_m(X)
    RX Ft = GCD(CompMod(F1,X2t,phimxmod),phimxmod);
    if (Ft != localFactors[i]) {
      cout << "Ft != F1(X^t) mod Phi_m(X), t=" << t << endl;
      exit(0);
    }
  }*******************************************************************/

  if (r == 1) {
    build(PhimXMod, phimxmod);
    factors = localFactors;
    pPowRContext.save();

    // Compute the CRT coefficients for the Ft's
    crtCoeffs.SetLength(nSlots);
    for (long i=0; i<nSlots; i++) {
      RX te = phimxmod / factors[i]; // \prod_{j\ne i} Fj
      te %= factors[i];              // \prod_{j\ne i} Fj mod Fi
      InvMod(crtCoeffs[i], te, factors[i]); // \prod_{j\ne i} Fj^{-1} mod Fi
    }
  }
  else {
    PAlgebraLift(zMStar.getPhimX(), localFactors, factors, crtCoeffs, r);
    RX phimxmod1;
    conv(phimxmod1, zMStar.getPhimX());
    build(PhimXMod, phimxmod1);
    pPowRContext.save();
  }

  // set factorsOverZZ
  factorsOverZZ.resize(nSlots);
  for (long i = 0; i < nSlots; i++)
    conv(factorsOverZZ[i], factors[i]);

  genCrtTable();
  genMaskTable();
}

// Assumes current zz_p modulus is p^r
// computes S = F^{-1} mod G via Hensel lifting
void InvModpr(zz_pX& S, const zz_pX& F, const zz_pX& G, long p, long r)
{
  ZZX ff, gg, ss, tt;

  ff = to_ZZX(F);
  gg = to_ZZX(G);

  zz_pBak bak;
  bak.save();
  zz_p::init(p);

  zz_pX f, g, s, t;
  f = to_zz_pX(ff);
  g = to_zz_pX(gg);
  s = InvMod(f, g);
  t = (1-s*f)/g;
  assert(s*f + t*g == 1);
  ss = to_ZZX(s);
  tt = to_ZZX(t);

  ZZ pk = to_ZZ(1);

  for (long k = 1; k < r; k++) {
    // lift from p^k to p^{k+1}
    pk = pk * p;

    assert(divide(ss*ff + tt*gg - 1, pk));

    zz_pX d = to_zz_pX( (1 - (ss*ff + tt*gg))/pk );
    zz_pX s1, t1;
    s1 = (s * d) % g;
    t1 = (d-s1*f)/g;
    ss = ss + pk*to_ZZX(s1);
    tt = tt + pk*to_ZZX(t1);
  }

  bak.restore();

  S = to_zz_pX(ss);

  assert((S*F) % G == 1);
}

template<class T>
void PAlgebraLift(const ZZX& phimx, const T& lfactors, T& factors, T& crtc, long r)
{
   Error("uninstatiated version of PAlgebraLift");
}

// This specialized version of PAlgebraLift does the hensel
// lifting needed to finish off the initialization.
// It assumes the zz_p modulus is initialized to p
// when called, and leaves it set to p^r

template<>
void PAlgebraLift(const ZZX& phimx, const vec_zz_pX& lfactors, vec_zz_pX& factors, vec_zz_pX& crtc, long r)
{
  long p = zz_p::modulus();
  long nSlots = lfactors.length();

  vec_ZZX vzz;             // need to go via ZZX

  // lift the factors of Phi_m(X) from mod-2 to mod-2^r
  if (lfactors.length() > 1)
    MultiLift(vzz, lfactors, phimx, r); // defined in NTL::ZZXFactoring
  else {
    vzz.SetLength(1);
    vzz[0] = phimx;
  }

  // Compute the zz_pContext object for mod p^r arithmetic
  zz_p::init(power_long(p, r));

  zz_pX phimxmod = to_zz_pX(phimx);
  factors.SetLength(nSlots);
  for (long i=0; i<nSlots; i++)             // Convert from ZZX to zz_pX
    conv(factors[i], vzz[i]);

  // Finally compute the CRT coefficients for the factors
  crtc.SetLength(nSlots);
  for (long i=0; i<nSlots; i++) {
    zz_pX& fct = factors[i];
    zz_pX te = phimxmod / fct; // \prod_{j\ne i} Fj
    te %= fct;                // \prod_{j\ne i} Fj mod Fi
    InvModpr(crtc[i], te, fct, p, r);// \prod_{j\ne i} Fj^{-1} mod Fi
  }

}

// Returns a vector crt[] such that crt[i] = p mod Ft (with t = T[i])
template<class type>
void PAlgebraModDerived<type>::CRT_decompose(vector<RX>& crt, const RX& H) const
{
  unsigned long nSlots = zMStar.getNSlots();

  if (isDryRun()) {
    crt.clear();
    return;
  }
  crt.resize(nSlots);
  for (unsigned long i=0; i<nSlots; i++)
    rem(crt[i], H, factors[i]); // crt[i] = H % factors[i]
}

template<class type>
void PAlgebraModDerived<type>::embedInAllSlots(RX& H, const RX& alpha,
                                            const MappingData<type>& mappingData) const
{
  if (isDryRun()) {
    H = RX::zero();
    return;
  }
  FHE_TIMER_START;
  long nSlots = zMStar.getNSlots();

  vector<RX> crt(nSlots); // alloate space for CRT components

  // The i'th CRT component is (H mod F_t) = alpha(maps[i]) mod F_t,
  // where with t=T[i].

  if (IsX(mappingData.G) || deg(alpha) <= 0) {
    // special case...no need for CompMod, which is
    // is not optimized for this case

    for (long i=0; i<nSlots; i++)   // crt[i] = alpha(maps[i]) mod Ft
      crt[i] = ConstTerm(alpha);
  }
  else {
    // general case...

    for (long i=0; i<nSlots; i++)   // crt[i] = alpha(maps[i]) mod Ft
      CompMod(crt[i], alpha, mappingData.maps[i], factors[i]);
  }

  CRT_reconstruct(H,crt); // interpolate to get H
  FHE_TIMER_STOP;
}

template<class type>
void PAlgebraModDerived<type>::embedInSlots(RX& H, const vector<RX>& alphas,
                                         const MappingData<type>& mappingData) const
{
  if (isDryRun()) {
    H = RX::zero();
    return;
  }
  FHE_TIMER_START;

  long nSlots = zMStar.getNSlots();
  assert(lsize(alphas) == nSlots);

  for (long i = 0; i < nSlots; i++) assert(deg(alphas[i]) < mappingData.degG);

  vector<RX> crt(nSlots); // alloate space for CRT components

  // The i'th CRT component is (H mod F_t) = alphas[i](maps[i]) mod F_t,
  // where with t=T[i].

  if (IsX(mappingData.G)) {
    // special case...no need for CompMod, which is
    // is not optimized for this case

    for (long i=0; i<nSlots; i++)   // crt[i] = alpha(maps[i]) mod Ft
      crt[i] = ConstTerm(alphas[i]);
  }
  else {
    // general case...still try to avoid CompMod when possible,
    // which is the common case for encoding masks

    for (long i=0; i<nSlots; i++) {   // crt[i] = alpha(maps[i]) mod Ft
      if (deg(alphas[i]) <= 0)
        crt[i] = alphas[i];
      else
        CompMod(crt[i], alphas[i], mappingData.maps[i], factors[i]);
    }
  }

  CRT_reconstruct(H,crt); // interpolate to get p

  FHE_TIMER_STOP;
}

template<class type>
void PAlgebraModDerived<type>::CRT_reconstruct(RX& H, vector<RX>& crt) const
{
  if (isDryRun()) {
    H = RX::zero();
    return;
  }
  FHE_TIMER_START;
  long nslots = zMStar.getNSlots();

  const vector<RX>& ctab = crtTable;

  clear(H);
  RX tmp1, tmp2;

  bool easy = true;
  for (long i = 0; i < nslots; i++)
    if (!IsZero(crt[i]) && !IsOne(crt[i])) {
      easy = false;
      break;
    }

  if (easy) {
    for (long i=0; i<nslots; i++)
      if (!IsZero(crt[i]))
        H += ctab[i];
  }
  else {
    vector<RX> crt1;
    crt1.resize(nslots);
    for (long i = 0; i < nslots; i++)
       MulMod(crt1[i], crt[i], crtCoeffs[i], factors[i]);

    evalTree(H, crtTree, crt1, 0, nslots);
  }
  FHE_TIMER_STOP;
}

template<class type>
void PAlgebraModDerived<type>::mapToFt(RX& w,
			     const RX& G,unsigned long t,const RX* rF1) const
{
  if (isDryRun()) {
    w = RX::zero();
    return;
  }
  long i = zMStar.indexOfRep(t);
  if (i < 0) { clear(w); return; }

  if (rF1==NULL) {               // Compute the representation "from scratch"
    // special case
    if (G == factors[i]) {
      SetX(w);
      return;
    }

    //special case
    if (deg(G) == 1) {
      w = -ConstTerm(G);
      return;
    }

    // the general case: currently only works when r == 1
    assert(r == 1);

    REBak bak; bak.save();
    RE::init(factors[i]);        // work with the extension field GF_p[X]/Ft(X)
    REX Ga;
    conv(Ga, G);                 // G as a polynomial over the extension field

    vec_RE roots;
    FindRoots(roots, Ga);        // Find roots of G in this field
    RE* first = &roots[0];
    RE* last = first + roots.length();
    RE* smallest = min_element(first, last);
                                // make a canonical choice
    w=rep(*smallest);
    return;
  }
  // if rF1 is set, then use it instead, setting w = rF1(X^t) mod Ft(X)
  RXModulus Ft(factors[i]);
  //  long tInv = InvMod(t,m);
  RX X2t = PowerXMod(t,Ft);    // X2t = X^t mod Ft
  w = CompMod(*rF1,X2t,Ft);      // w = F1(X2t) mod Ft

  /* Debugging sanity-check: G(w)=0 in the extension field (Z/2Z)[X]/Ft(X)
  RE::init(factors[i]);
  REX Ga;
  conv(Ga, G); // G as a polynomial over the extension field
  RE ra;
  conv(ra, w);         // w is an element in the extension field
  eval(ra,Ga,ra);  // ra = Ga(ra)
  if (!IsZero(ra)) {// check that Ga(w)=0 in this extension field
    cout << "rF1(X^t) mod Ft(X) != root of G mod Ft, t=" << t << endl;
    exit(0);
  }*******************************************************************/
}

template<class type>
void PAlgebraModDerived<type>::mapToSlots(MappingData<type>& mappingData, const RX& G) const
{
  assert(deg(G) > 0 && zMStar.getOrdP() % deg(G) == 0);
  assert(LeadCoeff(G) == 1);
  mappingData.G = G;
  mappingData.degG = deg(mappingData.G);

  long nSlots = zMStar.getNSlots();
  long m = zMStar.getM();

  mappingData.maps.resize(nSlots);

  mapToF1(mappingData.maps[0],mappingData.G); // mapping from base-G to base-F1
  for (long i=1; i<nSlots; i++)
    mapToFt(mappingData.maps[i], mappingData.G, zMStar.ith_rep(i), &(mappingData.maps[0]));

  REBak bak; bak.save();
  RE::init(mappingData.G);
  mappingData.contextForG.save();

  if (deg(mappingData.G)==1) return;

  mappingData.rmaps.resize(nSlots);

  if (G == factors[0]) {
    // an important special case

    for (long i = 0; i < nSlots; i++) {
        long t = zMStar.ith_rep(i);
        long tInv = InvMod(t, m);

        RX ct_rep;
        PowerXMod(ct_rep, tInv, G);

        RE ct;
        conv(ct, ct_rep);

        REX Qi;
        SetCoeff(Qi, 1, 1);
        SetCoeff(Qi, 0, -ct);

        mappingData.rmaps[i] = Qi;
    }
  }
  else
  {
    // the general case: currently only works when r == 1

    assert(r == 1);

    vec_REX FRts;
    for (long i=0; i<nSlots; i++) {
      // We need to lift Fi from R[Y] to (R[X]/G(X))[Y]
      REX  Qi;
      long t, tInv=0;

      if (i == 0) {
        conv(Qi,factors[i]);
        FRts=EDF(Qi, FrobeniusMap(Qi), deg(Qi)/deg(G));
        // factor Fi over GF(p)[X]/G(X)
      }
      else {
        t = zMStar.ith_rep(i);
        tInv = InvMod(t, m);
      }

      // need to choose the right factor, the one that gives us back X
      long j;
      for (j=0; j<FRts.length(); j++) {
        // lift maps[i] to (R[X]/G(X))[Y] and reduce mod j'th factor of Fi

        REX FRtsj;
        if (i == 0)
           FRtsj = FRts[j];
        else {
            REX X2tInv = PowerXMod(tInv, FRts[j]);
            IrredPolyMod(FRtsj, X2tInv, FRts[j]);
        }

        // FRtsj is the jth factor of factors[i] over the extension field.
        // For j > 0, we save some time by computing it from the jth factor
        // of factors[0] via a minimal polynomial computation.

        REX GRti;
        conv(GRti, mappingData.maps[i]);
        GRti %= FRtsj;

        if (IsX(rep(ConstTerm(GRti)))) { // is GRti == X?
          Qi = FRtsj;                // If so, we found the right factor
          break;
        } // If this does not happen then move to the next factor of Fi
      }

      assert(j < FRts.length());
      mappingData.rmaps[i] = Qi;
    }
  }
}

template<class type>
void PAlgebraModDerived<type>::decodePlaintext(
   vector<RX>& alphas, const RX& ptxt, const MappingData<type>& mappingData) const
{
  long nSlots = zMStar.getNSlots();
  if (isDryRun()) {
    alphas.assign(nSlots, RX::zero());
    return;
  }

  // First decompose p into CRT components
  vector<RX> CRTcomps(nSlots); // allocate space for CRT component
  CRT_decompose(CRTcomps, ptxt);  // CRTcomps[i] = p mod facors[i]

  if (mappingData.degG==1) {
    alphas = CRTcomps;
    return;
  }

  alphas.resize(nSlots);

  REBak bak; bak.save(); mappingData.contextForG.restore();

  for (long i=0; i<nSlots; i++) {
    REX te;
    conv(te, CRTcomps[i]);   // lift i'th CRT componnet to mod G(X)
    te %= mappingData.rmaps[i];  // reduce CRTcomps[i](Y) mod Qi(Y), over (Z_2[X]/G(X))

    // the free term (no Y component) should be our answer (as a poly(X))
    alphas[i] = rep(ConstTerm(te));
  }
}

template<class type>
void PAlgebraModDerived<type>::decodeSlots(std::vector <RX> &alphas,
                                           const RX &ptxt,
                                           const std::vector<long> &positions,
                                           const MappingData <type> &mappingData) const
{
  long nSlots = zMStar.getNSlots();
  for (auto position : positions)
    assert(position >= 0 && position < nSlots && "Invalid positions");

  if (isDryRun()) {
    alphas = std::vector<RX>(positions.size(), RX::zero());
    return;
  }

  size_t nPos = positions.size();
  vector<RX> CRTcomps(nPos); // allocate space for CRT component
  for (size_t i = 0; i < nPos; i++)
    rem(CRTcomps[i], ptxt, factors[positions[i]]); // CRTcomp = H % factors[i]

  if (mappingData.degG==1) {
    alphas = CRTcomps;
    return;
  }

  alphas.resize(nPos);

  REBak bak; bak.save(); mappingData.contextForG.restore();

  for (size_t i = 0; i < nPos; i++) {
    REX te;
    conv(te, CRTcomps[i]);   // lift i'th CRT componnet to mod G(X)
    te %= mappingData.rmaps[positions[i]];  // reduce CRTcomps[i](Y) mod Qi(Y), over (Z_2[X]/G(X))

    // the free term (no Y component) should be our answer (as a poly(X))
    alphas[i] = rep(ConstTerm(te));
  }
}

template<class type>
void PAlgebraModDerived<type>::decode1Slot(RX &alpha, const RX &ptxt, const long i,
                                           const MappingData <type> &mappingData) const
{
  std::vector<RX> oneSlot;
  std::vector<long> position(1, i);
  decodeSlots(oneSlot, ptxt, position, mappingData);
  alpha = position.front();
}

template<class type>
void PAlgebraModDerived<type>::
buildLinPolyCoeffs(vector<RX>& C, const vector<RX>& L,
                   const MappingData<type>& mappingData) const
{
  REBak bak; bak.save(); mappingData.contextForG.restore();

  long d = RE::degree();
  long p = zMStar.getP();

  assert(lsize(L) == d);

  vec_RE LL;
  LL.SetLength(d);

  for (long i = 0; i < d; i++)
    conv(LL[i], L[i]);

  vec_RE CC;
  ::buildLinPolyCoeffs(CC, LL, p, r);

  C.resize(d);
  for (long i = 0; i < d; i++)
    C[i] = rep(CC[i]);
}

// code for generating mask tables
// the tables are generated "on demand"

template<class type>
void PAlgebraModDerived<type>::genMaskTable()
{
  // This is only called by the constructor, which has already
  // set the zz_p context

  RX tmp1;

  maskTable.resize(zMStar.numOfGens());
  for (long i = 0; i < (long)zMStar.numOfGens(); i++) {
    long ord = zMStar.OrderOf(i);
    maskTable[i].resize(ord+1);
    maskTable[i][ord] = 0;
    for (long j = ord-1; j >= 1; j--) {
      // initialize mask that is 1 whenever the ith coordinate is at least j
      // Note: maskTable[i][0] = constant 1, maskTable[i][ord] = constant 0
      maskTable[i][j] = maskTable[i][j+1];
      for (long k = 0; k < (long)zMStar.getNSlots(); k++) {
         if (zMStar.coordinate(i, k) == j) {
           div(tmp1, PhimXMod, factors[k]);
           mul(tmp1, tmp1, crtCoeffs[k]);
           add(maskTable[i][j], maskTable[i][j], tmp1);
         }
      }
    }
    maskTable[i][0] = 1;
  }
}

// code for generating crt tables
// the tables are generated "on demand"

template<class type>
void PAlgebraModDerived<type>::genCrtTable()
{
  // This is only called by the constructor, which has already
  // set the zz_p context

  long nslots = zMStar.getNSlots();
  crtTable.resize(nslots);
  for (long i = 0; i < nslots; i++) {
    RX allBut_i = PhimXMod / factors[i]; // = \prod_{j \ne i }Fj
    allBut_i *= crtCoeffs[i]; // = 1 mod Fi and = 0 mod Fj for j \ne i
    crtTable[i] = allBut_i;
  }

  buildTree(crtTree, 0, nslots);
}

template<class type>
void PAlgebraModDerived<type>::
  buildTree(shared_ptr< TNode<RX> >& res, long offset, long extent) const
{
  if (extent == 1)
    res = buildTNode<RX>(nullTNode<RX>(), nullTNode<RX>(),
                            factors[offset]);
  else {
    long half = extent/2;
    shared_ptr< TNode<RX> > left, right;
    buildTree(left, offset, half);
    buildTree(right, offset+half, extent-half);
    RX data = left->data * right->data;
    res = buildTNode<RX>(left, right, data);
  }
}

template<class type>
void PAlgebraModDerived<type>::evalTree(RX& res,
              shared_ptr< TNode<RX> > tree,
              const vector<RX>& crt1,
              long offset, long extent) const
{
  if (extent == 1)
    res = crt1[offset];
  else {
    long half = extent/2;
    RX lres, rres;
    evalTree(lres, tree->left, crt1, offset, half);
    evalTree(rres, tree->right, crt1, offset+half, extent-half);
    RX tmp1, tmp2;
    mul(tmp1, lres, tree->right->data);
    mul(tmp2, rres, tree->left->data);
    add(tmp1, tmp1, tmp2);
    res = tmp1;
  }
}

// Explicit instantiation

template class PAlgebraModDerived<PA_GF2>;
template class PAlgebraModDerived<PA_zz_p>;

// Helper function
CubeSignature::CubeSignature(const PAlgebra& alg): ndims(0)
{
  Vec<long> _dims(INIT_SIZE, alg.numOfGens());
  for (long i=0; i<(long)alg.numOfGens(); i++) _dims[i] = alg.OrderOf(i);
  initSignature(_dims);
}