/*
 * position_control.cpp
 *
 * Author:mz
 *
 * Time: 2018.11.27
 *
 * 说明: mavros位置控制示例程序
 *      输入：mavros发布的位置/速度信息
 *      输出：无人机的推力和姿态信息
 *      采用位置环/速度环串级PID控制，位置环P控制，速度环PID控制
 */
#include <fstream>
#include <math.h>
#include <string>
#include <time.h>
#include <queue>
#include <vector>
#include <cstdlib>
#include <stdlib.h>
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/Core>

#include <ros/ros.h>
#include "Parameter.h"
#include <PID.h>
#include <FILTER.h>

//topic
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/PositionTarget.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64.h>
#include <mavros_msgs/Thrust.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <sensor_msgs/Imu.h>

#include "ros/ros.h"
#include "std_msgs/Float32.h"
#include <chrono>
#include "iomanip"
#include <thread>
#include <unistd.h>
#include <mutex>
#include "offb_posctl/controlstate.h"

using namespace Eigen;//释放eigen命名空间 矩阵库
using namespace std;

///>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

mavros_msgs::State current_state;           //无人机当前状态
nav_msgs::Odometry pose_drone_odom;       //读入的无人机drone当前位置，x，y，z+姿态
sensor_msgs::Imu pose_drone_Imu;       //读入的无人机drone当前位置，x，y，z+姿态
nav_msgs::Odometry pose_car_odom;       //读入car当前位置
geometry_msgs::TwistStamped vel_drone;      //读入的无人机当前速度 线速度+角速度
geometry_msgs::Quaternion orientation_target;   //发给无人机的姿态指令  四元数
geometry_msgs::Vector3 angle_target;   //欧拉角
geometry_msgs::Vector3 vel_target;   //期望速度
geometry_msgs::Point plane_expected_position; //车的零点和飞机的零点差3m，根据车的当前位置计算飞机位置
geometry_msgs::PoseStamped target_attitude;  //1
mavros_msgs::Thrust target_thrust_msg; //1循环
std_msgs::Float64 plane_real_alt; //control前
mavros_msgs::AttitudeTarget target_atti_thrust_msg; //最终发布的消息 油门+角度
mavros_msgs::AttitudeTarget base_atti_thrust_msg; //最终发布的消息 油门+角度
//mavros_msgs::PositionTarget target_pos_msg;
nav_msgs::Odometry  planned_postwist_msg;
nav_msgs::Odometry planned_u_msg;
nav_msgs::Odometry current_relativepostwist_msg;
geometry_msgs::Vector3 targeterror_msg;
geometry_msgs::Vector3 temp_angle;
geometry_msgs::Vector3 rpy;
offb_posctl::controlstate controlstatearray_msg;

float thrust_target;        //期望推力
float Yaw_Init;
float Yaw_Locked = 0;           //锁定的偏航角(一般锁定为0)
bool got_initial_point = false;
PID PIDVX, PIDVY, PIDVZ;    //声明PID类
Parameter param;
std::ofstream logfile;

///for psopt
float px_ini = -3.0;
float pz_ini = 0;
float py_ini=0;
float vx_ini = -0.1;
float vz_ini = 0.0;
float vy_ini=0;
float phi_ini=0;
float omegax_ini=0;
float thrust_ini=9.8;
float tau_ini=0;
FILTER derivation_omegax(3);
float t_end = 3;
double thrustforceacc = 0.0;
int pointnumber=150;// the number is almost always 20. It less, the accuracy won't be enough, if more, the time consumpiton will be too large.
int controlfreq=50;
int discretizedpointpersecond = (int)pointnumber/t_end;
int controlcounter=0;
//int controltime = 15; //取的控制序列点数
int controlmode = 0;//0为最优控制,1为tractor
bool adjust_flag = true;
double euler_anlge_limit=0.314;

///for pitch compensation
float comp_integrate,comp_last_error;
float comp_kp = 0,comp_ki = 0,comp_kd = 0;

/// for tractor
std::mutex mtx;
double amp=0.5;
float sinrate=3;
double ocpPitch=0;
double ocpRoll=0;
double ax=0.0,az=0.0,ay=0.0;
double axp=0.0,azp=0.0,ayp=0.0;
double axv=0.0,azv=0.0,ayv=0.0;

///virtual car pos_twist
float virtual_pos_x = 0;
float virtual_vel_x = 0;

///
double bvp_restrict_ax, bvp_restrict_az;// bvp acc_x&z restriction
double bvp_feedback_gain_x[2] = {0.5,0.3}, bvp_feedback_gain_z[2] = {0.5,0.3}; ///bvp pos&vel feedback gain
double bvp_acc_x, bvp_acc_y,bvp_acc_z, bvp_ocpPitch, bvp_thrustforceacc; ///bvp feedback acc desired
///>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

//欧拉角转四元数
geometry_msgs::Quaternion euler2quaternion(float roll, float pitch, float yaw);//geometry_msgs的Quaternion类型的函数
geometry_msgs::Vector3 quaternion2euler(float x, float y, float z, float w);

float get_ros_time(ros::Time time_begin);                                            //获取ros当前时间
int pix_controller(float cur_time);
void vector3dLimit(Vector3d &v, double limit) ; ///limit should be positive
Vector3d vectorElementMultiply(Vector3d v1, Vector3d v2);
void tractor_controller(float time);
//int pix_controller(int cur_time);
void data_log(std::ofstream &logfile, float cur_time);
float pitch_compensation(float theta_dc, float theta_dn, float yk[],float uk[]);

///>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

void state_cb(const mavros_msgs::State::ConstPtr &msg){
    current_state = *msg;

}//当有消息到达topic时会自动调用一次
bool planeupdateflag= false;
void plane_pos_cb(const nav_msgs::Odometry::ConstPtr &msg){
    pose_drone_odom = *msg;//pose_drone_odom is nav_msgs::Odometry type
    planeupdateflag= true;
}
void plane_imu_cb(const sensor_msgs::Imu::ConstPtr &msg){
    pose_drone_Imu = *msg;
}

void plane_vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg){
    vel_drone = *msg;
}

void car_pos_cb(const nav_msgs::Odometry::ConstPtr &msg) {
    pose_car_odom = *msg;

    plane_expected_position.x = pose_car_odom.pose.pose.position.x; //-1 means axis difference
    plane_expected_position.y = pose_car_odom.pose.pose.position.y; //-1 means axis difference
    plane_expected_position.z = pose_car_odom.pose.pose.position.z + 0.5;
}
void plane_alt_cb(const std_msgs::Float64::ConstPtr &msg){
    plane_real_alt = *msg;
}
bool contstaterecieveflag= false;
void controlstate_cb(const offb_posctl::controlstate::ConstPtr &msg)
{
    controlstatearray_msg = *msg;
    controlcounter = controlstatearray_msg.inicounter;
    if(contstaterecieveflag == false)//第一次回调时初始化，之后这个flag一直是true
    {
        contstaterecieveflag= true;
        discretizedpointpersecond=controlstatearray_msg.discrepointpersecond;
    }

}

///>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)//argc  argument count 传参个数，argument value
{
    ros::init(argc, argv, "position_control");//初始化节点名称
    ros::NodeHandle nh;

    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>(
            "mavros/cmd/arming"); //使能解锁飞机  创建client对象并向arming发出请求，服务类型为CommandBool
    ros::ServiceClient setmode_client = nh.serviceClient<mavros_msgs::SetMode>(
            "mavros/set_mode"); //设置为自动控制模式 服务类型为SetMode

    // 【订阅】无人机当前状态/位置/速度信息
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 1,
                                                                 state_cb);//订阅器，订阅接收mavros/state话题的mavros_msgs::State类型的消息，有消息到达这个话题时会自动调用state_cb函数
    ros::Subscriber plane_position_pose_sub = nh.subscribe<nav_msgs::Odometry>("mavros/local_position/odom", 1,
                                                                               plane_pos_cb);//pos+twist
    ros::Subscriber plane_poseimu_sub = nh.subscribe<sensor_msgs::Imu>("/mavros/imu/data", 1,
                                                                       plane_imu_cb);//pos+twist
    ros::Subscriber plane_velocity_sub = nh.subscribe<geometry_msgs::TwistStamped>(
            "mavros/local_position/velocity_local", 1, plane_vel_cb); //twist
    ros::Subscriber car_position_sub = nh.subscribe<nav_msgs::Odometry>("odom", 1, car_pos_cb); //车的pos+twist
    ros::Subscriber plane_alt_sub = nh.subscribe<std_msgs::Float64>("mavros/global_position/rel_alt", 1, plane_alt_cb);
    ros::Subscriber controlstate_sub = nh.subscribe<offb_posctl::controlstate>("bvp_controlstate", 1, controlstate_cb);
    // 【发布】飞机姿态/拉力信息 坐标系:NED系
    ros::Publisher ocplan_postwist_pub = nh.advertise<nav_msgs::Odometry>("ocplan_positiontwist", 1);//bvp计算的期望位置
    ros::Publisher ocplan_u_pub = nh.advertise<nav_msgs::Odometry>("ocplan_u", 1);//bvp计算的期望控制量(,没有转化的)
    ros::Publisher target_atti_thrust_pub = nh.advertise<mavros_msgs::AttitudeTarget>("mavros/setpoint_raw/attitude",
                                                                                1);//发布给mavros的控制量(经过换算)
    ros::Publisher plane_rpy_pub = nh.advertise<geometry_msgs::Vector3>("drone/current_rpy", 1);//飞机当前的rpy角
    ros::Publisher current_relativepostwist_pub = nh.advertise<nav_msgs::Odometry>("current_relative_postwist",
                                                                                1);//当前状态方程中的状态量,即相对量
    ros::Publisher targeterror_pub = nh.advertise<geometry_msgs::Vector3>("targeterror", 1);//目标误差
    // 频率 [30Hz]
    ros::Rate rate(controlfreq);   //50hz的频率发送/接收topic  ros与pixhawk之间,50Hz control frequency

    // log输出文件初始化
    logfile.open("/home/sensenliu/catkin_ws/src/gazebo_ros_learning/offb_posctl/log/pitch_log_hover1.csv", std::ios::out);
    if (!logfile.is_open()) {
        ROS_ERROR("log to file error!");
//        return 0;
    }

    // 读取PID参数
    std::string paraadr("/home/sensenliu/catkin_ws/src/gazebo_ros_learning/offb_posctl/src/param");
    if (param.readParam(paraadr.c_str()) == 0) {
        std::cout << "read config file error!" << std::endl;
//        return 0;
    }

    /// 设置速度环PID参数 比例参数 积分参数 微分参数
    PIDVX.setPID(param.vx_p, param.vx_i, param.vx_d);
    PIDVY.setPID(param.vy_p, param.vy_i, param.vy_d);
    PIDVZ.setPID(param.vz_p, param.vz_i, param.vz_d);
    // 设置速度环积分上限 控制量最大值 误差死区
    PIDVX.set_sat(6, 10, 0);
    PIDVY.set_sat(2, 3, 0);
    PIDVZ.set_sat(2, 5, 0);

    /// 等待和飞控的连接
    while (ros::ok() && current_state.connected == 0) {

        ros::spinOnce(); //调用回调函数
        ros::Duration(1).sleep();
        ROS_INFO("Not Connected");
    }
    ROS_INFO("Connected!!");

    target_atti_thrust_msg.orientation.x = 0;
    target_atti_thrust_msg.orientation.y = 0;
    target_atti_thrust_msg.orientation.z = 0;
    target_atti_thrust_msg.orientation.w = -1;
    target_atti_thrust_msg.thrust = 0.65; //65%的油门 50%与重力平衡即悬停

    /// get car current pose to set plane pose
    float x = pose_car_odom.pose.pose.orientation.x;
    float y = pose_car_odom.pose.pose.orientation.y;
    float z = pose_car_odom.pose.pose.orientation.z;
    float w = pose_car_odom.pose.pose.orientation.w;
    Yaw_Init = quaternion2euler(x, y, z, w).z;

    ///set initial hover position and pose
    target_atti_thrust_msg.orientation.x = x;
    target_atti_thrust_msg.orientation.y = y;
    target_atti_thrust_msg.orientation.z = z;
    target_atti_thrust_msg.orientation.w = w;
    ROS_INFO("got initial point ");

    for (int i = 10; ros::ok() && i >0; --i)// let drone take off slightly at begining, but this step seems useless because the drono has not been armed
    {
        target_atti_thrust_pub.publish(target_atti_thrust_msg);
        ros::spinOnce();//让回调函数有机会被执行
        rate.sleep();
    }
    ROS_INFO("OUT OF LOOP WAIT");

    mavros_msgs::CommandBool arm_cmd; //解锁
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    ///解锁飞机
    while (ros::ok()) {
        if (current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))) {
            if (setmode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if (!current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))) {
                if (arming_client.call(arm_cmd) &&
                    arm_cmd.response.success) {
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }
        target_atti_thrust_pub.publish(target_atti_thrust_msg);
        if (plane_real_alt.data > 0.7) {
            ROS_INFO("plane takeoff !");
            break;
        }
        ros::spinOnce();
        rate.sleep();
    }

    /// reach initial hover position and pose by position control
    /*float error_position_sum = 0;
    float error_pose_sum = 5;
    float yaw_current;
    ros::Time begin_time_01 = ros::Time::now();
    int count = 0;
    float thrust_target_sum = 0;
    vector<float> orientation_x;
    vector<float> orientation_y;
    vector<float> orientation_z;
    vector<float> orientation_w;
    got_initial_point = true;
    base_atti_thrust_msg.thrust=0.57;

    while (ros::ok() && count < 500)//没有给小车速度时始终在这个循环里
    {

        ros::spinOnce();
        plane_expected_position.x = 0; //-1 means axis difference
        plane_expected_position.y = 0; //-1 means axis difference
        plane_expected_position.z = 0.5;

        float cur_time_01 = get_ros_time(begin_time_01);  // 相对时间
        pix_controller(cur_time_01);
        target_atti_thrust_msg.header.stamp.sec = pose_car_odom.header.stamp.sec;
        target_atti_thrust_msg.header.stamp.nsec = pose_car_odom.header.stamp.nsec;
        target_atti_thrust_msg.orientation = orientation_target;
        target_atti_thrust_msg.thrust = thrust_target;

        temp_angle = quaternion2euler(pose_drone_odom.pose.pose.orientation.x, pose_drone_odom.pose.pose.orientation.y,
                                      pose_drone_odom.pose.pose.orientation.z, \
                                      pose_drone_odom.pose.pose.orientation.w);//欧拉角
        rpy.y = temp_angle.y;
        rpy.x = temp_angle.x;
        rpy.z = temp_angle.z;
        plane_rpy_pub.publish(rpy);

        target_atti_thrust_pub.publish(target_atti_thrust_msg);
        rate.sleep();//休息

        count += 1;
        if (count >= 500)//count到达500以后,不会再增加
        {
            cout << "You can run Waffle pi!" << endl;
        }

    }

    ROS_INFO("reached initial point and pose ");
    */
    // 记录启控时间
    ros::Time begin_time_02 = ros::Time::now();
    int quad_state=0;//0 climbhover, 1 AggressiveFly, 2 AdhesionPhase, 3 keep current state hover, 4 AdhesionSuccess
    int tempcounter=0;
    float ascentvel=15;
    float tempCurrentPx=0,tempCurrentPy=0,tempCurrentPz=0;
    int lefnodeindex = 0;
    int rightnodeindex = 0;

///>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while (ros::ok()) {

        ros::spinOnce();//刷新callback的消息

        float cur_time = get_ros_time(begin_time_02);  // 当前时间
        switch (quad_state) {
            case 0:
                plane_expected_position.z=plane_expected_position.z+ascentvel*cur_time;
                plane_expected_position.z=min(plane_expected_position.z,0.5);
                plane_expected_position.x=0;
                plane_expected_position.y=0;
                pix_controller(cur_time);

                if(plane_expected_position.z>=0.5)
                {
                    tempcounter++;
                    if(tempcounter>=150)
                    {
                        quad_state=1;
                        controlcounter=1;
                        tempcounter=0;
                    }
                }
                break;
            case 1:
                if(contstaterecieveflag)  //订阅到bvp计算的控制量则flag为true,用于起始时刻,还没算出bvp时
                {
                    lefnodeindex = controlcounter;
                    if (lefnodeindex+1 < controlstatearray_msg.arraylength)
                    {
                        ///compensation
                        ocpRoll = controlstatearray_msg.phiarray[lefnodeindex];
                        thrustforceacc = controlstatearray_msg.thrustarray[lefnodeindex];

                        plane_expected_position.z=controlstatearray_msg.stateZarray[lefnodeindex];
                        plane_expected_position.x=controlstatearray_msg.stateXarray[lefnodeindex];
                        plane_expected_position.y=controlstatearray_msg.stateYarray[lefnodeindex];
                        pix_controller(cur_time);

                        if (lefnodeindex <= 6)
                        {
                            orientation_target = euler2quaternion(ocpRoll, angle_target.y, angle_target.z);
                            thrust_target = (float) (param.hoverthrust) * thrustforceacc / 9.8;
                        }
                    }
                    controlcounter++;
                    if(controlcounter>=controlstatearray_msg.arraylength ||controlstatearray_msg.arraylength<=10)
                    {
                        quad_state=2;
                    }
                } else{
                    pix_controller(cur_time);
                }
                break;
            case 2:
                tempCurrentPx=pose_drone_odom.pose.pose.position.x;
                tempCurrentPy=pose_drone_odom.pose.pose.position.y;
                tempCurrentPz=pose_drone_odom.pose.pose.position.z;;
                tempcounter++;
                if(tempCurrentPz<controlstatearray_msg.stateZarray[controlstatearray_msg.arraylength-1]-0.15)
                {
                    quad_state=3;
                    tempcounter=0;
                }
                if(tempcounter>=100&&tempCurrentPz>=controlstatearray_msg.stateZarray[controlstatearray_msg.arraylength-1]-0.15)
                {
                    quad_state=4;
                    tempcounter=0;
                }
                orientation_target = euler2quaternion(controlstatearray_msg.phiarray[controlstatearray_msg.arraylength-1], controlstatearray_msg.thetaarray[controlstatearray_msg.arraylength-1], angle_target.z);
                thrust_target  = param.hoverthrust*cos(controlstatearray_msg.phiarray[controlstatearray_msg.arraylength-1])*cos(controlstatearray_msg.thetaarray[controlstatearray_msg.arraylength-1]);   //目标推力值 to alleviate the gravity's component along the drone's z axis
//                thrust_target  = 0.2;   //目标推力值,只是用来保证提供扭矩，the drone is easy to fall freely and crash
                ROS_INFO_STREAM("Duringsuck_thrust_target: "<< thrust_target<<" roll:"<<controlstatearray_msg.phiarray[controlstatearray_msg.arraylength-1]<<" pitch:"<<controlstatearray_msg.thetaarray[controlstatearray_msg.arraylength-1]<<" yaw:"<<angle_target.z);
                break;
                break;
            case 3:
                plane_expected_position.x=tempCurrentPx;
                plane_expected_position.y=tempCurrentPy;
                plane_expected_position.z=tempCurrentPz;
                pix_controller(cur_time);
                break;
            case 4:
                orientation_target = euler2quaternion(controlstatearray_msg.phiarray[controlstatearray_msg.arraylength-1], controlstatearray_msg.thetaarray[controlstatearray_msg.arraylength-1], angle_target.z);
                thrust_target  = 0;   //目标推力值
                ROS_INFO_STREAM("Sucksuccess_thrust_target: "<< thrust_target<<" roll:"<<angle_target.x<<" pitch:"<<angle_target.y<<" yaw:"<<angle_target.z);
                break;
            default:
                break;
        }
//        if(quad_state!=4&&quad_state!=2)
//        {
//            pix_controller(cur_time);
//        }
        cout<<"refpos x y z: "<<plane_expected_position.x<<"   y:"<<plane_expected_position.y<<"  z:"<<plane_expected_position.z<<endl;

        if (planeupdateflag && pose_drone_odom.pose.pose.position.z>=0.2)   //订阅到飞机位置则flag为true，发布完相对位置的消息后flag置false
        {
            omegax_ini=min(max(pose_drone_odom.twist.twist.angular.x,-10.0),10.0);

            tau_ini=derivation_omegax.derivation(cur_time,pose_drone_odom.twist.twist.angular.x);
            tau_ini=min(max((double)tau_ini,-10.0),10.0);

            phi_ini=temp_angle.x;

            px_ini = pose_drone_odom.pose.pose.position.x;
            pz_ini = max(pose_drone_odom.pose.pose.position.z,0.2);
            py_ini = pose_drone_odom.pose.pose.position.y;
            vx_ini = vel_drone.twist.linear.x;
            vz_ini = vel_drone.twist.linear.z;
            vy_ini = vel_drone.twist.linear.y;

            thrust_ini=thrust_target/param.hoverthrust*9.8;
            thrust_ini=min((double)thrust_ini,19.6);

            std::cout << "px_ini:  " << px_ini << "pz_ini:  " << pz_ini << "vx_ini:  " << vx_ini << "vz_ini:  "
                      << vz_ini << std::endl;//输出,放到py文件中求解
            cout<<"va_ini:"<<vel_drone.twist.linear.x<<endl;
            current_relativepostwist_msg.pose.pose.position.x = px_ini;
            current_relativepostwist_msg.pose.pose.position.z = pz_ini;
            current_relativepostwist_msg.pose.pose.position.y = py_ini;
            current_relativepostwist_msg.twist.twist.linear.x = vx_ini;
            current_relativepostwist_msg.twist.twist.linear.y = vy_ini;
            current_relativepostwist_msg.twist.twist.linear.z = vz_ini;
            current_relativepostwist_msg.pose.pose.orientation.x=phi_ini;
            current_relativepostwist_msg.pose.pose.orientation.y=omegax_ini;
            current_relativepostwist_msg.pose.pose.orientation.z=thrust_ini;
            current_relativepostwist_msg.pose.pose.orientation.w=tau_ini;
            current_relativepostwist_msg.header.stamp = pose_drone_odom.header.stamp;
            current_relativepostwist_pub.publish(current_relativepostwist_msg);
            planeupdateflag = false;
        }

            ///publish plane current rpy
            temp_angle = quaternion2euler(pose_drone_odom.pose.pose.orientation.x,
                                          pose_drone_odom.pose.pose.orientation.y,
                                          pose_drone_odom.pose.pose.orientation.z,
                                          pose_drone_odom.pose.pose.orientation.w);//欧拉角
            rpy.y = temp_angle.y;
            rpy.x = temp_angle.x;
            rpy.z = temp_angle.z;
            plane_rpy_pub.publish(rpy);

            ///publish thrust & orientation
            std::cout << "thrust_target: " << thrust_target << std::endl;
            target_atti_thrust_msg.header.stamp = ros::Time::now();
            target_atti_thrust_msg.orientation = orientation_target;
            target_atti_thrust_msg.thrust = thrust_target;
            target_atti_thrust_pub.publish(target_atti_thrust_msg);

            ///publish planned pos&twist in x&z
            planned_postwist_msg.header.stamp = ros::Time::now();
            ocplan_postwist_pub.publish(planned_postwist_msg);

            ///publish planned thrust & pitch
            ocplan_u_pub.publish(planned_u_msg);

            ///publish targeterror_msg
            targeterror_msg.x = px_ini + 3;
            targeterror_msg.z = pz_ini + px_ini * rpy.y;
            targeterror_msg.y = py_ini;//pub time consumption
            targeterror_pub.publish(targeterror_msg);
            rate.sleep();

        }
        logfile.close();
        return 0;
    }

/**
 * 获取当前时间 单位：秒
 */
float get_ros_time(ros::Time time_begin)
{
    ros::Time time_now = ros::Time::now();
    float currTimeSec = time_now.sec-time_begin.sec;
    float currTimenSec = time_now.nsec / 1e9 - time_begin.nsec / 1e9;
    return (currTimeSec + currTimenSec);
}

///>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函 数 定 义<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
float pitch_compensation(float theta_dc, float theta_dn, float yk[],float uk[])
{
    float y_next,pitch_revise,current_error;

    y_next = 1.461 * yk[0] - 0.5345 * yk[1] + 0.01827 * uk[3] +0.03653 * uk[4] + 0.01827 * uk[5]; // the predicted pitch in the next moment
    current_error = theta_dn - y_next;//current error
    comp_integrate += current_error;//error sum

    pitch_revise = theta_dc + comp_kp*current_error + comp_ki*comp_integrate + comp_kd*(current_error - comp_last_error)*controlfreq;
    comp_last_error = current_error;

    return (pitch_revise);
}

void vector3dLimit(Vector3d &v, double limit)  ///limit should be positive
{
    if(limit > 0){
        for(int i=0; i<3; i++){
            v(i) = fabs(v(i)) > limit ? (v(i) > 0 ? limit : -limit) : v(i);
        }
    }
}

Vector3d vectorElementMultiply(Vector3d v1, Vector3d v2)
{
    Vector3d result;
    result << v1(0)*v2(0), v1(1)*v2(1), v1(2)*v2(2);
    return result;
}

void tractor_controller(float time)
{
    static Vector3d z_w_norm(0, 0, 1.0);
    Vector3d p_error;
    Vector3d v_error;
    Vector3d planned_a;
    switch (controlmode)
    {
        case 0 :{
            p_error << (planned_postwist_msg.pose.pose.position.x - px_ini), (pose_car_odom.pose.pose.position.y-py_ini), (planned_postwist_msg.pose.pose.position.z-pz_ini);
            v_error << (planned_postwist_msg.twist.twist.linear.x - vx_ini), (pose_car_odom.twist.twist.linear.y-vy_ini), (planned_postwist_msg.twist.twist.linear.z-vz_ini);
            planned_a << sin(ocpPitch)*thrustforceacc, 0, thrustforceacc-9.8;
            break;
        }
        case 1 :{
            p_error << (planned_postwist_msg.pose.pose.position.x - px_ini), (pose_car_odom.pose.pose.position.y-py_ini), (planned_postwist_msg.pose.pose.position.z-pz_ini);
            v_error << (planned_postwist_msg.twist.twist.linear.x - vx_ini), (pose_car_odom.twist.twist.linear.y-vy_ini), (planned_postwist_msg.twist.twist.linear.z-vz_ini);
            planned_a << -amp*sinrate*sinrate*sin(sinrate*time), 0, 0;
            break;
        }
    }

    static Vector3d p_error_last;
    static Vector3d v_error_last;
    static Vector3d p_error_accumulate;
    static Vector3d v_error_accumulate;
    static bool if_init = true;
    static Vector3d position_error_p(param.txp_p,param.typ_p,param.tzp_p);
    static Vector3d position_error_d(param.txp_d,param.typ_d,param.tzp_d);
    static Vector3d position_error_i(param.txp_i,param.typ_i,param.tzp_i);
    static Vector3d velocity_error_p(param.txv_p,param.tyv_p,param.tzv_p);
    static Vector3d velocity_error_d(param.txv_d,param.tyv_d,param.tzv_d);
    static Vector3d velocity_error_i(param.txv_i,param.tyv_i,param.tzv_i);

    if(if_init){
        if_init = false;
        p_error_last = p_error;
        v_error_last = v_error;
        p_error_accumulate = p_error;
        v_error_accumulate = v_error;
        return;
    }

    /**Core code**/
    Vector3d delt_p_error = p_error - p_error_last;
    Vector3d delt_v_error = v_error - v_error_last;

    p_error_accumulate += p_error;
    v_error_accumulate += v_error;
    vector3dLimit(p_error_accumulate, 0.6);
    vector3dLimit(v_error_accumulate, 0.5);

    Vector3d a_fb =   /// PID
            vectorElementMultiply(p_error, position_error_p) + vectorElementMultiply(v_error, velocity_error_p) +
            vectorElementMultiply(delt_p_error, position_error_d) + vectorElementMultiply(delt_v_error, velocity_error_d) +
            vectorElementMultiply(p_error_accumulate, position_error_i) + vectorElementMultiply(v_error_accumulate, velocity_error_i);

    ax=a_fb(0);
    az=a_fb(2);
    ay=a_fb(1);
    axp=(vectorElementMultiply(p_error, position_error_p)+vectorElementMultiply(delt_p_error, position_error_d)+vectorElementMultiply(p_error_accumulate, position_error_i))(0);
    azp=(vectorElementMultiply(p_error, position_error_p)+vectorElementMultiply(delt_p_error, position_error_d)+vectorElementMultiply(p_error_accumulate, position_error_i))(2);
    ayp=(vectorElementMultiply(p_error, position_error_p)+vectorElementMultiply(delt_p_error, position_error_d)+vectorElementMultiply(p_error_accumulate, position_error_i))(1);
    axv=(vectorElementMultiply(v_error, velocity_error_p)+vectorElementMultiply(delt_v_error, velocity_error_d)+ vectorElementMultiply(v_error_accumulate, velocity_error_i))(0);
    azv=(vectorElementMultiply(v_error, velocity_error_p)+vectorElementMultiply(delt_v_error, velocity_error_d)+ vectorElementMultiply(v_error_accumulate, velocity_error_i))(2);
    ayv=(vectorElementMultiply(v_error, velocity_error_p)+vectorElementMultiply(delt_v_error, velocity_error_d)+ vectorElementMultiply(v_error_accumulate, velocity_error_i))(1);

    p_error_last = p_error;
    v_error_last = v_error;

    Vector3d a_des = a_fb + planned_a + 9.8 * z_w_norm;
    Vector3d att_des_norm = a_des / a_des.norm();
    ///quaternion way to determine attitude
//    Quaterniond att_des_q = Quaterniond::FromTwoVectors(z_w_norm, att_des_norm);
//    //add yaw
//    Quaterniond yaw_quat(cos(0/2.0), att_des_norm(0)*sin(0/2.0),
//                         att_des_norm(1)*sin(0/2.0),att_des_norm(2)*sin(0/2.0));
//    att_des_q = yaw_quat * att_des_q;
//
//    //Calculate thrust
//    orientation_target.x = att_des_q.x();
//    orientation_target.y = att_des_q.y();
//    orientation_target.z = att_des_q.z();
//    orientation_target.w = att_des_q.w();
    ///quaternion way to determine attitude

    angle_target.x = asin(-a_des(1)/a_des.norm());
    angle_target.y = atan(a_des(0)/a_des(2));
    angle_target.z = Yaw_Init;
    orientation_target = euler2quaternion(angle_target.x, angle_target.y, angle_target.z);

//    thrust_target  = (float)a_des.norm() /9.8*(base_atti_thrust_msg.thrust);   //目标推力值

    temp_angle = quaternion2euler(pose_drone_odom.pose.pose.orientation.x, pose_drone_odom.pose.pose.orientation.y,
                                  pose_drone_odom.pose.pose.orientation.z, \
        pose_drone_odom.pose.pose.orientation.w);
    thrust_target  = (float)(a_des(0)*sin(temp_angle.y)*cos(temp_angle.x)-a_des(1)*sin(temp_angle.x)+a_des(2)*cos(temp_angle.y)*cos(temp_angle.x)) /9.8*(base_atti_thrust_msg.thrust);   //目标推力值
}

int pix_controller(float cur_time)
{
//位 置 环
    //计算误差
    float error_x = plane_expected_position.x - pose_drone_odom.pose.pose.position.x;
    float error_y = plane_expected_position.y - pose_drone_odom.pose.pose.position.y;
    float error_z = plane_expected_position.z - plane_real_alt.data;
//    std::cout << "error: x：" << error_x << "\ty：" << error_y << "\tz：" << error_z << std::endl;
    //计算指定速度误差
    float vel_xd = param.x_p * error_x;
    float vel_yd = param.y_p * error_y;
    float vel_zd = param.z_p * error_z;
    vel_target.x = vel_xd;
    vel_target.y = vel_yd;
    vel_target.z = vel_zd;

//速 度 环
    //积分标志位.未进入OFFBOARD时,不累积积分项;进入OFFBOARD时,开始积分.
    PIDVX.start_intergrate_flag = true;
    PIDVY.start_intergrate_flag = true;
    PIDVZ.start_intergrate_flag = true;
    if(got_initial_point == false){
        PIDVX.start_intergrate_flag = false;
        PIDVY.start_intergrate_flag = false;
        PIDVZ.start_intergrate_flag = false;
    }
    //计算误差
    float error_vx = vel_xd - vel_drone.twist.linear.x;
    float error_vy = vel_yd - vel_drone.twist.linear.y;
    float error_vz = vel_zd - vel_drone.twist.linear.z;
    //传递误差
    PIDVX.add_error(error_vx, cur_time); //把error放到list中
    PIDVY.add_error(error_vy, cur_time);
    PIDVZ.add_error(error_vz, cur_time);
    //计算输出
    PIDVX.pid_output();
    PIDVY.pid_output();
    PIDVZ.pid_output();

//    Matrix2f A_yaw;
//    A_yaw << sin(Yaw_Locked), cos(Yaw_Locked),
//            -cos(Yaw_Locked), sin(Yaw_Locked);
//    Vector2f mat_temp(PIDVX.Output,PIDVY.Output);       //赋值到期望推力和姿态 x是前后，y是左右
//    Vector2f euler_temp= 1/9.8 * A_yaw.inverse() * mat_temp;
//    angle_target.x = euler_temp[0];
//    angle_target.y = euler_temp[1];
//    std::cout << " PIDVX.pid_output(): " << PIDVX.Output << "\tangle_target.y: " << angle_target.y << std::endl;
////    angle_target.z = Yaw_Locked + Yaw_Init;
//    angle_target.z = Yaw_Init;

    angle_target.x = asin(-PIDVY.Output/sqrt(pow(PIDVX.Output,2)+pow(PIDVY.Output,2)+pow(PIDVZ.Output+9.8,2)));
    angle_target.y = atan(PIDVX.Output/(PIDVZ.Output+9.8));
    angle_target.z = Yaw_Init;

    orientation_target = euler2quaternion(angle_target.x, angle_target.y, angle_target.z);
//    thrust_target = (float)(0.05 * (9.8 + PIDVZ.Output));   //目标推力值
    thrust_target  = (float)sqrt(pow(PIDVX.Output,2)+pow(PIDVY.Output,2)+pow(PIDVZ.Output+9.8,2))/9.8*(param.hoverthrust);   //目标推力值

//    std::cout << "PIDVZ.OUTPUT:  " << PIDVZ.Output << std::endl;
//    std::cout << "thrust_target:  " << thrust_target << std::endl;

    return 0;
}
/**
 * 将欧拉角转化为四元数
 * @param roll
 * @param pitch
 * @param yaw
 * @return 返回四元数
 */
geometry_msgs::Quaternion euler2quaternion(float roll, float pitch, float yaw)
{
    geometry_msgs::Quaternion temp;
    temp.w = cos(roll/2)*cos(pitch/2)*cos(yaw/2) + sin(roll/2)*sin(pitch/2)*sin(yaw/2);
    temp.x = sin(roll/2)*cos(pitch/2)*cos(yaw/2) - cos(roll/2)*sin(pitch/2)*sin(yaw/2);
    temp.y = cos(roll/2)*sin(pitch/2)*cos(yaw/2) + sin(roll/2)*cos(pitch/2)*sin(yaw/2);
    temp.z = cos(roll/2)*cos(pitch/2)*sin(yaw/2) - sin(roll/2)*sin(pitch/2)*cos(yaw/2);
    return temp;
}

/**
 * 将四元数转化为欧拉角形式
 * @param x
 * @param y
 * @param z
 * @param w
 * @return 返回Vector3的欧拉角
 */
geometry_msgs::Vector3 quaternion2euler(float x, float y, float z, float w)
{
    geometry_msgs::Vector3 temp;
    temp.x = atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
    temp.y = asin(2.0 * (w * y - z * x));
    temp.z = atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
    return temp;
}

/**
 * 将进入offboard后的位置&速度&姿态信息记录进文件
 * @param cur_time
 */

void data_log(std::ofstream &logfile, float cur_time)
{
    logfile<<cur_time<<","<<rpy.y<<std::endl;

}


// Copyright (C) 2012  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <dlib/svm.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.rls");

    void test_rls()
    {
        dlib::rand rnd;

        running_stats<double> rs1, rs2, rs3, rs4, rs5;

        for (int k = 0; k < 2; ++k)
        {
            for (long num_vars = 1; num_vars < 4; ++num_vars)
            {
                print_spinner();
                for (long size = 1; size < 300; ++size)
                {
                    {
                        matrix<double> X = randm(size,num_vars,rnd);
                        matrix<double,0,1> Y = randm(size,1,rnd);

                        const double C = 1000;
                        const double forget_factor = 1.0;
                        rls r(forget_factor, C);
                        for (long i = 0; i < Y.size(); ++i)
                        {
                            r.train(trans(rowm(X,i)), Y(i));
                        }

                        matrix<double> w = pinv(1.0/C*identity_matrix<double>(X.nc()) + trans(X)*X)*trans(X)*Y;

                        rs1.add(length(r.get_w() - w));
                    }

                    {
                        matrix<double> X = randm(size,num_vars,rnd);
                        matrix<double,0,1> Y = randm(size,1,rnd);

                        matrix<double,0,1> G(size,1);

                        const double C = 10000;
                        const double forget_factor = 0.8;
                        rls r(forget_factor, C);
                        for (long i = 0; i < Y.size(); ++i)
                        {
                            r.train(trans(rowm(X,i)), Y(i));

                            G(i) = std::pow(forget_factor, i/2.0);
                        }

                        G = flipud(G);

                        X = diagm(G)*X;
                        Y = diagm(G)*Y;

                        matrix<double> w = pinv(1.0/C*identity_matrix<double>(X.nc()) + trans(X)*X)*trans(X)*Y;

                        rs5.add(length(r.get_w() - w));
                    }

                    {
                        matrix<double> X = randm(size,num_vars,rnd);
                        matrix<double> Y = colm(X,0)*10;

                        const double C = 1000000;
                        const double forget_factor = 1.0;
                        rls r(forget_factor, C);
                        for (long i = 0; i < Y.size(); ++i)
                        {
                            r.train(trans(rowm(X,i)), Y(i));
                        }

                        matrix<double> w = pinv(1.0/C*identity_matrix<double>(X.nc()) + trans(X)*X)*trans(X)*Y;

                        rs2.add(length(r.get_w() - w));
                    }

                    {
                        matrix<double> X = join_rows(randm(size,num_vars,rnd)-0.5, ones_matrix<double>(size,1));
                        matrix<double> Y = uniform_matrix<double>(size,1,10);

                        const double C = 1e7;
                        const double forget_factor = 1.0;

                        matrix<double> w = pinv(1.0/C*identity_matrix<double>(X.nc()) + trans(X)*X)*trans(X)*Y;

                        rls r(forget_factor, C);
                        for (long i = 0; i < Y.size(); ++i)
                        {
                            r.train(trans(rowm(X,i)), Y(i));
                            rs3.add(std::abs(r(trans(rowm(X,i))) - 10));
                        }

                    }
                    {
                        matrix<double> X = randm(size,num_vars,rnd)-0.5;
                        matrix<double> Y = colm(X,0)*10;

                        const double C = 1e6;
                        const double forget_factor = 0.7;

                        rls r(forget_factor, C);
                        DLIB_TEST(std::abs(r.get_c() - C) < 1e-10);
                        DLIB_TEST(std::abs(r.get_forget_factor() - forget_factor) < 1e-15);
                        DLIB_TEST(r.get_w().size() == 0);

                        for (long i = 0; i < Y.size(); ++i)
                        {
                            r.train(trans(rowm(X,i)), Y(i));
                            rs4.add(std::abs(r(trans(rowm(X,i))) - X(i,0)*10));
                        }

                        DLIB_TEST(r.get_w().size() == num_vars);

                        decision_function<linear_kernel<matrix<double,0,1> > > df = r.get_decision_function();
                        DLIB_TEST(std::abs(df(trans(rowm(X,0))) - r(trans(rowm(X,0)))) < 1e-15);
                    }
                }
            }
        }

        dlog << LINFO << "rs1.mean(): " << rs1.mean();
        dlog << LINFO << "rs2.mean(): " << rs2.mean();
        dlog << LINFO << "rs3.mean(): " << rs3.mean();
        dlog << LINFO << "rs4.mean(): " << rs4.mean();
        dlog << LINFO << "rs5.mean(): " << rs5.mean();
        dlog << LINFO << "rs1.max(): " << rs1.max();
        dlog << LINFO << "rs2.max(): " << rs2.max();
        dlog << LINFO << "rs3.max(): " << rs3.max();
        dlog << LINFO << "rs4.max(): " << rs4.max();
        dlog << LINFO << "rs5.max(): " << rs5.max();

        DLIB_TEST_MSG(rs1.mean() < 1e-10, rs1.mean());
        DLIB_TEST_MSG(rs2.mean() < 1e-9, rs2.mean());
        DLIB_TEST_MSG(rs3.mean() < 1e-6, rs3.mean());
        DLIB_TEST_MSG(rs4.mean() < 1e-6, rs4.mean());
        DLIB_TEST_MSG(rs5.mean() < 1e-3, rs5.mean());

        DLIB_TEST_MSG(rs1.max() < 1e-10, rs1.max());
        DLIB_TEST_MSG(rs2.max() < 1e-6,  rs2.max());
        DLIB_TEST_MSG(rs3.max() < 0.001, rs3.max());
        DLIB_TEST_MSG(rs4.max() < 0.01,  rs4.max());
        DLIB_TEST_MSG(rs5.max() < 0.1,  rs5.max());

    }

    class rls_tester : public tester
    {
    public:
        rls_tester (
        ) :
            tester ("test_rls",
                    "Runs tests on the rls component.")
        {}

        void perform_test (
        )
        {
            test_rls();
        }
    } a;

}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse secant in \f$\pi\f$ multiples:
    \f$(1/\pi) \arccos(1/x)\f$.

    @see asec, asecd, cospi, acospi

    @par Header <boost/simd/function/asecpi.hpp>

    @par Example:

      @snippet asecpi.cpp asecpi

    @par Possible output:

      @snippet asecpi.txt asecpi

  **/
  IEEEValue asecpi(IEEEValue const & x);
} }
#endif

#include <boost/simd/function/scalar/asecpi.hpp>
#include <boost/simd/function/simd/asecpi.hpp>

#endif


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/div.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/ext/std/ratio.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/mod.hpp>
#include <boost/hana/mult.hpp>
#include <boost/hana/one.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/zero.hpp>

#include <ratio>
namespace hana = boost::hana;

BOOST_HANA_CONSTANT_CHECK(hana::equal(
    hana::plus(std::ratio<5, 3>{}, std::ratio<3, 12>{}),
    std::ratio<23, 12>{}
));

BOOST_HANA_CONSTANT_CHECK(hana::equal(
    hana::minus(std::ratio<5, 3>{}, std::ratio<3, 13>{}),
    std::ratio<56, 39>{}
));

BOOST_HANA_CONSTANT_CHECK(hana::equal(
    hana::mult(std::ratio<5, 3>{}, std::ratio<3, 13>{}),
    std::ratio<15, 39>{}
));

BOOST_HANA_CONSTANT_CHECK(hana::equal(
    hana::div(std::ratio<5, 3>{}, std::ratio<3, 13>{}),
    std::ratio<65, 9>{}
));

// The mod of two ratios is always 0, because they can always be
// divided without remainder.
BOOST_HANA_CONSTANT_CHECK(hana::equal(
    hana::mod(std::ratio<5, 3>{}, std::ratio<3, 13>{}),
    std::ratio<0>{}
));

BOOST_HANA_CONSTANT_CHECK(hana::equal(
    hana::zero<hana::ext::std::ratio_tag>(),
    std::ratio<0>{}
));

BOOST_HANA_CONSTANT_CHECK(hana::equal(
    hana::one<hana::ext::std::ratio_tag>(),
    std::ratio<1>{}
));

int main() { }


#include "numpy_utils.hpp"
#include <boost/foreach.hpp>
#include <boost/python.hpp>
#include <Eigen/Core>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <iostream>
#include <iostream>
#include <vector>

using namespace Eigen;
using namespace std;
namespace py = boost::python;

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

// http://en.wikipedia.org/wiki/Axis_angle#Log_map_from_SO.283.29_to_so.283.29
Vector3d LogMap(const Matrix3d& m) {
  double cosarg = (m.trace() - 1)/2;
  cosarg = fmin(cosarg, 1);
  cosarg = fmax(cosarg, -1);
  double theta = acos( cosarg );
  if (theta==0) return Vector3d::Zero();
  else return theta*(1/(2*sin(theta))) * Vector3d(m(2,1) - m(1,2), m(0,2)-m(2,0), m(1,0)-m(0,1));
}

double RotReg(const Matrix3d& b, const Vector3d& rot_coeffs, double scale_coeff) {
  // regularize rotation using polar decomposition
  JacobiSVD<Matrix3d> svd(b.transpose(), ComputeFullU | ComputeFullV);
  Vector3d s = svd.singularValues();
  if (b.determinant() <= 0) return INFINITY;
  return LogMap(svd.matrixU() * svd.matrixV().transpose()).cwiseAbs().dot(rot_coeffs) + s.array().log().square().sum()*scale_coeff;
}

Matrix3d RotRegGrad(const Matrix3d& b, const Vector3d& rot_coeffs, double scale_coeff) {
  Matrix3d out;
  double y0 = RotReg(b, rot_coeffs, scale_coeff);
  Matrix3d xpert = b;
  double epsilon = 1e-5;
  for (int i=0; i < 3; ++i) {
    for (int j=0; j < 3; ++j) {
      xpert(i,j) = b(i,j) + epsilon;
      out(i,j) = (RotReg(xpert, rot_coeffs, scale_coeff) - y0)/epsilon;
      xpert(i,j) = b(i,j);
    }
  }
  return out;
}

Vector3d gRotCoeffs;
double gScaleCoeff;

void PySetCoeffs(py::object rot_coeffs, py::object scale_coeff) {
  gRotCoeffs = Vector3d(py::extract<double>(rot_coeffs[0]), py::extract<double>(rot_coeffs[1]), py::extract<double>(rot_coeffs[2]));
  gScaleCoeff = py::extract<double>(scale_coeff);
}

double PyRotReg(const py::object& m ){
  const double* data = getPointer<double>(m);
  return RotReg( Map< const Matrix<double,3,3,RowMajor> >(data), gRotCoeffs, gScaleCoeff);
}

py::object PyRotRegGrad(const py::object& m) {
  static py::object np_mod = py::import("numpy");
  py::object out = np_mod.attr("empty")(py::make_tuple(3,3));
  const double* data = getPointer<double>(m);
  Matrix<double,3,3,RowMajor> g = RotRegGrad( Map< const Matrix<double,3,3,RowMajor> >(data), gRotCoeffs, gScaleCoeff);
  memcpy(getPointer<double>(out), g.data(), sizeof(double)*9);
  return out;
}

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

void Interp(float x0, float x1, const VectorXf& y0, const VectorXf& y1, const VectorXf& newxs, MatrixXf& newys) {
  float dx = x1 - x0;
  for (int i=0; i < newxs.size(); ++i) {
    newys.row(i) = ((x1 - newxs[i])/dx) * y0 + ((newxs[i] - x0)/dx) * y1;
  }
}

vector<int> Resample(const MatrixXf& x, const VectorXf& _t, float max_err, float max_dx, float max_dt) {
  int N = x.rows(); // number of timesteps
  VectorXf t;
  if (_t.size() == 0){
    t.resize(N);
    for (int i=0; i < N; ++i) t[i] = i;
  }
  else t=_t;
  VectorXi cost(N); // shortest path cost
  VectorXi pred(N); // shortest path predecessor

  MatrixXf q(20, x.cols()); // scratch space for interpolation
  q.setConstant(-666);

  const int NOPRED = -666;
  const int BIGINT = 999999;

  pred.setConstant(NOPRED);
  cost.setConstant(BIGINT);
  cost(0) = 0;
  pred(0) = NOPRED;
  for (int iSrc = 0; iSrc < N; ++iSrc) {
    for (int iTarg = iSrc+1; iTarg < N; ++iTarg) {
      float dx = (x.row(iTarg) - x.row(iSrc)).maxCoeff();
      float dt = t(iTarg) - t(iSrc);
      int seglen = iTarg - iSrc + 1;
      if (q.rows() < seglen) q.resize(2*q.rows(), q.cols());
      Interp(t(iSrc), t(iTarg), x.row(iSrc), x.row(iTarg), t.middleRows(iSrc, seglen), q);
      float err = (q.block(0,0,seglen, x.cols()) - x.middleRows(iSrc, seglen)).cwiseAbs().maxCoeff();
      if ((dx <= max_dx) && (dt <= max_dt || iTarg == iSrc+1) && (err <= max_err)) {
        int newcost = cost(iSrc) + 1;
        if (newcost < cost(iTarg)) {
          cost(iTarg) = newcost;
          pred(iTarg) = iSrc;
        }
      }
      else break;
    }
  }

  int i=N-1;
  vector<int> revpath;
  while (i > 0) {
    revpath.push_back(i);
    i = pred(i);
  }
  revpath.push_back(0);
  std::reverse(revpath.begin(), revpath.end());
  return revpath;
}

py::object pyResample(py::object x, py::object t, float max_err, float max_dx, float max_dt) {
  x = np_mod.attr("array")(x, "float32");
  int xdim0 = py::extract<int>(x.attr("shape")[0]);
  int xdim1 = py::extract<int>(x.attr("shape")[1]);
  float* xdata = getPointer<float>(x);
  t = np_mod.attr("array")(t, "float32");
  int tdim0 = py::extract<int>(t.attr("__len__")());
  float* tdata = getPointer<float>(t);
  vector<int> inds = Resample(Map<MatrixXf>(xdata, xdim0, xdim1), Map<VectorXf>(tdata, tdim0), max_err, max_dx, max_dt);
  return toNdarray1<int>(inds.data(), inds.size());
}
//BOOST_PYTHON_FUNCTION_OVERLOADS(resample_overloads, pyResample, 3, 5);

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

BOOST_PYTHON_MODULE(fastrapp) {

  np_mod = py::import("numpy");

  py::def("resample", &pyResample, (py::arg("x"), py::arg("t"), py::arg("max_err"),   py::arg("max_dx")=INFINITY, py::arg("max_dt")=INFINITY));

  py::def("set_coeffs", &PySetCoeffs, (py::arg("rot_coeffs"), py::arg("scale_coeff")));
  py::def("rot_reg", &PyRotReg, (py::arg("B")));
  py::def("rot_reg_grad", &PyRotRegGrad, (py::arg("B")));

}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#ifndef ROKKO_UTILITY_XYZ_HAMILTONIAN_HPP
#define ROKKO_UTILITY_XYZ_HAMILTONIAN_HPP

#include <vector>
#include <boost/tuple/tuple.hpp>
#include <rokko/localized_matrix.hpp>

namespace rokko {

namespace xyz_hamiltonian {

template<typename T>
void multiply(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling, const T* v, T* w) {
  int N = 1 << L;
  for (int l = 0; l < lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();

    double diag_plus = jz / 4.0;
    double diag_minus = - jz / 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;
    for (int k=0; k<N; ++k) {
      if (((k & m3) == m1) || ((k & m3) == m2)) {
        // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
        w[k] += diag_minus * v[k] + offdiag_plus * v[k^m3];
      } else {
        w[k] += diag_plus * v[k] + offdiag_minus * v[k^m3];
      }
    }
  }
}

template<typename T>
void multiply(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling, const std::vector<T>& v,
  std::vector<T>& w) {
  multiply(L, lattice, coupling, &v[0], &w[0]);
}

template<typename T>
void fill_diagonal(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling, T* w) {
  int N = 1 << L;
  for (int k=0; k<N; ++k) {
    w[k] = 0;
  }
  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();

    double diag_plus = jz / 4.0;
    double diag_minus = - jz / 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;
    for (int k=0; k<N; ++k) {
      if (((k & m3) == m1) || ((k & m3) == m2)) {
        // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
        w[k] += diag_minus;
      } else {
        w[k] += diag_plus;
      }
    }
  }
}

template<typename T>
void fill_diagonal(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling, std::vector<T>& w) {
  fill_diagonal(L, lattice, coupling, &w[0]);
}

template<typename T, typename MATRIX_MAJOR>
void generate(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling,
  rokko::localized_matrix<T, MATRIX_MAJOR>& mat) {
  mat.set_zeros();
  int N = 1 << L;
  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();
    double diag_plus = jz / 4.0;
    double diag_minus = - jz/ 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;
    for (int k=0; k<N; ++k) {
      if (((k & m3) == m1) || ((k & m3) == m2)) {
        // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
        mat(k^m3, k) += offdiag_plus;
        mat(k, k) += diag_minus;
      } else {
        mat(k^m3, k) += offdiag_minus;
        mat(k, k) += diag_plus;
      }
    }
  }
}

} // namespace xyz_hamiltonian

} // namespace rokko

#endif // ROKKO_UTILITY_XYZ_HAMILTONIAN_HPP


#include <iostream>
#include <iomanip>
#include <chrono>
#include <limits>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>

#include "poseocv.h"
#include "math.hh"
#include "display.h"

namespace poseocv
{
   void homography_pose(const cv::Mat& intrinsics, const std::vector<cv::Point3d>& train_img_pts,
                        const std::vector<cv::Point3d>& query_img_pts, std::vector<cv::Mat>& rotations,
                        std::vector<cv::Mat>& translations, std::vector<cv::Mat>& normals, bool isRANSAC)
//---------------------------------------------------------------------------------------------
   {
      cv::Mat H;
      rotations.clear(); translations.clear();
      std::vector<cv::Point2f> train_pts, query_pts;
      for (size_t i = 0; i < std::min(train_img_pts.size(), query_img_pts.size()); i++)
      {
         const cv::Point3d tpt = train_img_pts[i];
         train_pts.emplace_back(tpt.x, tpt.y);
         const cv::Point3d qpt = query_img_pts[i];
         query_pts.emplace_back(qpt.x, qpt.y);
      }
      if ((isRANSAC) && (train_img_pts.size() > 3))
         H = cv::findHomography(train_pts, query_pts, cv::RANSAC);
      else
         H = cv::findHomography(train_pts, query_pts);
      if (H.empty()) return;
      cv::decomposeHomographyMat(H, intrinsics, rotations, translations, normals);
   }

   bool pose_PnP(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point3d>& image_pts,
                 const cv::Mat& intrinsics, cv::Mat& rotation_vec, cv::Mat& translations3x1, cv::Mat* R,
                 long& total_time, bool is_refine, int flags, PnPRANSACParameters* RANSACParams)
//--------------------------------------------------------------------------------------------------
   {
      std::vector<cv::Point2f> points2d;
      std::vector<cv::Point3f> points3d;
      std::size_t n = std::min(world_pts.size(), image_pts.size());
      for (size_t i = 0; i < n; i++)
      {
         if ((i >= 4) &&
             ((flags == cv::SOLVEPNP_P3P) || (flags == cv::SOLVEPNP_AP3P)))
         {
//         std::cout << "poseocv::pose_PnP WARNING: Abbreviated input point count to " << points2d.size() << std::endl;
            break;
         }
         cv::Point3d pt = image_pts[i];
         points2d.emplace_back(pt.x, pt.y);
         pt = world_pts[i];
         points3d.emplace_back(pt.x, pt.y, pt.z);
      }
      if ( (RANSACParams != nullptr) && (n > 4) )
      {
         std::vector<unsigned char> inliers(points3d.size());
         auto start = std::chrono::high_resolution_clock::now();
         if (cv::solvePnPRansac(points3d, points2d, intrinsics, cv::noArray(), rotation_vec, translations3x1,
                                is_refine, RANSACParams->iterationsCount, RANSACParams->reprojectionError,
                                RANSACParams->confidence, inliers, flags))
         {
            auto diff = std::chrono::high_resolution_clock::now() - start;
            total_time = std::chrono::duration_cast<std::chrono::nanoseconds>(diff).count();
//         for (size_t j = 0; j < points3d.size(); j++)
//         {
//            if (!inliers[j])
//               fprintf(stdout, "outlier (%.5f, %.5f) (%.5f, %.5f, %.5f)\n", points2d[j].x, points2d[j].y, points3d[j].x,
//                       points3d[j].y, points3d[j].z);
//         }
            if (R != nullptr)
               cv::Rodrigues(rotation_vec, *R);
            return true;
         }
         else
            total_time = -1;
      }
      else
      {
         auto start = std::chrono::high_resolution_clock::now();
         try
         {
            if (cv::solvePnP(points3d, points2d, intrinsics, cv::noArray(), rotation_vec, translations3x1, is_refine,
                             flags))
            {
               auto diff = std::chrono::high_resolution_clock::now() - start;
               total_time = std::chrono::duration_cast<std::chrono::nanoseconds>(diff).count();
               if (R != nullptr)
                  cv::Rodrigues(rotation_vec, *R);
               return true;
            }
            else
               total_time = -1;
         }
         catch (...)
         {
            return false;
         }
      }
      return false;
   }

   void display_PnP(const std::vector<cv::Point3d>& world_points,
                    const std::vector<cv::Point3d>& image_points,
                    const cv::Mat& intrinsics, bool isMinimal, const cv::Mat* query_img,
                    bool show_reprojection, bool save_reprojection, bool is_time)
//----------------------------------------------------------------------------------------------------
   {
      cv::Mat rotation_vec, translations3x1, R;
      std::array<std::pair<std::string, int>, 5> methods {std::make_pair("Iterative", cv::SOLVEPNP_ITERATIVE),
                                                          std::make_pair("P3P", cv::SOLVEPNP_P3P),
                                                          std::make_pair("EPnP", cv::SOLVEPNP_EPNP),
                                                          std::make_pair("DLS", cv::SOLVEPNP_DLS),
                                                          std::make_pair("AP3P", cv::SOLVEPNP_AP3P)
      };
      for (auto pp : methods)
      {
         bool is_pose;
         std::vector<cv::Point3d> world_pts, image_pts, new_world_pts, new_image_pts;
         if (world_points.size() != image_points.size())
         {
            size_t n = std::min(world_points.size(), image_points.size());
            std::copy(world_points.begin(), world_points.begin() + n, std::back_inserter(world_pts));
            std::copy(image_points.begin(), image_points.begin() + n, std::back_inserter(image_pts));
         }
         else
         {
            world_pts = world_points;
            image_pts = image_points;
         }
         switch (pp.second)
         {
            case cv::SOLVEPNP_AP3P:
            case cv::SOLVEPNP_P3P:
               if (world_pts.size() > 4)
               {
                  std::copy(world_pts.begin(), world_pts.begin() + 4, std::back_inserter(new_world_pts));
                  std::copy(image_pts.begin(), image_pts.begin() + 4, std::back_inserter(new_image_pts));
               }
         }
         long time_ns;
         for (int i = 0; i < 1; i++)
         {
            bool is_ransac = (i > 0);
            PnPRANSACParameters RANSAC_params, *pRANSAC_params = nullptr;
            if (is_ransac)
            {
               std::cout << "RANSAC:";
               RANSAC_params.iterationsCount = 1000;
               pRANSAC_params = &RANSAC_params;
            }
            try
            {
               if (new_world_pts.size() > 0)
                  is_pose = pose_PnP(new_world_pts, new_image_pts, intrinsics, rotation_vec, translations3x1, &R,
                                     time_ns, false, pp.second, pRANSAC_params);
               else
                  is_pose = pose_PnP(world_pts, image_pts, intrinsics, rotation_vec, translations3x1, &R,
                                     time_ns, false, pp.second, pRANSAC_params);
            }
            catch (std::exception& e)
            {
//            std::cerr << "   display_PnP " << pp.first << " Exception: " << e.what() << std::endl;
               std::cout << "    No PnP solution (exception)" << std::endl;
               continue;
            }
            if (is_pose)
            {
               std::cout << "    ***** " << pp.first << " (" << time_ns << "ns )" << std::endl;
               cv::Mat TT = translations3x1.t();
               Eigen::Matrix3d ER;
               cv::cv2eigen(R, ER);
               Eigen::AngleAxisd aa(ER);
               Eigen::Quaterniond QQ(ER);
               if (!isMinimal)
               {
                  std::cout << "    Axis: " << aa.axis().transpose() << " Angle: " << mut::radiansToDegrees(aa.angle())
                            << std::endl;
                  std::cout << "    Quaternion: [" << QQ.w() << ", (" << QQ.vec().transpose() << ")" << std::endl;
               }

               const cv::Vec3f& euler = mut::rotation2Euler(R);
               std::cout << std::fixed << std::setprecision(4) << "Rotation Roll,Pitch,Yaw: ["
                         << mut::radiansToDegrees(euler[0]) << ","
                         << mut::radiansToDegrees(euler[1]) << "," << mut::radiansToDegrees(euler[2]) << " ]\u00B0 ("
                         << euler[0] << "," << euler[1] << "," << euler[2] << ") radians ("
                         << mut::radiansToDegrees(euler[0]) << "\\textdegree,"
                         << mut::radiansToDegrees(euler[1]) << "\\textdegree," << mut::radiansToDegrees(euler[2])
                         << "\\textdegree)" << std::endl;
               std::cout << std::fixed << std::setprecision(4) << "    Translation: " << TT.at<double>(0, 0) << ","
                         << TT.at<double>(0, 1) << "," << TT.at<double>(0, 2) << std::endl;
               cv::Mat empty_img;
               if (query_img == nullptr)
               {
                  query_img = &empty_img;
                  show_reprojection = false;
               }
               Eigen::Quaterniond Q(aa);
               Eigen::Vector3d t;
               cv::cv2eigen(translations3x1, t);
               std::string name;
               char* pname = nullptr;
               if ((save_reprojection) && (query_img != nullptr))
               {
                  name = "reproject-" + pp.first + ".jpg";
                  pname = const_cast<char*>(name.c_str());
               }
               double max_error, mean_error, stddev_error;
               show_projection(*query_img, world_pts, image_pts, intrinsics, Q, t[0], t[1], t[2],
                               max_error, mean_error, stddev_error, show_reprojection, pname);
               std::cout << std::setprecision(4) << "    Max Error " << max_error << ", mean error " << mean_error
                         << " Std Deviation " << stddev_error << std::endl;
            }
            else
               std::cout << "    No PnP solution" << std::endl;
            std::cout << "---------------------------------------------" << std::endl;
         }
      }
   }
}


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

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include <vector>

#include "tudat/math/basic/numericalDerivative.h"
#include "tudat/basics/testMacros.h"
#include "tudat/basics/utilities.h"
#include "tudat/io/matrixTextFileReader.h"

#include "tudat/math/interpolators/createInterpolator.h"
#include "tudat/io/basicInputOutput.h"

#include "tudat/interface/spice/spiceInterface.h"

namespace tudat
{
namespace unit_tests
{

template< typename TimeType, typename StateScalarType >
std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > computeSecondOrderCentralDifferenceCartesianStateDerivative(
        const std::map< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > >& cartesianStateMap )
{
    std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > cartesianStateDerivativeMap;

    auto lowerIterator = cartesianStateMap.begin( );
    auto centralIterator = cartesianStateMap.begin( );
    std::advance( centralIterator, 1 );
    auto upperIterator = cartesianStateMap.begin( );
    std::advance( upperIterator, 2 );

    Eigen::Matrix< StateScalarType, 6, 1 > currentStateDerivative;
    while( upperIterator != cartesianStateMap.end( ) )
    {
        if( lowerIterator == cartesianStateMap.begin( ) )
        {
            currentStateDerivative.segment( 0, 3 ) = lowerIterator->second.segment( 3, 3 );
            currentStateDerivative.segment( 3, 3 ) =
                    ( upperIterator->second.segment( 3, 3 ) - centralIterator->second.segment( 3, 3 ) ) /
                    ( upperIterator->first - centralIterator->first );
            cartesianStateDerivativeMap.push_back( currentStateDerivative );

        }

        currentStateDerivative.segment( 0, 3 ) = centralIterator->second.segment( 3, 3 );
        currentStateDerivative.segment( 3, 3 ) =
                ( upperIterator->second.segment( 3, 3 ) - lowerIterator->second.segment( 3, 3 ) ) /
                ( upperIterator->first - lowerIterator->first );
        cartesianStateDerivativeMap.push_back( currentStateDerivative );

        lowerIterator++;
        centralIterator++;
        upperIterator++;

        if( upperIterator == cartesianStateMap.end( ) )
        {
            currentStateDerivative.segment( 0, 3 ) = upperIterator->second.segment( 3, 3 );
            currentStateDerivative.segment( 3, 3 ) =
                    ( centralIterator->second.segment( 3, 3 ) - lowerIterator->second.segment( 3, 3 ) ) /
                    ( centralIterator->first - lowerIterator->first );
            cartesianStateDerivativeMap.push_back( currentStateDerivative );

        }

    }

    return cartesianStateDerivativeMap;
}

using namespace interpolators;

BOOST_AUTO_TEST_SUITE( test_interpolator_vector_conversion )

BOOST_AUTO_TEST_CASE( testInterpolatorVectorConversion )
{
    spice_interface::loadStandardSpiceKernels( );

    std::map< double, Eigen::Vector6d > vector6dInterpolatorInput;
    std::map< double, Eigen::VectorXd > vectorXdInterpolatorInput;
    std::map< double, Eigen::MatrixXd > matrixXdInterpolatorInput;
    for( int i = 0; i < 30; i++ )
    {
        double time = static_cast< double >( i ) * 86400.0;
        vector6dInterpolatorInput[ time ] = spice_interface::getBodyCartesianStateAtEpoch(
                    "Moon", "Earth", "J2000", "None", time );
        vectorXdInterpolatorInput[ time ] = vector6dInterpolatorInput[ time ];
        matrixXdInterpolatorInput[ time ] = vector6dInterpolatorInput[ time ];
    }

    std::vector< Eigen::Vector6d > vector6dDerivativeInput =
            computeSecondOrderCentralDifferenceCartesianStateDerivative(
                vector6dInterpolatorInput );
    std::vector< Eigen::VectorXd > vectorXdDerivativeInput;
    std::vector< Eigen::MatrixXd > matrixXdDerivativeInput;

    for( unsigned int i = 0; i < vector6dDerivativeInput.size( ); i++ )
    {
        vectorXdDerivativeInput.push_back( vector6dDerivativeInput.at( i ) );
        matrixXdDerivativeInput.push_back( vector6dDerivativeInput.at( i ) );

    }

    std::vector< double > interpolationTimes = { 1.0, 5.0 * 86400.0, 12.43 * 86400.0 };

    for( int i = 0; i < 5; i++ )
    {
        std::shared_ptr< InterpolatorSettings > interpolatorSettings;
        switch( i )
        {
        case 0:
            interpolatorSettings =
                    std::make_shared< InterpolatorSettings >( linear_interpolator );
            break;
        case 1:
            interpolatorSettings =
                    std::make_shared< InterpolatorSettings >( cubic_spline_interpolator );
            break;
        case 2:
            interpolatorSettings =
                    std::make_shared< LagrangeInterpolatorSettings >( 8 );
            break;
        case 3:
            interpolatorSettings =
                    std::make_shared< InterpolatorSettings >( hermite_spline_interpolator );
            break;
        case 4:
            interpolatorSettings =
                    std::make_shared< InterpolatorSettings >( piecewise_constant_interpolator );
            break;
        }
        std::shared_ptr< OneDimensionalInterpolator< double, Eigen::Vector6d > > direct6dInterpolator =
                createOneDimensionalInterpolator(
                    vector6dInterpolatorInput, interpolatorSettings,
                    std::make_pair( IdentityElement::getAdditionIdentity< Eigen::Vector6d >( ),
                                    IdentityElement::getAdditionIdentity< Eigen::Vector6d >( ) ),
                    vector6dDerivativeInput );
        std::shared_ptr< OneDimensionalInterpolator< double, Eigen::VectorXd > > directXdInterpolator =
                createOneDimensionalInterpolator(
                    vectorXdInterpolatorInput, interpolatorSettings,
                    std::make_pair( IdentityElement::getAdditionIdentity< Eigen::VectorXd >( ),
                                    IdentityElement::getAdditionIdentity< Eigen::VectorXd >( ) ),
                    vectorXdDerivativeInput );
        std::shared_ptr< OneDimensionalInterpolator< double, Eigen::MatrixXd > > directXdMatrixInterpolator =
                createOneDimensionalInterpolator(
                    matrixXdInterpolatorInput, interpolatorSettings,
                    std::make_pair( IdentityElement::getAdditionIdentity< Eigen::MatrixXd >( ),
                                    IdentityElement::getAdditionIdentity< Eigen::MatrixXd >( ) ),
                    matrixXdDerivativeInput);

        std::shared_ptr< OneDimensionalInterpolator< double, Eigen::Vector6d > > converted6dInterpolator =
                convertBetweenStaticDynamicEigenTypeInterpolators<  double, double, -1, 1, 6, 1 >( directXdInterpolator );
        std::shared_ptr< OneDimensionalInterpolator< double, Eigen::Vector6d > > converted6dInterpolator2 =
                convertBetweenStaticDynamicEigenTypeInterpolators<  double, double, -1, -1, 6, 1 >( directXdMatrixInterpolator );

        std::shared_ptr< OneDimensionalInterpolator< double, Eigen::VectorXd > > convertedXdInterpolator =
                convertBetweenStaticDynamicEigenTypeInterpolators<  double, double, 6, 1, -1, 1 >( direct6dInterpolator );
        std::shared_ptr< OneDimensionalInterpolator< double, Eigen::VectorXd > > convertedXdInterpolator2 =
                convertBetweenStaticDynamicEigenTypeInterpolators<  double, double, -1, -1, -1, 1 >( directXdMatrixInterpolator );

        std::shared_ptr< OneDimensionalInterpolator< double, Eigen::MatrixXd > > convertedXdMatrixInterpolator =
                convertBetweenStaticDynamicEigenTypeInterpolators<  double, double, 6, 1, -1, -1 >( direct6dInterpolator );
        std::shared_ptr< OneDimensionalInterpolator< double, Eigen::MatrixXd > > convertedXdMatrixInterpolator2 =
                convertBetweenStaticDynamicEigenTypeInterpolators<  double, double, -1, 1, -1, -1 >( directXdInterpolator );

        for( unsigned int j = 0; j < interpolationTimes.size( ); j++ )
        {
            std::vector< Eigen::MatrixXd > testMatrices;
            testMatrices.push_back( direct6dInterpolator->interpolate( interpolationTimes.at( j ) ) );
            testMatrices.push_back( directXdInterpolator->interpolate( interpolationTimes.at( j ) ) );
            testMatrices.push_back( directXdMatrixInterpolator->interpolate( interpolationTimes.at( j ) ) );

            testMatrices.push_back( converted6dInterpolator->interpolate( interpolationTimes.at( j ) ) );
            testMatrices.push_back( converted6dInterpolator2->interpolate( interpolationTimes.at( j ) ) );

            testMatrices.push_back( convertedXdInterpolator->interpolate( interpolationTimes.at( j ) ) );
            testMatrices.push_back( convertedXdInterpolator2->interpolate( interpolationTimes.at( j ) ) );

            testMatrices.push_back( convertedXdMatrixInterpolator->interpolate( interpolationTimes.at( j ) ) );
            testMatrices.push_back( convertedXdMatrixInterpolator2->interpolate( interpolationTimes.at( j ) ) );

            for( unsigned int k = 1; k < testMatrices.size( ); k++ )
            {
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            ( testMatrices.at( 0 ) ), ( testMatrices.at( k ) ), std::numeric_limits< double >::epsilon( ) );
            }

        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include "functions/integral_polynomial.hh"
#include "functions/polynomial.hh"
#include "functions/full_function_defs.hh"
#include "functions/operators.hh"
#include <boost/test/unit_test.hpp>
#include <complex>
#include "functions/std_functions.hh"
#include "functions/all_simplifications.hh"

BOOST_AUTO_TEST_CASE(polynomial_test) {
  using namespace manifolds;

  auto p = GetPolynomial(1, 0.5);
  BOOST_CHECK_EQUAL(p(0), 1);
  BOOST_CHECK_EQUAL(p(1), 1.5);
  BOOST_CHECK_EQUAL(p(2), 2);

  auto p2 = GetPolynomial(2, 5, 6);
  auto p3 = p + p2;
  static_assert(
      std::is_same<decltype(p3), Polynomial<double, int_<3> > >::value,
      "Failed to simplify adding two polynomials!");
  BOOST_CHECK_EQUAL(p3(0), 3);
  BOOST_CHECK_EQUAL(p3(1), 14.5);
  BOOST_CHECK_EQUAL(p3(-1), 3.5);

  auto p4 = p * p2;
  static_assert(
      std::is_same<decltype(p4), Polynomial<double, int_<4> > >::value,
      "Failed to simplify multiplying"
      " two polynomials!");
  BOOST_CHECK_EQUAL(p4(0), p(0) * p2(0));
  BOOST_CHECK_EQUAL(p4(1), p(1) * p2(1));
  BOOST_CHECK_EQUAL(p4(2), p(2) * p2(2));

  auto p5 = p3(p4);

  static_assert(Simplifies<decltype(p5)>::value,
                "Failed to simplify polynomial composition");
  auto p6 = Composition<decltype(p3), decltype(p4)>(p3, p4);
  for (int i = -3; i < 4; i++) {
    BOOST_CHECK_EQUAL(p5(i), p6(i));
  }

  std::complex<double> c{ 1, 2 };
  auto pc = GetPolynomial(c, 2.0 * c, c * c);

  BOOST_CHECK_EQUAL(pc(0.0), c);
  using namespace std::literals;
  BOOST_CHECK_EQUAL(pc(1i), 0i);

  auto ps = sin_ + sin_ + sin_;

  BOOST_CHECK_EQUAL(ps(2), 3 * sin_(2));

  ps = sin_ + (sin_ + sin_);
  // The fact that the line above compiles means that
  // the reverse generic simplification worked and gave
  // the expected type.

  BOOST_CHECK_EQUAL(ps(2), 3 * sin_(2));

  IntegralPolynomial<1, -2, 3> ip;
  IntegralPolynomial<3, 3, 7, 4> ip2;

  auto ip3 = ip + ip2;

  BOOST_CHECK_EQUAL(ip3(0), ip(0) + ip2(0));
  BOOST_CHECK_EQUAL(ip3(1), ip(1) + ip2(1));
  BOOST_CHECK_EQUAL(ip3(2), ip(2) + ip2(2));
  BOOST_CHECK_EQUAL(ip3(3), ip(3) + ip2(3));
  BOOST_CHECK_EQUAL(ip3(4), ip(4) + ip2(4));

  auto ip4 = ip * ip2;

  BOOST_CHECK_EQUAL(ip4(0), ip(0) * ip2(0));
  BOOST_CHECK_EQUAL(ip4(1), ip(1) * ip2(1));
  BOOST_CHECK_EQUAL(ip4(2), ip(2) * ip2(2));
  BOOST_CHECK_EQUAL(ip4(3), ip(3) * ip2(3));
  BOOST_CHECK_EQUAL(ip4(4), ip(4) * ip2(4));
  BOOST_CHECK_EQUAL(ip4(5), ip(5) * ip2(5));
  BOOST_CHECK_EQUAL(ip4(6), ip(6) * ip2(6));

  auto ip5 = ip4 + IntegralPolynomial<-1, -1, -1, 0, -13, -12>();
  static_assert(decltype(ip5)::num_coeffs == 4,
                "Failed to remove extraneous coefficients");

  auto p7 = p6 * ip5;
  static_assert(!IsVariadic<Multiplication, decltype(p7)>::value,
                "Failed to simplify multiplication of polynomial "
                "and integral polynomial");
}


/***************************************************************************
 *   Copyright (C) 2006 by Nicola Bellotto                                 *
 *   nbellotto@lincoln.ac.uk                                                    *
 *                                                                         *
 *   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 "models.h"
#include <bayes_tracking/BayesFilter/matSup.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <float.h>

using namespace Bayesian_filter;
using namespace Models;

//*************************************************************************
//                      CV PREDICTION MODEL
//*************************************************************************

CVModel::CVModel(Float wxSD, Float wySD) :
        Linrz_predict_model(x_size, q_size),
        Sampled_predict_model(),
        fx(x_size),
        genn(rnd),
        xp(x_size),
        n(q_size), rootq(q_size),
        m_wxSD(wxSD), m_wySD(wySD)
{
    first_init = true;
    init();
}

void CVModel::init()
{
    Fx.clear();
    // x
    Fx(0,0) = 1.;
    Fx(0,1) = dt;
    Fx(0,2) = 0.;
    Fx(0,3) = 0.;
    // dx
    Fx(1,0) = 0.;
    Fx(1,1) = 1.;
    Fx(1,2) = 0.;
    Fx(1,3) = 0.;
    // y
    Fx(2,0) = 0.;
    Fx(2,1) = 0.;
    Fx(2,2) = 1.;
    Fx(2,3) = dt;
    // dy
    Fx(3,0) = 0.;
    Fx(3,1) = 0.;
    Fx(3,2) = 0.;
    Fx(3,3) = 1.;
    // noise
    q[0] = sqr(m_wxSD); // cov(w_x)
    q[1] = sqr(m_wySD); // cov(w_y)
    G.clear();
    G(0,0) = 0.5*sqr(dt);
    G(1,0) = dt;
    G(2,1) = 0.5*sqr(dt);
    G(3,1) = dt;
}

const FM::Vec& CVModel::f(const FM::Vec& x) const
{  // human model
    fx[0] = x[0] + x[1] * dt;  // x
    fx[1] = x[1];              // dx
    fx[2] = x[2] + x[3] * dt;  // y
    fx[3] = x[3];              // dy
    return fx;
}

void CVModel::update(double dt)
{
    // time interval
    this->dt = dt;

    //     | 0.5dt^2     0    |
    // G = |    dt       0    |
    //     |    0     0.5dt^2 |
    //     |    0        dt   |
    G(0,0) = 0.5*sqr(dt);
    G(1,0) = dt;
    G(2,1) = 0.5*sqr(dt);
    G(3,1) = dt;
}

void CVModel::updateJacobian(const FM::Vec& x) {
    //      | 1  dt 0  0 |
    // Fx = | 0  1  0  0 |
    //      | 0  0  1  dt|
    //      | 0  0  0  1 |
    Fx(0,1) = dt;
    Fx(2,3) = dt;
}

const Vec& CVModel::fw(const FM::Vec& x) const
/*
   * Definition of sampler for additive noise model given state x
   *  Generate Gaussian correlated samples
   * Precond: init_GqG, automatic on first use
   */
{
    if (first_init)
        init_GqG();
    // Predict state using supplied functional predict model
    xp = f(x);
    // Additive random noise
    CVModel::genn.normal(n);            // independant zero mean normal
    // multiply elements by std dev
    for (FM::DenseVec::iterator ni = n.begin(); ni != n.end(); ++ni) {
        *ni *= rootq[ni.index()];
    }
    FM::noalias(xp) += FM::prod(this->G,n);         // add correlated noise
    return xp;
}

void CVModel::init_GqG() const
/* initialise predict given a change to q,G
   *  Implementation: Update rootq
   */
{
    first_init = false;
    for (FM::Vec::const_iterator qi = this->q.begin(); qi != this->q.end(); ++qi) {
        if (*qi < 0)
            error (Numeric_exception("Negative q in init_GqG"));
        rootq[qi.index()] = std::sqrt(*qi);
    }
}

//*************************************************************************
//                  2D CARTESIAN SUBTRACTION OBSERVATION MODEL
//*************************************************************************

CartesianModel::CartesianModel(Float xSD, Float ySD) :
        Linrz_correlated_observe_model(x_size, z_size),
        Likelihood_observe_model(z_size),
        z_pred(z_size),
        li(z_size)
{
    // Hx = | 1  0  0  0 |
    //      | 0  0  1  0 |
    Hx.clear();
    Hx(0,0) = 1.;
    Hx(1,2) = 1.;
    // noise
    Z.clear();
    Z(0,0) = sqr(xSD);
    Z(1,1) = sqr(ySD);
}

Bayes_base::Float
CartesianModel::Likelihood_correlated::L(const Correlated_additive_observe_model& model, const FM::Vec& z, const FM::Vec& zp) const
/*
 * Definition of likelihood given an additive Gaussian observation model:
 *  p(z|x) = exp(-0.5*(z-h(x))'*inv(Z)*(z-h(x))) / sqrt(2pi^nz*det(Z));
 *  L(x) the the Likelihood L(x) doesn't depend on / sqrt(2pi^nz) for constant z size
 * Precond: Observation Information: z,Z_inv,detZterm
 */
{
    if (!zset)
        Bayes_base::error (Logic_exception ("BGSubModel used without Lz set"));
    // Normalised innovation
    zInnov = z;
    model.normalise (zInnov, zp);
    FM::noalias(zInnov) -= zp;

    Float logL = scaled_vector_square(zInnov, Z_inv);
    using namespace std;
    return exp(Float(-0.5)*(logL + z.size()*log(2*M_PI) + logdetZ));   // normalized likelihood
}

void CartesianModel::Likelihood_correlated::Lz (const Correlated_additive_observe_model& model)
/* Set the observation zz and Z about which to evaluate the Likelihood function
 * Postcond: Observation Information: z,Z_inv,detZterm
 */
{
    zset = true;
    // Compute inverse of Z and its reciprocal condition number
    Float detZ;
    Float rcond = FM::UdUinversePD (Z_inv, detZ, model.Z);
    model.rclimit.check_PD(rcond, "Z not PD in observe");
    // detZ > 0 as Z PD
    using namespace std;
    logdetZ = log(detZ);
}

Bayes_base::Float
CartesianModel::Likelihood_correlated::scaled_vector_square(const FM::Vec& v, const FM::SymMatrix& V)
/*
 * Compute covariance scaled square inner product of a Vector: v'*V*v
 */
{
    return FM::inner_prod(v, FM::prod(V,v));
}

const FM::Vec& CartesianModel::h(const FM::Vec& x) const
{
    z_pred[0] = x[0];
    z_pred[1] = x[2];
    return z_pred;
};

void CartesianModel::updateJacobian(const FM::Vec& x) {
    // nothing to do
}

void CartesianModel::normalise(FM::Vec& z_denorm, const FM::Vec& z_from) const {
}

//*************************************************************************
//                      CV 3D PREDICTION MODEL
//*************************************************************************

CVModel3D::CVModel3D(Float wxSD, Float wySD, Float wzSD) :
        Linrz_predict_model(x_size, q_size),
        Sampled_predict_model(),
        fx(x_size),
        genn(rnd),
        xp(x_size),
        n(q_size), rootq(q_size),
        m_wxSD(wxSD), m_wySD(wySD),m_wzSD(wzSD)
{
    first_init = true;
    init();
}

void CVModel3D::init()
{
    Fx.clear();
    // x
    Fx(0,0) = 1.;
    Fx(0,1) = dt;
    Fx(0,2) = 0.;
    Fx(0,3) = 0.;
    Fx(0,4) = 0.;
    Fx(0,5) = 0.;
    // dx
    Fx(1,0) = 0.;
    Fx(1,1) = 1.;
    Fx(1,2) = 0.;
    Fx(1,3) = 0.;
    Fx(1,4) = 0.;
    Fx(1,5) = 0.;
    // y
    Fx(2,0) = 0.;
    Fx(2,1) = 0.;
    Fx(2,2) = 1.;
    Fx(2,3) = dt;
    Fx(2,4) = 0.;
    Fx(2,5) = 0.;
    // dy
    Fx(3,0) = 0.;
    Fx(3,1) = 0.;
    Fx(3,2) = 0.;
    Fx(3,3) = 1.;
    Fx(3,4) = 0.;
    Fx(3,5) = 0.;
    // z
    Fx(4,0) = 0.;
    Fx(4,1) = 0.;
    Fx(4,2) = 0.;
    Fx(4,3) = 0.;
    Fx(4,4) = 1.;
    Fx(4,5) = dt;
    // dz
    Fx(5,0) = 0.;
    Fx(5,1) = 0.;
    Fx(5,2) = 0.;
    Fx(5,3) = 0.;
    Fx(5,4) = 0.;
    Fx(5,5) = 1.;

    // noise
    q[0] = sqr(m_wxSD); // cov(w_x)
    q[1] = sqr(m_wySD); // cov(w_y)
    q[2] = sqr(m_wzSD); // cov(w_z)
    G.clear();
    G(0,0) = 0.5*sqr(dt);
    G(1,0) = dt;
    G(2,1) = 0.5*sqr(dt);
    G(3,1) = dt;
    G(4,2) = 0.5*sqr(dt);
    G(5,2) = dt;
}

const FM::Vec& CVModel3D::f(const FM::Vec& x) const
{  // human model
    fx[0] = x[0] + x[1] * dt;  // x
    fx[1] = x[1];              // dx
    fx[2] = x[2] + x[3] * dt;  // y
    fx[3] = x[3];              // dy
    fx[4] = x[4] + x[5] * dt;  // z
    fx[5] = x[5];              // dz
    return fx;
}

void CVModel3D::update(double dt)
{
    // time interval
    this->dt = dt;

    //     | 0.5dt^2     0        0    |
    //     |    dt       0        0    |
    //     |    0     0.5dt^2     0    |
    // G = |    0        dt       0    |
    //     |    0        0     0.5dt^2 |
    //     |    0        0         dt  |
    G(0,0) = 0.5*sqr(dt);
    G(1,0) = dt;
    G(2,1) = 0.5*sqr(dt);
    G(3,1) = dt;
    G(4,2) = 0.5*sqr(dt);
    G(5,2) = dt;
}

void CVModel3D::updateJacobian(const FM::Vec& x) {
    //      | 1  dt 0  0  0  0 |
    // Fx = | 0  1  0  0  0  0 |
    //      | 0  0  1  dt 0  0 |
    //      | 0  0  0  1  0  0 |
    //      | 0  0  0  0  1  dt|
    //      | 0  0  0  0  0  1 |
    Fx(0,1) = dt;
    Fx(2,3) = dt;
    Fx(4,5) = dt;
}

const Vec& CVModel3D::fw(const FM::Vec& x) const
/*
   * Definition of sampler for additive noise model given state x
   *  Generate Gaussian correlated samples
   * Precond: init_GqG, automatic on first use
   */
{
    if (first_init)
        init_GqG();
    // Predict state using supplied functional predict model
    xp = f(x);
    // Additive random noise
    CVModel3D::genn.normal(n);            // independent zero mean normal
    // multiply elements by std dev
    for (FM::DenseVec::iterator ni = n.begin(); ni != n.end(); ++ni) {
        *ni *= rootq[ni.index()];
    }
    FM::noalias(xp) += FM::prod(this->G,n);         // add correlated noise
    return xp;
}

void CVModel3D::init_GqG() const
/* initialise predict given a change to q,G
   *  Implementation: Update rootq
   */
{
    first_init = false;
    for (FM::Vec::const_iterator qi = this->q.begin(); qi != this->q.end(); ++qi) {
        if (*qi < 0)
            error (Numeric_exception("Negative q in init_GqG"));
        rootq[qi.index()] = std::sqrt(*qi);
    }
}

//*************************************************************************
//                      STATIC 3D PREDICTION MODEL
//*************************************************************************

StaticModel3D::StaticModel3D() :
        CVModel3D(0., 0., 0.),
        fx(x_size)
//        genn(rnd),
//        xp(x_size)
//        n(q_size), rootq(q_size)
//        m_wxSD(wxSD), m_wySD(wySD),m_wzSD(wzSD)
{
//    first_init = true;
    init();
}

void StaticModel3D::init()
{
    //      | 1  0  0  0  0  0 | x
    // Fx = | 0  0  0  0  0  0 | vx
    //      | 0  0  1  0  0  0 | y
    //      | 0  0  0  0  0  0 | vy
    //      | 0  0  0  0  1  0 | z
    //      | 0  0  0  0  0  0 | vz

    Fx.clear();

    Fx(0,0) = 1.;
    Fx(2,2) = 1.;
    Fx(4,4) = 1.;

    q.clear();
//    q[0] = 0.; // cov(w_x)
//    q[1] = 0.; // cov(w_y)
//    q[2] = 0.; // cov(w_z)
    G.clear(); // G = zero matrix

}

const FM::Vec& StaticModel3D::f(const FM::Vec& x) const
{  // static object model
    fx[0] = x[0];  // x
    fx[1] = 0;     // dx
    fx[2] = x[2];  // y
    fx[3] = 0;     // dy
    fx[4] = x[4];  // z
    fx[5] = 0;     // dz
    return fx;
}

void StaticModel3D::update(double dt)
{
    // time interval
    this->dt = dt;
//  G = zero matrix (no noise in static state)
}

void StaticModel3D::updateJacobian(const FM::Vec& x) {
    //      | 1  0  0  0  0  0 | x
    // Fx = | 0  0  0  0  0  0 | vx
    //      | 0  0  1  0  0  0 | y
    //      | 0  0  0  0  0  0 | vy
    //      | 0  0  0  0  1  0 | z
    //      | 0  0  0  0  0  0 | vz
    // No need to update the Jacobian since the function is linear and independent from state
}

const Vec& StaticModel3D::fw(const FM::Vec& x) const
/*
   * Definition of sampler for additive noise model given state x
   */
{
    return f(x);
}

//*************************************************************************
//                 3D  CARTESIAN SUBTRACTION OBSERVATION MODEL
//*************************************************************************

CartesianModel3D::CartesianModel3D(Float xSD, Float ySD, Float zSD) :
        Linrz_correlated_observe_model(x_size, z_size),
        Likelihood_observe_model(z_size),
        z_pred(z_size),
        li(z_size)
{
    // Hx = | 1  0  0  0  0  0 |
    //      | 0  0  1  0  0  0 |
    //      | 0  0  0  0  1  0 |
    Hx.clear();
    Hx(0,0) = 1.;
    Hx(1,2) = 1.;
    Hx(2,4) = 1.;
    // noise
    Z.clear();
    Z(0,0) = sqr(xSD);
    Z(1,1) = sqr(ySD);
    Z(2,2) = sqr(zSD);
}

Bayes_base::Float
CartesianModel3D::Likelihood_correlated::L(const Correlated_additive_observe_model& model, const FM::Vec& z, const FM::Vec& zp) const
/*
 * Definition of likelihood given an additive Gaussian observation model:
 *  p(z|x) = exp(-0.5*(z-h(x))'*inv(Z)*(z-h(x))) / sqrt(2pi^nz*det(Z));
 *  L(x) the the Likelihood L(x) doesn't depend on / sqrt(2pi^nz) for constant z size
 * Precond: Observation Information: z,Z_inv,detZterm
 */
{
    if (!zset)
        Bayes_base::error (Logic_exception ("BGSubModel used without Lz set"));
    // Normalised innovation
    zInnov = z;
    model.normalise (zInnov, zp);
    FM::noalias(zInnov) -= zp;

    Float logL = scaled_vector_square(zInnov, Z_inv);
    using namespace std;
    return exp(Float(-0.5)*(logL + z.size()*log(2*M_PI) + logdetZ));   // normalized likelihood
}

void CartesianModel3D::Likelihood_correlated::Lz (const Correlated_additive_observe_model& model)
/* Set the observation zz and Z about which to evaluate the Likelihood function
 * Postcond: Observation Information: z,Z_inv,detZterm
 */
{
    zset = true;
    // Compute inverse of Z and its reciprocal condition number
    Float detZ;
    Float rcond = FM::UdUinversePD (Z_inv, detZ, model.Z);
    model.rclimit.check_PD(rcond, "Z not PD in observe");
    // detZ > 0 as Z PD
    using namespace std;
    logdetZ = log(detZ);
}

Bayes_base::Float
CartesianModel3D::Likelihood_correlated::scaled_vector_square(const FM::Vec& v, const FM::SymMatrix& V)
/*
 * Compute covariance scaled square inner product of a Vector: v'*V*v
 */
{
    return FM::inner_prod(v, FM::prod(V,v));
}

const FM::Vec& CartesianModel3D::h(const FM::Vec& x) const
{
    z_pred[0] = x[0];
    z_pred[1] = x[2];
    z_pred[2] = x[4];
    return z_pred;
};

void CartesianModel3D::updateJacobian(const FM::Vec& x) {
    // nothing to do
}

void CartesianModel3D::normalise(FM::Vec& z_denorm, const FM::Vec& z_from) const {
}


#include <gtest/gtest.h>
#include <Eigen/Dense>

#include <ceres/internal/eigen.h>
#include <ceres/ceres.h>
#include <ceres/rotation.h>

struct PointToPlaneFunctor {

    explicit PointToPlaneFunctor(Eigen::Vector3d *reference,
                                 Eigen::Vector3d *target,
                                 Eigen::Vector3d *reference_normal) : reference_(reference),
                                                                      target_(target),
                                                                      reference_normal_(reference_normal) {}

    template<typename T>
    bool operator()(const T *const rot_params, const T *const trans_params, T *residual) const {
        Eigen::Map<Eigen::Quaternion<T>> quat(const_cast<T *>(rot_params));
        Eigen::Matrix<T, 3, 1> transformed = quat * target_->template cast<T>();
        transformed(0, 0) += trans_params[0];
        transformed(1, 0) += trans_params[1];
        transformed(2, 0) += trans_params[2];

        residual[0] =
                (reference_->template cast<T>() - transformed).transpose() * reference_normal_->template cast<T>();
        return true;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
    Eigen::Vector3d *reference_;
    Eigen::Vector3d *target_;
    Eigen::Vector3d *reference_normal_;
};

TEST(Ceres, ceres) {

    int num_points = 10000;
    std::vector<Eigen::Vector3d> points(num_points), transformed_points(num_points), normals(num_points);
    Eigen::Quaterniond rot = Eigen::Quaterniond::UnitRandom();
    Eigen::Vector3d trans = Eigen::Vector3d::Random();

    Eigen::Vector3d point, normal, transformed;
    for (auto i(0); i < num_points; ++i) {
        point = Eigen::Vector3d::Random();
        normal = Eigen::Vector3d::Random();
        transformed = rot * point + trans;
        points[i] = point;
        normals[i] = normal;
        transformed_points[i] = transformed;
    }

    Eigen::Quaterniond rot_init = rot.slerp(0.01, Eigen::Quaterniond::UnitRandom());
    Eigen::Vector3d trans_init = trans + Eigen::Vector3d::Random() * 0.1;

    ceres::Solver::Options options;
    options.max_num_iterations = 10;
    options.linear_solver_type = ceres::DENSE_QR;
    ceres::Solver::Summary summary;

    ceres::Problem problem;
    ceres::EigenQuaternionParameterization parameterization;
    ceres::CauchyLoss *loss = new ceres::CauchyLoss(0.1);

    // Add Parameters Block
    Eigen::Matrix<double, 4, 1> rot_coeffs = rot_init.coeffs();
    problem.AddParameterBlock(&rot_coeffs(0, 0), 4);
    problem.AddParameterBlock(&trans_init(0, 0), 3);

    for (int i(0); i < num_points; ++i) {
        ceres::CostFunction *cost_function =
                new ceres::AutoDiffCostFunction<PointToPlaneFunctor, 1, 4, 3>(
                        new PointToPlaneFunctor(&transformed_points[i], &points[i], &normals[i]));

        problem.AddResidualBlock(cost_function, loss, &rot_coeffs(0, 0), &trans_init(0, 0));
    }

    // Add Residual Blocks
    ceres::Solve(options, &problem, &summary);
    ASSERT_TRUE(summary.IsSolutionUsable());

    std::cout << summary.FullReport() << std::endl;

}