#include <iostream>

#include <CameraMarkerTransform.hpp>

#include <ros/ros.h>

#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <cv_bridge/cv_bridge.h>

#include <Eigen/Geometry>

#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/expressions.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>

#include <vector>

using namespace dls::perception;
using namespace cv;
using namespace cv::aruco;
using namespace gtsam;

typedef typename message_filters::Subscriber<sensor_msgs::Image> ImgSubscriber;
typedef typename std::shared_ptr<ImgSubscriber> ImgSubscriberPtr;

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> MyPolicy;
typedef typename message_filters::Synchronizer<MyPolicy> Synchronizer;
typedef typename std::shared_ptr<Synchronizer> SynchronizerPtr;

typedef Expression<Pose3> Pose3_;

class MultiCameraCalibNode {
public:
    typedef Eigen::Affine3d Transform;
public:
    MultiCameraCalibNode(ros::NodeHandle& nh) :
        nh_(nh),
        x_sensor(Symbol('x', 0))
    {

    tf::Matrix3x3 temp_tf_matrix;
    temp_tf_matrix.setRPY(0,30*M_PI/180,0);
    tf::Vector3 temp_tf_vector(0.65,0,0.1);
    tf::Transform temp_tf_transform(temp_tf_matrix,temp_tf_vector);
    tf::transformTFToEigen(temp_tf_transform,init_sensor);

    tf::Matrix3x3 temp_tf_matrix2;
    temp_tf_matrix2.setRPY(0,0,0);
    tf::Vector3 temp_tf_vector2(0.9,0,0);
    tf::Transform temp_tf_transform2(temp_tf_matrix2,temp_tf_vector2);
    tf::transformTFToEigen(temp_tf_transform2,init_sensor2);

    priorModel = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(1e-6), Vector3::Constant(1e-4)).finished());
    measurementModel = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(1e-6), Vector3::Constant(1e-4)).finished());
    }

    void run(){
        bool tf_ok_sensor = false;
        bool tf_ok_extender=false;

        if(verbose){
            ROS_INFO("Preparing to run the calibration with parameters:");
            ROS_INFO_STREAM("sensor_image_topic: " << image_topic);
            ROS_INFO_STREAM("sensor_camera_info_topic: " << camera_info_topic);
            ROS_INFO_STREAM("sensor_camera_frame_name: " << camera_frame_name);
            ROS_INFO_STREAM("sensor_sensor_frame_name: " << sensor_frame_name);
            ROS_INFO_STREAM("base_frame_name: " << base_frame_name);
            ROS_INFO_STREAM("marker_frame_name" << marker_frame_name);
            ROS_INFO_STREAM("knee_frame_name" << knee_frame_name);
        }

        sensor_camera_info_sub_ = nh_.subscribe(camera_info_topic,10,&MultiCameraCalibNode::sensorInfoCallback,this);
        camera_sub_ = nh_.subscribe(image_topic,10, &MultiCameraCalibNode::cameraCallback, this);
        // Don't leave until you get the camera to sensor mount transform!
        while(nh_.ok() && !tf_ok_sensor && !tf_ok_extender){
                   try{
                       ros::Time now = ros::Time::now();
                       tf_listener_.waitForTransform(sensor_frame_name, camera_frame_name,
                                                     now, ros::Duration(3.0));
                       tf_listener_.lookupTransform(sensor_frame_name, camera_frame_name,
                                                    now, tf_transform_);

                       tf::transformTFToEigen(tf_transform_, sTc_sensor);

                       if(verbose){
                           ROS_INFO("sensor Camera to sensor captured:");
                           ROS_INFO_STREAM(sTc_sensor.matrix().format(fmt_));
                       }
                       tf_ok_sensor = true;

                   }
                   catch (tf::TransformException ex){
                       ROS_ERROR("%s",ex.what());
                       ros::Duration(1.0).sleep();
                   }

                   try{
                       ros::Time now = ros::Time::now();
                       tf_listener_.waitForTransform(marker_frame_name, knee_frame_name,
                                                     now, ros::Duration(3.0));
                       tf_listener_.lookupTransform(marker_frame_name, knee_frame_name,
                                                    now, tf_transform_);

                       tf::transformTFToEigen(tf_transform_, mTk_);

                       if(verbose){
                           ROS_INFO("Knee to Marker captured:");
                           ROS_INFO_STREAM(mTk_.matrix().format(fmt_));
                       }
                       tf_ok_extender = true;

                   }
                   catch (tf::TransformException ex){
                       ROS_ERROR("%s",ex.what());
                       ros::Duration(1.0).sleep();
                   }
               }

        // don't leave until I say so!
        while(nh_.ok() && !stop){
            ros::spinOnce();
        }

                std::cout << " Time to process data and factor graph " << std::endl;

                graph.print("\nFactor Graph:\n");

                Values initials;
                initials.insert(Symbol('x', 0), Pose3(Rot3(init_sensor.rotation()),init_sensor.translation()));

                for (int i=1;i<=points_number;i++)
                {
                    std::cout << "init: " << i << std::endl;
                    initials.insert(Symbol('x',i),Pose3(Rot3(init_sensor2.rotation()),init_sensor2.translation()));
                }

                GaussNewtonParams parameters;
                parameters.setVerbosity("ERROR");

                // optimize!
                 GaussNewtonOptimizer optimizer(graph, initials, parameters);
                 Values results = optimizer.optimize();

               // print final values
                results.print("Final Result:\n");

    }

    void cameraCallback(const sensor_msgs::ImageConstPtr& image_sensor)
    {
        std::cout << "msg received" << std::endl;

        cv_bridge::CvImageConstPtr sensor_temp_image = cv_bridge::toCvShare(image_sensor, "mono8");
        Mat sensor_frame = sensor_temp_image->image;

            if(debug){
                std::cout << "sensor image: " << sensor_frame.rows
                          << " rows and " << sensor_frame.cols << " cols" << std::endl;
            }
            std::cout << "\nsensor looks for the image ... " <<std::endl;
            if(!sensor_camera_marker_transf_.getCameraMarkerTransform(sensor_frame,cTm_sensor))
            {
                return;
            }

            ros::Time now_sensor = image_sensor->header.stamp;
            ros::Time now  = ros::Time(0);
            if(1)
            {
                std::cout << " sensor_stamp " << now_sensor << " multisese_stamp "  << " now " << now << std::endl;
            }

            try{
                       tf_listener_.waitForTransform(base_frame_name, knee_frame_name,
                                                    now_sensor, ros::Duration(3.0));
                       tf_listener_.lookupTransform(base_frame_name, knee_frame_name,
                                                   now_sensor, tf_transform_);

                       tf::transformTFToEigen(tf_transform_, bTk_);

                   }
                   catch (tf::TransformException ex)
                   {
                       ROS_ERROR("%s",ex.what());
                       ros::Duration(1.0).sleep();
                       std::cout << " DATA SKIPPED " << std::endl;
                       return;
                   }

    factor_dot_sensor = bTk_ * mTk_.inverse();
    sTm = sTc_sensor*cTm_sensor;

    chain = bTk_ * mTk_.inverse() * cTm_sensor.inverse() * sTc_sensor.inverse();
    if (verbose)
           {
           std::cout << " \n<------------------DOT FACTORS --------------->" << std::endl;
           std::cout << " sTc_sensor " << std::endl;
           std::cout << sTc_sensor.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " cTm_sensor " << std::endl;
           std::cout << cTm_sensor.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " mTk_ " << std::endl;
           std::cout << mTk_.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " bTk_ " << std::endl;
           std::cout << bTk_.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " kTb_ " << std::endl;
           std::cout << bTk_.inverse().matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " factor_dot_sensor " << std::endl;
           std::cout << factor_dot_sensor.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " sTc_sensor*cTm_sensor " << std::endl;
           std::cout << (sTc_sensor*cTm_sensor).matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " sTc_sensor*cTm_sensor " << std::endl;
           std::cout << (sTc_sensor*cTm_sensor).inverse().matrix().format(fmt_) << std::endl << std::endl;

           }

    if (count <= points_number)
    {
Pose3_ foot_pose(Symbol('x', count));

    graph.addExpressionFactor(foot_pose,
                              Pose3(Rot3(factor_dot_sensor.rotation()),
                                    factor_dot_sensor.translation()),
                              priorModel);

    graph.addExpressionFactor(between(foot_pose,x_sensor),
                          Pose3(Rot3( sTm.inverse().rotation()),
                                sTm.inverse().translation()),
                                measurementModel);

}
         if (count == points_number)
         {
             std::cout << " Accumulated point exceeded, stop = true " << std::endl;
             stop=true;
         }

         if (1)
         {
             std::cout << "Number of factors accumulated " << count << std::endl;
         }
         count++;

    }

    void sensorInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info_msg){
        if(verbose){
            std::cout << "Received camera info: " << std::endl;
            std::cout << "-- distortion model: " << camera_info_msg->distortion_model << std::endl;
            std::cout << "-- coefficients: (k1, k2, t1, t2, k3): ";
        }
        for(int i = 0; i < 5; i++){
            std::cout << camera_info_msg->D[i]  << " ";
        }
        std::cout << std::endl;

        std::cout << camera_info_msg->D.size() << std::endl;

        if(!camera_info_msg->distortion_model.compare("plumb_bob") == 0){
            std::cout << "Sorry, only plumb_bob distortion model is supported" << std::endl;
            return;
        }

        Matx33d cameraMatrix;
        for(int i = 0; i < 9; i++) {
            cameraMatrix(i / 3, i % 3) = camera_info_msg->K[i];
        }

        cv::Vec<double,5> distCoeff;
        for(int i = 0; i < 5; i++) {
            distCoeff(i) = (double)camera_info_msg->D[i];
        }

        if(verbose){
            std::cout << "-- Receiving sensor parameters" << std::endl;
            std::cout << "   Camera Matrix: " << std::endl << cameraMatrix << std::endl;
            std::cout << std::endl;
            std::cout << "   Distortion coefficients: " << std::endl << distCoeff;
            std::cout << std::endl;
        }
        sensor_camera_marker_transf_.setCameraParameters(cameraMatrix, distCoeff);

        // we need the parameters only once.
        sensor_camera_info_sub_.shutdown();
    }

private:
    ros::NodeHandle nh_;
    CameraMarkerTransform sensor_camera_marker_transf_;

    ros::Subscriber camera_sub_;
    ros::Subscriber sensor_camera_info_sub_;

    tf::TransformListener tf_listener_;
    tf::StampedTransform tf_transform_;

    Transform cTm_sensor = Transform::Identity();
    Transform bTk_ = Transform::Identity(); // from base frame to knee frame
    Transform mTk_ = Transform::Identity(); // from knee frame to marker frame
    Transform sTc_sensor = Transform::Identity();

    Transform factor_dot_sensor=Transform::Identity();
    Transform sTm = Transform::Identity();
    Transform chain = Transform::Identity();

    Transform pose_sensor=Transform::Identity();

    Transform init_sensor=Transform::Identity();
    Transform init_sensor2=Transform::Identity();

    Eigen::IOFormat fmt_ = Eigen::IOFormat(Eigen::FullPrecision, 0, "\t ", "\n", "[", "]");

    bool stop = false;

    int count=1;

    ExpressionFactorGraph graph;
    noiseModel::Diagonal::shared_ptr measurementModel;
    noiseModel::Diagonal::shared_ptr priorModel;

    Pose3_ x_sensor;
    std::vector<gtsam::Pose3> poses;

public:
    std::string image_topic = "/multisense/left/image_mono";
    std::string camera_info_topic = "/multisense/left/image_mono/camera_info";

    std::string camera_frame_name = "/multisense/left_camera_optical_frame";
    std::string sensor_frame_name = "/head";
    std::string knee_frame_name = "/lf_lowerleg";

    std::string base_frame_name = "/base_link";
    std::string marker_frame_name = "/ext_calibration_plate_head";

    int points_number = 1;

    bool debug = false;
    bool verbose = false;
};

int main(int argc, char** argv)
{
    ros::init(argc,argv,"multicamera_calibrator_node");
    ros::NodeHandle nh("~");
    MultiCameraCalibNode calib_node(nh);

    std::string image_topic = "/multisense/left/image_mono";
    std::string camera_info_topic = "/multisense/left/image_mono/camera_info";

    std::string camera_frame_name = "/multisense/left_camera_optical_frame";
    std::string sensor_frame_name = "/head";
    std::string knee_frame_name = "/lf_lowerleg";

    bool verbose = false;
    int points_number=1;

    // getting parameters from outside (e.g., launch file)
    nh.getParam("image_topic", image_topic);
    nh.getParam("camera_info_topic", camera_info_topic);

    nh.getParam("camera_frame_name", camera_frame_name);
    nh.getParam("sensor_frame_name",sensor_frame_name);

    nh.getParam("verbose", verbose);
    nh.getParam("points_number",points_number);

    calib_node.image_topic = image_topic;
    calib_node.camera_info_topic = camera_info_topic;
    calib_node.camera_frame_name = camera_frame_name;
    calib_node.sensor_frame_name = sensor_frame_name;

    calib_node.points_number = points_number;

    calib_node.verbose = verbose;

    calib_node.run();
    return 0;
}


#include <iostream>
#include <chrono>
#include <thread>
#include <mutex>
#include <boost/algorithm/string.hpp>
#include <boost/bind.hpp>
#include <nghttp2/asio_http2_server.h>

using namespace nghttp2::asio_http2;
using namespace nghttp2::asio_http2::server;
using namespace boost::posix_time;

std::string bitrate;
int num;

int segment_duration = 1000; // 1000ms
auto avail_seg = std::make_shared<int>();
auto server_seg = std::make_shared<int>();
const int MAX_SEGMENTS = 151;

bool on_pushing_in_periodic_mode = false;
bool on_steady_stage = false;

ptime start_time, end_time;

void init_new_connection() {
  on_pushing_in_periodic_mode = false;

  start_time = microsec_clock::local_time();
  *server_seg=1;
}

int get_time() {
  end_time = microsec_clock::local_time();
  return (end_time - start_time).total_milliseconds();
}

void print_new_seg(int cur_seg) {
  std::cout << std::endl << "Respond seg #" << cur_seg << " bitrate " << bitrate;
  std::cout << " at time: " << get_time() << "ms" << std::endl;
}

void push_remaining_files(const response *res) {
  if (*server_seg + 1 >= MAX_SEGMENTS) {
    res->write_head(200);
    res->end();
    return;
  }

  // if all required segments were pushed. Note that it must not combined
  // with the previous instruction
  if (num == 0) { return; }

  print_new_seg(*server_seg+1);
  num--;

  boost::system::error_code ec;
  auto push = res->push(ec, "GET", "/seg_"+std::to_string(*server_seg+1)+"_rate_"+bitrate);

  push->on_close([res](uint32_t error_code) {
    if (!error_code) {                    // if CANCEL is not sent
      *server_seg = *server_seg + 1;
      std::cout << "Sent seg #" << *server_seg << " bitrate " << bitrate
                        << " at time: " << get_time() << "ms" << std::endl;
    }

    if (num == 0) {
      on_pushing_in_periodic_mode = false;

        res->write_head(200);
        res->end();
    } else {
      on_pushing_in_periodic_mode = true;
      push_remaining_files(res);
    }

  });

  push->write_head(200);
  push->end(file_generator("./real_cbr/"+std::to_string(segment_duration)+"ms/"+bitrate));
  on_pushing_in_periodic_mode = true;
}

void push_file(
    //std::shared_ptr<boost::asio::basic_deadline_timer<ptime>> timer,
    const response *res, std::shared_ptr<bool> closed, boost::system::error_code &ec) {
  if (ec || *closed) {
    return;
  }

  // the time instant of the next segment, note that it is available only if num > 0
  // Hung_comment: Cai nay chi la dia chi thoi nhe!
  if (num > 0) {
  //   timer->expires_at(timer->expires_at() + milliseconds(segment_duration));
  //   timer->async_wait(boost::bind(push_file, timer, res, closed, ec));
    push_file(res,closed,ec);
  }

  // compute avail_seg (note that video segments will be generated every segment duration
 // *avail_seg = get_time() / segment_duration + 1;

  // if the server is pushing another segment
  if (on_pushing_in_periodic_mode) { return; }

  // push all remaining segments,
  // during this duration, on_pushing_in_periodic_mode = true
  push_remaining_files(res);
}

int main(int argc, char *argv[]) {
  boost::system::error_code ec;
  boost::asio::ssl::context tls(boost::asio::ssl::context::sslv23);

//  tls.use_private_key_file("server.key", boost::asio::ssl::context::pem);
//  tls.use_certificate_chain_file("server.crt");
//  configure_tls_context_easy(ec, tls);

  http2 server;
  server.num_threads(100);

  server.handle("/reset_session", [](const request &req, const response &res) {
    on_steady_stage = false;
    res.write_head(200);
    res.end();
  });

  server.handle("/rebuff/", [](const request &req, const response &res) {
    if (!on_steady_stage) {
      init_new_connection();
      on_steady_stage = true;
    }

    //*server_seg = get_time() / segment_duration;

    // get url, e.g. http://127.0.0.1:3002/rebuff/bitrate=2000/num=4 -> bitrate = 2000kbps, number of segments = 4.
    // currently, the special symbol & is not allowed in the url
    std::vector<std::string> strs;
    boost::split(strs, req.uri().path, boost::is_any_of("/"));
    if (strs.size() != 4) {
      std::cout << "[ERROR] : url is incorrect" << std::endl;
      return;
    }

    std::vector<std::string> temp;
    for (int i = 2; i < strs.size(); i++) {
      boost::split(temp, strs[i], boost::is_any_of("="));
      if (temp[0] == "bitrate")
        bitrate = temp[1];
      else
        num = std::stoi(temp[1]);
    }

    // compute the available time instant of the next segment
    //auto avail_time = start_time + milliseconds(*server_seg * segment_duration);

    // compute the wait interval to the next available time instant. It may be negative
    //auto wait_intv = avail_time - microsec_clock::local_time();

    // call the tick function
    boost::system::error_code ec;

    //auto timer = std::make_shared<boost::asio::deadline_timer>(res.io_service(), wait_intv);
    auto closed = std::make_shared<bool>();

    res.on_close([closed](uint32_t error_code) {
      std::cout << "Finished rebuffering !!!" << std::endl;
      //timer->cancel();
      *closed = true;
    });

   // timer->async_wait(boost::bind(push_file, timer, &res, closed, ec));
    push_file(&res,closed,ec);
  });

  server.handle("/req_vod/", [](const request &req, const response &res) {
    // get url, e.g. http://127.0.0.1:3002/req_vod/bitrate=2000/num=4 -> bitrate = 2000kbps, number of segments = 4.
    // currently, the special symbol & is not allowed in the url
    std::vector<std::string> strs;
    boost::split(strs, req.uri().path, boost::is_any_of("/"));
    if (strs.size() != 4) {
      std::cout << "[ERROR] : url is incorrect" << std::endl;
      return;
    }

    std::vector<std::string> temp;
    for (int i = 2; i < strs.size(); i++) {
      boost::split(temp, strs[i], boost::is_any_of("="));
      if (temp[0] == "bitrate")
        bitrate = temp[1];
      else
        num = std::stoi(temp[1]);
    }

    // compute the available time instant of the next segment
    //auto avail_time = start_time + milliseconds(*server_seg * segment_duration);

    // compute the wait interval to the next available time instant. It may be negative
    //auto wait_intv = avail_time - microsec_clock::local_time();

    // call the tick function
    boost::system::error_code ec;

    //auto timer = std::make_shared<boost::asio::deadline_timer>(res.io_service(), wait_intv);
    auto closed = std::make_shared<bool>();

    res.on_close([closed](uint32_t error_code) {
     // timer->cancel();
      *closed = true;
    });

   // timer->async_wait(boost::bind(push_file, timer, &res, closed, ec));
    push_file(&res,closed,ec);
  });

  std::cout << "Listening at the address: " << "172.16.165.130"<< std::endl;
  if (server.listen_and_serve(ec, "172.16.165.130", "3002")) {
    std::cerr << "error: " << ec.message() << std::endl;
  }

/*
  if (server.listen_and_serve(ec, tls, "192.168.9.76", "3002")) {
    std::cerr << "error: " << ec.message() << std::endl;
  }
*/
}


#include "process.h"
#include "data.h"
#include "linear.h"
#include "Sample.h"
#include "RIRL.h"
#include "DETree.h"

#include <iostream>
#include <cstdlib>
#include <vector>
#include <algorithm>
#include <random>
#include <chrono>
#include <fstream>
#include <limits>
#include <cfloat>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>

using namespace std;

Process::Process()
{

}

Process::~Process()
{

}

//each sample is a tuple <x,y>
vector<vector<double> > Process::getContinuousSamples(Data &data,vector<double> previousPoint,vector<double> currentPoint){
    int numberOfSamples = data.getNumberOfSamples();

    double startPointX = previousPoint.at(0);
    double startPointY = previousPoint.at(1);

    double endPointX = currentPoint.at(0);
    double endPointY = currentPoint.at(1);

    double deltaX = abs((startPointX - endPointX)) / ((double) (numberOfSamples));
    double deltaY = abs((startPointY - endPointY)) / ((double) (numberOfSamples));

    double xSample = startPointX;
    double ySample = startPointY;

    vector<vector<double> > samplesList;

    for (int i = 0 ; i < numberOfSamples ; i++){
        vector<double> sample;
        sample.push_back(xSample);
        sample.push_back(ySample);

        samplesList.push_back(sample);

        xSample = xSample + deltaX;

        if(deltaX != 0.0){
            ySample = ((startPointY - endPointY) / (startPointX - endPointX)) * xSample
                    + (startPointY - ((startPointY - endPointY) /
                                      (startPointX - endPointX)) * startPointX);
        }else if(deltaX == 0.0 && deltaY != 0.0){
            ySample = ySample + deltaY;
        }else{
            ySample = ySample;
        }
    }

    return samplesList;
}

double Process::getDistanceToPowerTwo(vector <double> sample){
    return (pow ((sample.at(0) - 10), 2) + pow ((sample.at(1) - 10), 2));
}

vector<double> Process::getObsSampleList(Data &data,vector<double> previousPoint,vector<double> currentPoint){
    //for debugging
    //    if(currentPoint == previousPoint){
    //        cout << "we are turnning!" << endl;
    //    }

    //list of <x,y> between two points
    vector< vector<double> > continuousSampleList = getContinuousSamples(data, previousPoint, currentPoint);

    vector<double> obsList;

    // parameters of the normal distribution
    double sigma = data.getSigma();
    double mean = data.getMean();
    double obs;
    double accNoise = 0.0;
    for (int i = 0 ; i < (int)continuousSampleList.size() ; i++){
        vector<double> sample = continuousSampleList.at(i);

        double r2 = getDistanceToPowerTwo(sample);
        double p = data.getP(); // presure in intensity formula

        unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
        default_random_engine generator(seed);
        normal_distribution<double> distribution(mean,sigma);
        double noise = distribution(generator);
        //        double noise = 0.0; // for debugging noise free
        //        cout << (p / (4 * M_PI * r2)) << " + " << noise << endl;

        obs = (p / (4 * M_PI * r2)) + noise;
        accNoise = accNoise + noise * noise;
        obsList.push_back(obs);
    }

    data.updateNoise(accNoise);
    return obsList;
}

//changed
// don't make continus trajectory any more just the discrete one. center to center move
void Process::bulidData(Data &data, int lenghtOfTrajectory, bool forTrainingObs){

    vector<double> initialPoint;
    vector<double> firstPoint;

    vector<int> firstState;
    int initialAction;
    int action;
    vector<int> initialState; //at 0 facing east
    initialState.push_back(0);
    initialState.push_back(0);

    if(forTrainingObs == true){
        initialAction = randomPolicy(data);
    }else{
        //call rational policy here
        map<vector<int>,map<int,double>> ExpertPolicy = data.getExpertPolicy();
        vector<int> listOfActions = data.getListOfActions();
        double max = -DBL_MAX;

        for(int j = 0 ; j < (int)(listOfActions.size()) ; j++ ){
            int a = listOfActions.at(j);
            if(max < ExpertPolicy[initialState][a]){
                max = ExpertPolicy[initialState][a];
                initialAction = a;
            }
        }
        //print the T for debugging
        cout << "[< " << initialState.at(0) << " , "
             << initialState.at(1) << " > , " <<
                initialAction << " ]" << endl;
    }
    initialPoint = getCenterPointInState(initialState.at(0));
    firstPoint = initialPoint;

    firstState = initialState;
    action = initialAction;

    int counter = 0;

    bool done = false;

    while(!done){
        vector<double> secondPoint;

        vector<int> secondState = transitionFunction(data, firstState, action);

        if(firstState == secondState){
            secondPoint = firstPoint;
        }else{
            secondPoint = getCenterPointInState(secondState.at(0));
        }

        vector<double> obs = getObsSampleList(data,firstPoint, secondPoint);
        updateFlatObsList(data,firstState,action,obs,forTrainingObs);

        firstState = secondState;
        firstPoint = secondPoint;
        counter++;

        if(counter == lenghtOfTrajectory){
            done = true;
        }else{
            if(forTrainingObs == true){
                action = randomPolicy(data);
            }else{
                //call rational policy here
                map<vector<int>,map<int,double>> ExpertPolicy = data.getExpertPolicy();
                vector<int> listOfActions = data.getListOfActions();
                double max = -DBL_MAX;

                for(int j = 0 ; j < (int)(listOfActions.size()) ; j++ ){
                    int a = listOfActions.at(j);
                    if(max < ExpertPolicy[firstState][a]){
                        max = ExpertPolicy[firstState][a];
                        action = a;
                    }
                }
                //print the T for debugging
                cout << "[< " << firstState.at(0) << " , "
                     << firstState.at(1) << " > , " <<
                        action << " ]" << endl;
            }
        }
    }
}

// copress that to above function
void Process::updateFlatObsList(Data &data, vector<int> state, int action, vector<double> obs, bool forTrainingObs){
    int numberOfSamples = data.getNumberOfSamples();

    vector<double> time_x = data.getTimeChunk();
    Sample obsTuple;
    Maths::Regression::Linear linearRegression(numberOfSamples, time_x, obs); //Intensity_y = Obs
    double slope =linearRegression.getSlope();
    obsTuple.values.push_back(slope);
    double intercept =linearRegression.getIntercept();
    obsTuple.values.push_back(intercept);
    if(forTrainingObs){
        obsTuple.values.push_back(state.at(0)); // adding state.position
        obsTuple.values.push_back(state.at(1)); // adding state.orientation
        obsTuple.values.push_back(action); // adding action
    }

    obsTuple.p = data.getNormalizerFactorForP(); //uniform
    data.updateFlatObsList(obsTuple);
}

//changed
//just consider the center
//rawstate = S_0 to S_n
vector<double> Process::getCenterPointInState(int rawState){
    vector<double> result;

    double x = (rawState + 1) * 20 +10;
    double y = 30;

    result.push_back(x);
    result.push_back(y);

    return result;
}

vector<int> Process::calcStateBoundaries(Data &data, int state){
    vector<int> result;

    int stepSize = data.getStepSize();
    int remainder = state % stepSize;
    int quotient = state / stepSize;

    int stateLowerX = remainder * stepSize;
    int stateUpperX = remainder  * stepSize + stepSize;
    int stateLowerY = quotient * stepSize - stepSize;
    int stateUpperY = quotient * stepSize;

    result.push_back(stateLowerX);
    result.push_back(stateUpperX);
    result.push_back(stateLowerY);
    result.push_back(stateUpperY);

    return result;
}

//return a random action
//done
int Process::randomPolicy(Data &data){
    vector<int> listOfActions = data.getListOfActions();
    int randomAction = listOfActions.at(getRandomIntNumber(listOfActions.size()));
    return randomAction;
}

//I have to chagne it later
double Process::probablityOfNextStateGivenCurrentStateAction(Data &data,vector<int> nextState, vector<int> currentState, int currentAction){
    double stochasticity = data.getStochasticity();
    vector<int> idealNextState = returnNextState(data,currentState,currentAction);
    double pr = 0.0;
    if (nextState == idealNextState){
        pr = 1.0 - stochasticity;
    }/*else if(areAdj(currentState,nextState) &&
             currentState.at(0) != 0 &&
             currentState.at(0) != data.getNumberOfRawStates()-1){ //if in the middle of hallway
        pr = stochasticity / 2.0; //confirm it with ken
    }else if(areAdj(currentState,nextState)){ //if at the end of the hallway
        pr = stochasticity / 1.0; //confirm it with ken
    }*/else if(nextState == currentState){
        pr = stochasticity;
    }

    return pr;
}

//changed
vector<int> Process::returnNextState(Data &data,vector<int> currentState, int currentAction){
    vector<int> nextState;
    if(/*(currentState.at(1) == 0 && currentAction == -1) ||*/ //facing east going backward
            (currentState.at(1) == 1 && currentAction == 1)){ //facing west going forward
        int nextPosition = currentState.at(0) - 1;
        //check if the next position in the range of possible positions
        if(nextPosition < 0){
            nextPosition = 0;
        }else if(nextPosition > data.getNumberOfRawStates() - 1){
            nextPosition = data.getNumberOfRawStates() - 1;
        }
        nextState.push_back(nextPosition);
        nextState.push_back(currentState.at(1));
    }else if(currentAction == 0){ //turning
        int nextOrientation;
        if(currentState.at(1) == 0){
            nextOrientation = 1;
        }else{
            nextOrientation = 0;
        }
        nextState.push_back(currentState.at(0));
        nextState.push_back(nextOrientation);
    }else if((currentState.at(1) == 0 && currentAction == 1) /*|| //facing east going forward
                                                                                (currentState.at(1) == 1 && currentAction == -1)*/){ //facing west going backward
        int nextPosition = currentState.at(0) + 1;
        //check if the next position (rawState) in the range of possible positions
        if(nextPosition < 0){
            nextPosition = 0;
        }else if(nextPosition > data.getNumberOfRawStates() - 1){
            nextPosition = data.getNumberOfRawStates() - 1;
        }
        nextState.push_back(nextPosition);
        nextState.push_back(currentState.at(1));
    }else{
        cout << "missing something!" << endl;
    }

    return nextState;
}

//changed
vector<int> Process::transitionFunction(Data &data,vector<int> currentState, int currentAction){
    vector<int> nextState;
    double stochasticity = data.getStochasticity();
    //    vector<int> listOfActions = data.getListOfActions();
    double stochasticityIndicator = getRandomDoubleNumber(0,1);

    if(stochasticityIndicator < stochasticity ){
        //        int randomAction = listOfActions.at(getRandomIntNumber(listOfActions.size()));
        //        while(randomAction == currentAction){
        //            randomAction = listOfActions.at(getRandomIntNumber(listOfActions.size()));
        //        }
        //        //generate next state here
        //        nextState = returnNextState(data, currentState, randomAction);
        nextState = currentState;
    }else{
        //generate next state here
        nextState = returnNextState(data, currentState, currentAction);
    }

    return nextState;
}

double Process::getRandomDoubleNumber(int lowerBound, int upperBound){
    double min = (double) lowerBound;
    double max = (double) upperBound;
    return (max - min) * ((double)rand() / (double)RAND_MAX) + min;
}

int Process::getRandomIntNumber(int upperBound){

    return  rand() % upperBound;
}

vector<double> Process::rowNormalizer(vector<int> row){
    vector<double> result;
    int sum = 0;
    for (int i = 0 ; i < (int)row.size() ; i++){
        sum = sum + row.at(i);
    }

    if (sum != 0){
        for (int i = 0 ; i < (int)row.size() ; i++){
            result.push_back((double) row.at(i) / (double) sum);
        }
    }else{
        for (int i = 0 ; i < (int)row.size() ; i++){
            result.push_back(0);
        }
    }

    return result;
}

void Process::trainObs(Data &data){
    vector<double> * low  = data.getLow();
    vector<double> * high = data.getHigh();
    DETree observationModel(data.getFlatObsList(), low, high);
    data.setObsModel(observationModel);
}

//changed
void Process::generateTrajectories(Data &data, int lenghtOfT, bool forTrainingObs){
    data.setSampleLength(lenghtOfT);
    //setting up expert's policy
    if(!forTrainingObs){
        map<vector<int>,map<int,double>> qValuesForExpert = qValueSoftMaxSolver(data, 0.001, data.getExpertWeights());
        constructExpertPolicy(data,qValuesForExpert);
    }
    cout << "Generating data!" << endl;
    data.setNormalizerFactorForP(lenghtOfT);
    bulidData(data, lenghtOfT, forTrainingObs);
    cout << "Data generated!" << endl;
    // claculating noise in relative mean square error fashion
    double rmsNoise = data.getNoise() / ((double)lenghtOfT * (double)data.getNumberOfSamples());
    cout << "Average Noise Added to observation = " << rmsNoise << endl;

    if(forTrainingObs){
        saveObsModel(data);
        cout << "Saving obsModel!\n";
        saveObsModel(data);
        cout << "obsModel saved!\n";
    }
}

//changed
vector<double> Process::getFeatures(Data &data, vector<int> state, int action){
    vector<double> features(data.getNumberOfFeatures(), 0.0);
    int rawState = state.at(0);
    int orientation = state.at(1);
    int activatedFeatureForTurningIndex;
    //    if(rawState == 9){
    //        cout << (data.getNumberOfRawStates() - 1) << endl;
    //    }
    if(action == 0){
        if (rawState < (data.getNumberOfRawStates() - 1) / 2 && orientation == 1){ //first half should face west
            activatedFeatureForTurningIndex = rawState;
            features.at(activatedFeatureForTurningIndex) = 1.0;
        }else if(rawState > (data.getNumberOfRawStates() - 1) / 2 && orientation == 0) { //second half should face east
            activatedFeatureForTurningIndex = data.getNumberOfRawStates() - rawState - 1;
            features.at(activatedFeatureForTurningIndex) = 1.0;
        }else if(rawState == (data.getNumberOfRawStates() - 1) / 2){
            activatedFeatureForTurningIndex = (data.getNumberOfRawStates() - 1) / 2;
            features.at(activatedFeatureForTurningIndex) = 1.0;
        }else{
            //            cout << "I missing some Feature!!" << endl;
            activatedFeatureForTurningIndex = features.size()-2;
            features.at(activatedFeatureForTurningIndex) = 1.0;
        }
    }else{
        //construct the feature vector here
        //last feature is for moving forward
        //I may change this part if i want to add moving backward!
        //I have to check if I am bangging the wall it should be no reward
        if(rawState == 0 || rawState == data.getNumberOfRawStates() - 1){
            //by not activating feature ==> no reward by going into the wall
            //            activatedFeatureForTurningIndex = features.size()-2;
            //            features.at(activatedFeatureForTurningIndex) = 1.0;
        }else{
            features.at(features.size()-1) = 1.0;
        }
    }

    return features;
}

//new
map<vector<int>,map<int,double>> Process::qValueSoftMaxSolver(Data &data, double err, vector<double> weights){

    map<vector<int>,double> V; //[State] //[State] ==> vector<int>
    map<vector<int>,map<int,double>> Q; //[State][Action] //[State] ==> vector<int> & [Action] ==> int
    vector<vector<int>> listOfStates = data.getListOfStates();
    vector<int> listOfActions = data.getListOfActions();

    for(int i = 0 ; i < (int)(listOfStates.size()) ; i++ ){ //foreach (State s ; model.S()) {
        vector<int> s = listOfStates.at(i);
        V[s] = 0.0;

        for(int j = 0 ; j < (int)(listOfActions.size()) ; j++ ){//foreach (a; model.A(s)) {
            int a = listOfActions.at(j);
            Q[s][a] = 0;
        }
    }
    //    V = V.rehash;
    //    Q = Q.rehash;
    double delta = 0;
    int iteration = 0; //size_t
    while (true) {
        delta = 0;

        for(int i = 0 ; i < (int)(listOfStates.size()) ; i++ ){ //foreach (State s ; model.S()) {
            vector<int> s = listOfStates.at(i);

            for(int j = 0 ; j < (int)(listOfActions.size()) ; j++ ){//foreach (a; model.A(s)) {
                int a = listOfActions.at(j);
                double r = reward(data,s,a,weights);//add reward function later
                //double[State] T = model.T(s, a);

                double expected_rewards = 0;
                for (int k = 0 ; k < int(listOfStates.size()) ; k++){ //foreach (s_prime, p; T){
                    vector<int> s_prime = listOfStates.at(k);
                    //p ==> probabilityOfTheNextState
                    double p  = probablityOfNextStateGivenCurrentStateAction(data,s_prime, s, a);
                    expected_rewards += p*V[s_prime];
                }

                Q[s][a] = r + data.getGamma() * expected_rewards;
            }
        }

        map<vector<int>,double> v;
        v = V;
        for(int i = 0 ; i < (int)(listOfStates.size()) ; i++ ){ //foreach (State s ; model.S()) {
            vector<int> s = listOfStates.at(i);
            double maxx = -DBL_MAX;
            for(int j = 0 ; j < (int)(listOfActions.size()) ; j++ ){//foreach (a; model.A(s)) {
                int a = listOfActions.at(j);
                maxx = max(maxx, Q[s][a]);
            }

            double e_sum = DBL_MIN;

            for(int j = 0 ; j < (int)(listOfActions.size()) ; j++ ){//foreach (a; model.A(s)) {
                int a = listOfActions.at(j);
                e_sum += exp(Q[s][a] - maxx);
            }

            v[s] = maxx - log(e_sum);
            delta = max(delta, abs(V[s] - v[s]));
        }

        V = v;
        //cout << "Current Iteration: " <<  delta << endl;

        if (delta < err || iteration > data.getMaxIter()) {
            map<vector<int>,map<int,double>> returnval; //[state][action]

            //doing this part to avoid overfelow problem when do e to the power in softmax!!
            for(int i = 0 ; i < (int)(listOfStates.size()) ; i++ ){ //foreach(state, actvalue; Q) {
                vector<int> state = listOfStates.at(i);

                for(int j = 0 ; j < (int)(listOfActions.size()) ; j++ ){//foreach(action, value; actvalue) {
                    int action = listOfActions.at(j);
                    double value = Q[state][action];
                    returnval[state][action] = value - v[state];
                }
            }
            //				writeln("Finished Q-Value");
            return returnval;
        }
        iteration ++;
    }
}

//new
//this function calculate the reward reciving state and action
double Process::reward(Data &data,vector<int> state, int action, vector<double> weights){
    double reward;
    vector<double> features = getFeatures(data,state,action);
    reward = calcInnerProduct(features,weights);
    return reward;
}

//construct the expert policy using the actual weights
//using softmax
void Process::constructExpertPolicy(Data &data, map<vector<int>,map<int,double>> Q_value){
    map<vector<int>,map<int,double>> policy; //[State][Action]
    vector<vector<int>> listOfStates = data.getListOfStates();
    vector<int> listOfActions = data.getListOfActions();

    for(int i = 0 ; i < (int)(listOfStates.size()) ; i++ ){ //foreach (State s ; model.S()) {
        vector<int> s = listOfStates.at(i);
        double normalizer = 0.0; //[State]

        for(int j = 0 ; j < (int)(listOfActions.size()) ; j++ ){//foreach (Action a; model.A(s)) {
            int a = listOfActions.at(j);
            normalizer = normalizer + exp(Q_value[s][a]);
        }

        for(int j = 0 ; j < (int)(listOfActions.size()) ; j++ ){//foreach (Action a; model.A(s)) {
            int a = listOfActions.at(j);
            policy[s][a] = exp(Q_value[s][a]) / normalizer;
        }
    }
    data.setExpertPolicy(policy);
}

double Process::calcInnerProduct(vector<double> v1, vector<double> v2){

    double result = 0.0;
    if(int(v1.size()) != int(v2.size())){
        cout << "Something is wrong in Process::calcInnerProduct" << endl;
        return 0.0;
    }
    for (int i = 0 ; i < int(v1.size()) ; i++){
        result = result + v1.at(i) * v2.at(i);
    }
    return result;
}

void Process::printPolicy(map<vector<int>,map<int,double>> policy){
    //    std::map< std::string, std::map<std::string, std::string> > m;

    for (auto i : policy){
        for (auto j : i.second){
            cout << " < " << i.first.at(0) << " , " << i.first.at(1) << " > , " << j.first << " ==> Pr = " << j.second << endl;
        }
        cout << "--------------------------------------------" << endl;
    }

}

vector<double> Process::multiply(double scalar, vector<double> v){
    vector<double> result;
    for (int i = 0 ; i < int(v.size()) ; i++){
        result.push_back(scalar * v.at(i));
    }
    return result;
}

vector<double> Process::divide(double scalar, vector<double> v){
    vector<double> result;
    for (int i = 0 ; i < int(v.size()) ; i++){
        result.push_back(v.at(i) / scalar);
    }
    return result;
}

vector<double> Process::add(vector<double> v1, vector<double> v2){
    vector<double> result;
    for (int i = 0 ; i < int(v1.size()) ; i++){
        result.push_back(v1.at(i) + v2.at(i));
    }
    return result;
}

double Process::l1norm(vector<double> v){
    double returnval = 0;
    for (int i = 0 ; i < int(v.size()) ; i++){
        returnval += abs(v.at(i));
    }
    return returnval;
}

double Process::l2norm(vector<double> v){
    return sqrt(calcInnerProduct(v,v));
}

void Process::loadObsModel(Data &data){
    vector<Sample> flatObsList;
    string fileAddress = data.getObsModelAddress();
    std::ifstream ifs(fileAddress);
    boost::archive::text_iarchive ia(ifs);
    ia >> flatObsList;
    vector<double> * low  = data.getLow();
    vector<double> * high = data.getHigh();
    DETree observationModel(flatObsList, low, high);
    data.setObsModel(observationModel);
}

void Process::saveObsModel(Data &data){
    vector<Sample> flatObsList = data.getFlatObsList();
    ofstream myfile;
    string fileAddress = data.getObsModelAddress();
    myfile.open (fileAddress.c_str());
    std::ofstream ofs(fileAddress.c_str());
    boost::archive::text_oarchive oa(ofs);
    // write class instance to archive
    oa << flatObsList;
}

//changed
bool  Process::areAdj(vector<int> currentState, vector<int> nextState){
    int rawState1 = currentState.at(0);
    int rawState2 = nextState.at(0);
    if(rawState1 - rawState2 == 0){
        return true;
    }else if(rawState2 - rawState1 == 1 && currentState.at(1) == nextState.at(1)){
        return true;
    }
    return false;
}

vector<double> Process::normalize(vector<double> v){
    double sum = 0;
    vector<double> result;
    for (int i = 0 ; i < int(v.size()) ; i++){
        sum = sum + v.at(i);
    }
    for (int i = 0 ; i < int(v.size()) ; i++){
        result.push_back(v.at(i) / sum);
    }
    return result;
}

vector<double> Process::getFeatureVectorOfT(Data &data,vector<vector<int>> t){
    vector<double> fv(data.getNumberOfFeatures(),0.0);
    for(int i = 0 ; i < int(t.size()) ; i++){
        vector<int> state;
        state.push_back(t.at(i).at(0));
        state.push_back(t.at(i).at(1));
        int action = t.at(i).at(2);
        fv = add(fv, getFeatures(data,state,action));
    }
    return fv;
}

void Process::saveDeterministicObsModel(Data &data){
    vector<vector<int>> listOfStates = data.getListOfStates();
    vector<int> listOfActions = data.getListOfActions();

    for(int i = 0 ; i < int(listOfStates.size()) ; i++){
        vector<int> firstState = listOfStates.at(i);
        for(int j = 0 ; j < int(listOfActions.size()) ; j++){
            int action = listOfActions.at(j);
            vector<int> secondState = returnNextState(data,firstState,action);
            vector<double> firstPoint = getCenterPointInState(firstState.at(0));
            vector<double> secondPoint = getCenterPointInState(secondState.at(0));
            vector<double> obs = getObsSampleList(data,firstPoint, secondPoint);
            updateFlatObsList(data,firstState,action,obs,true);
        }
    }
    vector<Sample> flatObsList = data.getFlatObsList();
    ofstream myfile;
    string fileAddress = data.getObsModelAddress();
    myfile.open (fileAddress.c_str());
    std::ofstream ofs(fileAddress.c_str());
    boost::archive::text_oarchive oa(ofs);
    // write class instance to archive
    oa << flatObsList;
}

void Process::loadDeterministicObsModel(Data &data){
    vector<Sample> dObsModel;
    string fileAddress = data.getObsModelAddress();
    std::ifstream ifs(fileAddress);
    boost::archive::text_iarchive ia(ifs);
    ia >> dObsModel;
    data.setDeterministicObsModel(dObsModel);
}

void Process::saveClusteringObsModel(Data &data, int dataPerStateActionPair){
    vector<vector<int>> listOfStates = data.getListOfStates();
    vector<int> listOfActions = data.getListOfActions();
    int numberOfSamples = data.getNumberOfSamples();
    vector<Sample> clusteringObsModel;
    for(int i = 0 ; i < int(listOfStates.size()) ; i++){
        vector<int> firstState = listOfStates.at(i);
        for(int j = 0 ; j < int(listOfActions.size()) ; j++){
            int action = listOfActions.at(j);
            Sample centroid;
            centroid.p = 0; //I not using p in here so just assign zero
            double slopeSum = 0.0;
            double intercepSum = 0.0;

            for(int k = 0 ; k < dataPerStateActionPair ; k++){
                // I was using returnNextState(data,firstState,action)
                // I have to use transitionFunction(data,firstState,action);
                // to handle stochastic T in observation
                //                vector<int> secondState = returnNextState(data,firstState,action);
                vector<int> secondState = transitionFunction(data,firstState,action);
                vector<double> firstPoint = getCenterPointInState(firstState.at(0));
                vector<double> secondPoint = getCenterPointInState(secondState.at(0));
                vector<double> obs = getObsSampleList(data,firstPoint, secondPoint);
                vector<double> time_x = data.getTimeChunk();

                Maths::Regression::Linear linearRegression(numberOfSamples, time_x, obs); //Intensity_y = Obs
                double slope =linearRegression.getSlope();
                slopeSum = slopeSum + slope;
                double intercept =linearRegression.getIntercept();
                intercepSum = intercepSum + intercept;
            }
            centroid.values.push_back(slopeSum / dataPerStateActionPair);
            centroid.values.push_back(intercepSum / dataPerStateActionPair);
            centroid.values.push_back(firstState.at(0));
            centroid.values.push_back(firstState.at(1));
            centroid.values.push_back(action);
            clusteringObsModel.push_back(centroid);
        }
    }

    ofstream myfile;
    string fileAddress = data.getObsModelAddress();
    myfile.open (fileAddress.c_str());
    std::ofstream ofs(fileAddress.c_str());
    boost::archive::text_oarchive oa(ofs);
    // write class instance to archive
    oa << clusteringObsModel;
}

void Process::loadClusteringObsModel(Data &data){
    vector<Sample> cObsModel;
    string fileAddress = data.getObsModelAddress();
    std::ifstream ifs(fileAddress);
    boost::archive::text_iarchive ia(ifs);
    ia >> cObsModel;
    data.setClusteringObsModel(cObsModel);
}


/*
 * Copyright 2014, Andrea Del Prete, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-torque-control 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 Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef __sot_torque_control_NumericalDifference_H__
#define __sot_torque_control_NumericalDifference_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#if defined (WIN32)
#  if defined (numerical_difference_EXPORTS)
#    define SOTNUMERICALDIFFERENCE_EXPORT __declspec(dllexport)
#  else
#    define SOTNUMERICALDIFFERENCE_EXPORT __declspec(dllimport)
#  endif
#else
#  define SOTNUMERICALDIFFERENCE_EXPORT
#endif

//#define VP_DEBUG 1        /// enable debug output
//#define VP_DEBUG_MODE 20

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* HELPER */
#include <boost/circular_buffer.hpp>
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/stop-watch.hh>

/* Polynomial estimators */
#include <sot/torque_control/utils/lin-estimator.hh>
#include <sot/torque_control/utils/quad-estimator.hh>

namespace dynamicgraph {
  namespace sot {
    namespace torque_control {

      /**
        * This Entity takes as inputs a signal and estimates its first two time derivatives.
        */
      class SOTNUMERICALDIFFERENCE_EXPORT NumericalDifference
          :public ::dynamicgraph::Entity
      {
        DYNAMIC_GRAPH_ENTITY_DECL();

      public:  /* --- SIGNALS --- */
        DECLARE_SIGNAL_IN(x,                 dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(x_filtered,       dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(dx,               dynamicgraph::Vector);
        DECLARE_SIGNAL_OUT(ddx,              dynamicgraph::Vector);

        /// The following inner signals are used because this entity has some output signals
        /// whose related quantities are computed at the same time by the same algorithm
        /// To avoid the risk of recomputing the same things twice, we create an inner signal that groups together
        /// all the quantities that are computed together. Then the single output signals will depend
        /// on this inner signal, which is the one triggering the computations.
        /// Inner signals are not exposed, so that nobody can access them.

        /// This signal contains the estimated positions, velocities and accelerations.
        DECLARE_SIGNAL_INNER(x_dx_ddx,              dynamicgraph::Vector);

      protected:

        double m_dt;      /// sampling timestep of the input signal
        double m_delay;   /// delay introduced by the estimation
        int x_size;
        /// std::vector to use with the filters
        /// All the variables whose name contains 'filter' are outputs of the filters
        std::vector<double> m_ddx_filter_std;  /// 2nd derivative
        std::vector<double> m_dx_filter_std;   /// 1st derivative
        std::vector<double> m_x_filter_std;    /// filtered output
        std::vector<double> m_x_std;           /// x signal

        /// polynomial-fitting filters
        PolyEstimator* m_filter;

      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        /** --- CONSTRUCTOR ---- */
        NumericalDifference( const std::string & name );

        /** Initialize the NumericalDifference.
         * @param timestep Period (in seconds) after which the sensors' data are updated.
         * @param sigSize  Size of the input signal.
         * @param delay    Delay (in seconds) introduced by the estimation.
         *                 This should be a multiple of timestep.
         * @param polyOrder Order of the approximation polynomial. int 1 or 2.
         * @note The estimationDelay is half of the length of the window used for the
         * polynomial fitting. The larger the delay, the smoother the estimations.
         */
        void init(const double &timestep, const int& sigSize,
                  const double &delay, const int& polyOrder);

      protected:
        void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
        {
	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
        }

      public: /* --- ENTITY INHERITANCE --- */
        virtual void display( std::ostream& os ) const;

      }; // class NumericalDifference

    } // namespace torque_control
  } // namespace sot
} // namespace dynamicgraph

#endif // #ifndef __sot_torque_control_NumericalDifference_H__


#ifndef icvCvMatData_h
#define icvCvMatData_h

#include "OpenICV/Core/icvDataObject.h"
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_pod.hpp>
#include <msgpack.hpp>
#include <string>
#include <opencv2/core/mat.hpp>
#include <opencv2/opencv.hpp>

using namespace msgpack;
using namespace std;
using namespace icv::core;

namespace icv
{
    namespace data
    {

          class icvCvMatData :public icv::core::icvDataObject
        {
        public:
            typedef cv::Mat UType;
            typedef icvCvMatData SelfType;
         struct ImageRecStruct
        {
            string imagename;
            string another_name;
            MSGPACK_DEFINE(imagename,another_name);
        } ;
        public:
        icvCvMatData(int rows, int cols, int type) :
            _init_type(type), _init_sizes {rows, cols} {

            Reserve();
            }

        icvCvMatData() :
         _init_type(CV_8UC3), _init_sizes {400, 600} {

           Reserve();
        }
        icvCvMatData(UType data_ini):_init_type(CV_8UC3), _init_sizes {400, 600}
            {
                Reserve();

                setvalueDeep(data_ini);

            }
        icvCvMatData(const vector<int>& sizes, int type)
            : _init_sizes(sizes), _init_type(type) {  Reserve();}

        virtual void Reserve() ICV_OVERRIDE
        {

            if (!_data) {_data = new cv::Mat(_init_sizes, _init_type);is_reserved=true;}
        }
       UType& getvalue()
        {

        return *_data;

        }

       virtual void Dispose() ICV_OVERRIDE
        {
            if (_data) delete _data;
            _data = ICV_NULLPTR;
        }

        virtual Uint64 GetActualMemorySize() ICV_OVERRIDE
        {
            if (_data)
            {
                if (_data->isContinuous()) return _data->total() * _data->elemSize();
                else return _data->step[0] * _init_sizes[0];
            }
            else return 0;
        }

            virtual void Serialize(std::stringstream& out, const uint32_t& version) const ICV_OVERRIDE
            {
                 std::stringstream buff ;
                pack(buff, _data_rec);

               // out <<this->GetSourceTime()<<" "<<buff.size()<<" "<< buff.data();
                out <<this->GetSourceTime()<<" "<<buff.str().size()<<" " << buff.str().data();

             //   ICV_LOG_INFO<<"data:"<<_data_rec.imagename <<"RECORD LENGTH"<<buff.size();

            }
            virtual void Deserialize(std::stringstream& in, const uint32_t& version) ICV_OVERRIDE
            {

            unpacked result;
            size_t len;
            string buff;
            time_t timestamp;

            in >> timestamp>>len >> buff;

            this->SetSourceTime(timestamp);
        //    ICV_LOG_INFO<<"Dese STAMP:"<<timestamp;

            if(len==0) _data=ICV_NULLPTR;
            else
            {
           // ICV_LOG_INFO<<"TIME length:"<<len;

           // ICV_LOG_INFO<<"TIME STAMP:"<<timestamp;
            unpack(&result, buff.c_str(), len);
            object obj = result.get();
            ImageRecStruct temp_V;
            obj.convert(temp_V);

            //ImageRecStruct _image_number=temp_V;
            //ICV_LOG_INFO<<"name STAMP:"<<temp_V.imagename;

            _data_rec.imagename=temp_V.imagename;

           }

            }

        string getImageName()
        {
            return _data_rec.imagename;
        }

            virtual icv::core::icvDataObject* DeepCopy() ICV_OVERRIDE
        {
            icvCvMatData* copy = new icvCvMatData(_init_sizes, _init_type);

            copy->SetSourceTime (_sourceTime );
            //copy->setimagename(_sourceTime);

            copy->_init_sizes = _init_sizes;
            copy->_init_type = _init_type;

            copy->Reserve();
            // *(copy->_data)=_data->clone();
            copy->_data_rec.imagename=_data_rec.imagename;
            _data->copyTo(*(copy->_data));
            return copy;
        }

         virtual void CopyTo(icvDataObject* dst)  ICV_OVERRIDE
         {

             SelfType* copy = static_cast< SelfType*>(dst);

            copy->SetSourceTime (_sourceTime );
            //copy->setimagename(_sourceTime);
            copy->_init_sizes = _init_sizes;
            copy->_init_type = _init_type;
            copy->_data_rec.imagename=_data_rec.imagename;
            copy->Reserve();

            *(copy->_data) = _data->clone();

         };
            void setvalue(const UType& data)
            {

            _assign_shallow(data);

            time_t temp_time=icvTime::time_us();
            _data_rec.imagename=to_string(temp_time);
 //ICV_LOG_INFO<<"SETOUT VALUE"<<temp_time;
            this->SetSourceTime(temp_time);
            //_image_name=to_string(temp_time);

            }
         void setvalueDeep(const UType& data)
            {

            _assign(data);

            time_t temp_time=icvTime::time_us();
            _data_rec.imagename=to_string(temp_time);
 //ICV_LOG_INFO<<"SETOUT VALUE"<<temp_time;
            this->SetSourceTime(temp_time);
            //_image_name=to_string(temp_time);

            }

                    icvCvMatData * get_Ptr()
        {

            return static_cast< icvCvMatData*> (this);

        }
            virtual std::string Print() ICV_OVERRIDE
        {
            string name = "cv::Mat shape:(";
            for (auto iter = _init_sizes.begin(); iter != _init_sizes.end(); iter++)
            {
                if (iter == _init_sizes.begin())
                    name.append(to_string(*iter));
                else name.append(", " + to_string(*iter));
            }
            name.append(")");
            return name;
        }

          //  operator cv::Mat&() { return *_data; }
          //  operator const cv::Mat&() const { return *_data; }

           // UType* operator->() { return _data; }
           // const UType* operator->() const { return _data; }

            icvCvMatData& operator = (const UType& data)
            {
               setvalueDeep(data);

                return *this;
            }

                private:
            cv::Mat * _data = ICV_NULLPTR;
            string _image_name;
            ImageRecStruct _data_rec ;
            // Params only for initialization
            std::vector<int> _init_sizes;
            int _init_type;
            vector<ImageRecStruct> _packdata;

        private:

        void _assign(const cv::Mat& data)
        {
            // Copy properties
            for (int i = 0; i < data.dims; i++)
                if (i < _init_sizes.size())
                    _init_sizes[i] = data.size[i];
                else
            _init_sizes.push_back(data.size[i]);
            _init_type = data.type();

            // Copy data
            *_data = data.clone();
        }

                void _assign_shallow(const cv::Mat& data)
        {
            // Copy properties
            for (int i = 0; i < data.dims; i++)
                if (i < _init_sizes.size())
                    _init_sizes[i] = data.size[i];
                else
                    _init_sizes.push_back(data.size[i]);
            _init_type = data.type();

            // Copy data
            *_data = data;
        }

        };
    }
}
#endif


/* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */
/*
 * Copyright (c) 2015 Klaus Schneider, University of Bamberg, Germany
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation;
 *
 * 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
 *
 * Author: Klaus Schneider <klaus.schneider@uni-bamberg.de>
 */

#ifndef NFD_DAEMON_FW_INTERFACE_ESTIMATION_HPP
#define NFD_DAEMON_FW_INTERFACE_ESTIMATION_HPP

#include "bandwidth-estimator.hpp"
#include <boost/chrono/duration.hpp>
#include <ndn-cxx/util/time.hpp>
#include <string>
#include "loss-estimator-time-window.hpp"
#include "rtt-estimator2.hpp"
#include "strategy-requirements.hpp"
#include <cstddef>
#include "../../core/common.hpp"

namespace nfd {
namespace fw {

/**
 * A class that combines all interface estimators (loss, delay & bandwidth) for easier handling.
 */
class InterfaceEstimation
{
public:

  // Some constants for the interface estimators
  const static int DEFAULT_INTEREST_LIFETIME = 2000;
  const static int CALCULATION_WINDOW_IN_MS = 5000;

  InterfaceEstimation(
      time::milliseconds interestLifetime = time::milliseconds(DEFAULT_INTEREST_LIFETIME),
      time::milliseconds calculationWindow = time::milliseconds(CALCULATION_WINDOW_IN_MS));

public:

  /**
   * Adds a sent interest to the loss estimator
   *
   * @param name The name of the Interest that should be added.
   */
  void addSentInterest(std::string name);

  /**
   * Removes a sent interest from the loss estimator (e.g. if it was tainted)
   *
   * @param name The name of the Interest that should be removed.
   */
  void removeSentInterest(std::string name);

  /**
   * Adds a satisfied interest to both loss and bandwidth estimators
   *
   * @param sizeInByte The size of the satisfied data packet's workload.
   * @param name The name of the satisfied data packet
   */
  void addSatisfiedInterest(size_t sizeInByte, std::string name);

  /**
   * Adds an rtt measurement to the delay estimator
   *
   * @param durationMicroSeconds The time it took from sending the Interest till its return in microseconds
   */
  void addRttMeasurement(time::microseconds durationMicroSeconds);

  /**
   * @param type The type of the requirement for which the value is requested.
   * @returns the current value for the type.
   * @returns -1 if the type is invalid.
   */
  double getCurrentValue(RequirementType type);

private:

  RttEstimator2 rtt; // an estimator for delay
  LossEstimatorTimeWindow loss; // an estimator for loss
  BandwidthEstimator bw; // an estimator for bandwidth

};

}
}

#endif


#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

#include <cmath>
#include <cstring>
#include <memory>
#include <vector>
#include <limits>
#include <algorithm>
#include <utility>
#include <iterator>

#include "statespace.hpp"

using namespace boost::numeric::ublas;

class ChangeEvent {
  /* struct (supporting comparison) representing
     a change-of-active-SSMs event. */
public:
  int t;
  int i_ssm;
  bool is_start;
  friend bool operator< (const ChangeEvent &c1, const ChangeEvent &c2);
};

bool operator< (const ChangeEvent &c1, const ChangeEvent &c2) {
    if (c1.t < c2.t) {
      return true;
    } else if (c1.t == c2.t) {
      // ends come before starts
      return c1.is_start > c2.is_start;
    }
    return false;
  }

int active_set_dimension(const std::vector<StateSpaceModel *> & ssms, vector<int> & active_set) {
  vector<int>::const_iterator it;
  int dimension = 0;
  for (it = active_set.begin(); it < active_set.end(); ++it) {
    if (*it < 0) continue;
    StateSpaceModel *ssm = ssms[*it];
    if (!ssm) continue;
    dimension += ssm->max_dimension;
  }
  return dimension;
}

TransientCombinedSSM::TransientCombinedSSM(\
       std::vector<StateSpaceModel *> & ssms, const vector<int> & start_idxs,
       const vector<int> & end_idxs, const std::vector<const double * > & scales,
       double obs_noise) : ssms(ssms), start_idxs(start_idxs),
			   end_idxs(end_idxs), scales(scales),
			   obs_noise(obs_noise), n_ssms(ssms.size()),
			   active_ssm_cache1_k(-1), active_ssm_cache2_k(-1) {

  this->ssms_tmp.resize(this->n_ssms);
  this->n_steps = *(max_element(end_idxs.begin(), end_idxs.end()));

  /* Compute a list of ChangeEvents, each representing the
   * activation or deactivation of a component SSM. The
   * list is sorted by the timestep at which the event
   * occurs (and secondarily, with deactivation events at
   * a timestep before activation events). */
  std::vector<ChangeEvent> events(start_idxs.size() + end_idxs.size());
  for (unsigned i=0; i < n_ssms; ++i) {
    events[2*i].t = start_idxs[i];
    events[2*i].i_ssm = i;
    events[2*i].is_start = true;

    events[2*i+1].t = end_idxs[i];
    events[2*i+1].i_ssm = i;
    events[2*i+1].is_start = false;
  }
  std::sort(events.begin(), events.end());

  /*
     Build a sorted list of changepoints (timesteps at which the set of active
     SSMs changes), along with the set of active SSMs at each changepoint.
     Also compute the max total dimension of the state space, by computing the
     dimension of each active set as we add it to the active_sets matrix.
   */
  // allocate the active_sets matrix by first computing the largest number of
  // SSMs that might be active at one time
  unsigned int n_active = 0;
  unsigned int max_active = 0;
  for(std::vector<ChangeEvent>::const_iterator idx = events.begin(); idx < events.end(); ++idx) {
    if (idx->is_start) n_active++; else n_active--;
    if (n_active > max_active) max_active = n_active;
  }
  active_sets.resize(2*n_ssms, max_active);

  vector<int> active_set(max_active);
  for (unsigned i=0; i < max_active; ++i) active_set(i) = -1;
  int t_prev = events[0].t, max_dimension = 0, i=0;
  std::vector<ChangeEvent>::const_iterator idx;
  for(i=0, idx = events.begin();
      idx < events.end(); ++idx) {

    // printf("event t %d i_ssm %d start %s\n", idx->t, idx->i_ssm, idx->is_start ? "true" : "false");

    // if this represents a change, add a new changepoint
    if (idx->t != t_prev) {
      changepoints.push_back(t_prev);

      // printf(" change detected (t_prev %d), copying to active_set %d\n", t_prev, i);

      // copy the current active set of SSMs
      // into the active_sets matrix (terminated with -1)
      for (unsigned k=0, j=0; k < max_active; ++k) {
	if (active_set(k) >= 0) {
	  active_sets(i, j++) = active_set(k);
	}
	if (j < max_active) {
	  active_sets(i, j) = -1;
	}
      }

      /*printf("matr active set(%d, ...) is", i);
      for(int tmpi=0; tmpi < max_active; ++tmpi) {
	printf(" %d", active_sets(i, tmpi));
      }
      printf("\n");*/

      t_prev = idx->t;
      i++;

      int current_dim = active_set_dimension(this->ssms, active_set);
      if (current_dim > max_dimension) max_dimension = current_dim;
    }

    // update the active set: add the new SSM if this
    // is a start event, otherwise remove it from
    // the active set.
    vector<int>::iterator it;
    if (idx->is_start) {
      it = std::find (active_set.begin(), active_set.end(), -1);
      if (it == active_set.end()) {
	printf("ERROR: need to start new SSM but no room in active_set vector!\n");
	exit(-1);
      } else {
	*it = idx->i_ssm;
      }
    } else {
      it = std::find (active_set.begin(), active_set.end(), idx->i_ssm);
      if (it == active_set.end()) {
	printf("ERROR: trying to remove SSM %d but it is not present in the active_set vector!\n", idx->i_ssm);
	exit(-1);
      } else {
	*it = -1;
      }
    }

  }

  this->max_dimension = max_dimension;
  this->is_cssm = false;
}

TransientCombinedSSM::~TransientCombinedSSM() {

  return;
};

int TransientCombinedSSM::active_set_idx(int k) {
  if (k == this->active_ssm_cache1_k)
    return this->active_ssm_cache1_v;
  else if (k == this->active_ssm_cache2_k)
    return this->active_ssm_cache2_v;

  std::vector<int>::iterator it = std::upper_bound(this->changepoints.begin(), this->changepoints.end(), k);
  int i = it - this->changepoints.begin() - 1;

  /*printf("active set at time k=%d is %d, changepoints %d %d %d %d\n", k, i,
	 i-1 >= 0 ? this->changepoints[i-1] : -1,
	 i >= 0 ? this->changepoints[i] : -1,
	 i+1 < this->changepoints.size() ? this->changepoints[i+1] : -1,
	 i+2 < this->changepoints.size() ? this->changepoints[i+2] : -1);*/

  this->active_ssm_cache2_k = this->active_ssm_cache1_k;
  this->active_ssm_cache2_v = this->active_ssm_cache1_v;
  this->active_ssm_cache1_k = k;
  this->active_ssm_cache1_v = i;
  return i;
}

unsigned int TransientCombinedSSM::state_size_at_timestep(unsigned int k) {

  unsigned int total_state_size = 0;

  int asidx = this->active_set_idx(k);
  if (asidx < 0) {
    return 0;
  }

  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    int i_ssm = *it;
    StateSpaceModel * ssm = this->ssms[i_ssm];
    if (!ssm) continue;

    unsigned int state_size = ssm->max_dimension;
    total_state_size += state_size;
  }
  return total_state_size;
}

int TransientCombinedSSM::apply_transition_matrix(const double * x, int k, double * result) {

  // first, loop over ssms active at the *previous*
  // timestep in order to cache the location of each
  // ssm in the previous state space.
  int j = 0;
  int asidx_prev = this->active_set_idx(k-1);
  int asidx = this->active_set_idx(k);
  bool same_active_set = (asidx==asidx_prev);

  if (k == 0) {
    printf("ERROR: requested transition INTO timestep 0 (invalid)!");
    exit(-1);
  }

  if (asidx < 0) {
    return 0;
  }

  if (!same_active_set) {
    matrix_row < matrix<int> > old_ssm_indices = row(this->active_sets, asidx_prev);
    for (matrix_row < matrix<int> >::const_iterator it = old_ssm_indices.begin();
	 it < old_ssm_indices.end() && *it >= 0; ++it) {

      // skip any null SSMs
      int i_ssm = *it;
      StateSpaceModel * ssm = this->ssms[i_ssm];
      if (!ssm) continue;

      this->ssms_tmp[i_ssm] = j;
      j += ssm->max_dimension;
    }
  }

  //printf("transition to time %d (asidx %d, %d):\n",  k, asidx_prev, asidx);

  // now apply the transition to the current time
  int i=0;
  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    // skip any null SSMs
    int i_ssm = *it;
    StateSpaceModel * ssm = this->ssms[i_ssm];
    if (!ssm) continue;

    unsigned int state_size = ssm->max_dimension;
    if (this->start_idxs[i_ssm] == k) {
      /* new ssms just get filled in as zero
         (prior means will be added by the
         transition_bias operator) */
      for (unsigned j=i; j < i+state_size; ++j) {
	result[j] = 0;
      }
      //printf("   new ssm %d active from %d to %d\n", i_ssm, i, i+state_size);

    } else {
      /* this ssm is persisting from the
       * previous timestep, so just run the
       * transition */
      unsigned int j = same_active_set ? i : this->ssms_tmp[i_ssm];

      ssm->apply_transition_matrix(x+j, k-this->start_idxs[i_ssm], result+i);
      //printf("   transitioning ssm %d, prev %d, in state %d to %d (sidx %d eidx %d)\n", i_ssm, j, i, i+state_size, this->start_idxs[i_ssm], this->end_idxs[i_ssm]);
    }
    i += state_size;
  }
  return i;
}

int TransientCombinedSSM::apply_transition_matrix( const matrix<double> &X,
						   unsigned int x_row_offset,
						   int k,
						   matrix<double> &result,
						   unsigned int r_row_offset,
						   unsigned int n) {
  int j = x_row_offset;
  int asidx_prev = this->active_set_idx(k-1);
  int asidx = this->active_set_idx(k);
  bool same_active_set = (asidx==asidx_prev);

  if (asidx < 0) {
    return 0;
  }

  if (!same_active_set) {
    matrix_row < matrix<int> > old_ssm_indices = row(this->active_sets, asidx_prev);
    for (matrix_row < matrix<int> >::const_iterator it = old_ssm_indices.begin();
	 it < old_ssm_indices.end() && *it >= 0; ++it) {

      // skip any null SSMs
      int i_ssm = *it;
      StateSpaceModel * ssm = this->ssms[i_ssm];
      if (!ssm) continue;

      this->ssms_tmp[i_ssm] = j;
      j += ssm->max_dimension;
    }
  }

  int i=x_row_offset;
  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    int i_ssm = *it;
    StateSpaceModel * ssm = this->ssms[i_ssm];
    if (!ssm) continue;

    unsigned int state_size = ssm->max_dimension;
    if (this->start_idxs[i_ssm] == k) {
      /* new ssms just get filled in as zero
         (prior means will be added by the
         transition_bias operator) */
      for (unsigned j=i; j < i+state_size; ++j) {
	for (unsigned jj=0; jj < n; ++jj) result(j, jj) = 0;
      }
    } else {
      unsigned int j = same_active_set ? i : this->ssms_tmp[i_ssm];
      //printf("MATR transitioning ssm %d, prev %d, in state %d to %d (sidx %d eidx %d)\n", i_ssm, j, i, i+state_size, this->start_idxs[i_ssm], this->end_idxs[i_ssm]);
      ssm->apply_transition_matrix(X, j, k-this->start_idxs[i_ssm], result, i, n);
    }
    i += state_size;
  }
  return i-x_row_offset;
}

void TransientCombinedSSM::transition_bias(int k, double *result) {

  int asidx = this->active_set_idx(k);
  if (asidx < 0) return;

  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    // skip any null SSMs
    int j = *it;
    StateSpaceModel * ssm = this->ssms[j];
    if (!ssm) continue;

    if (this->start_idxs[j] == k) {
      ssm->prior_mean(result);
    } else {
      ssm->transition_bias(k-this->start_idxs[j], result);
    }
    result += ssm->max_dimension;
  }
}

void TransientCombinedSSM::transition_noise_diag(int k, double *result) {
  int asidx = this->active_set_idx(k);
  if (asidx < 0) return;
  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    // skip any null SSMs
    int j = *it;
    StateSpaceModel * ssm = this->ssms[j];
    if (!ssm) continue;

    if (this->start_idxs[j] == k) {
      ssm->prior_vars(result);
    } else {
      ssm->transition_noise_diag(k-this->start_idxs[j], result);
    }
    result += ssm->max_dimension;
  }
}

double TransientCombinedSSM::apply_observation_matrix(const double *x, int k) {

  double r = 0;

  int asidx = this->active_set_idx(k);
  if (asidx < 0) return 0.0;
  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    int j = *it;
    StateSpaceModel * ssm = this->ssms[j];
    if (!ssm) continue;
    const double * scale = this->scales[j];
    double ri = ssm->apply_observation_matrix(x, k - this->start_idxs[j]);
    if (scale) ri *= scale[k - this->start_idxs[j]];
    r += ri;
    if (ssm) x += ssm->max_dimension;
  }
  return r;
}

void TransientCombinedSSM::apply_observation_matrix(const matrix<double> &X,
						    unsigned int row_offset, int k,
						    double *result, double *result_tmp, unsigned int n) {

  for (unsigned i=0; i < n; ++i) {
    result[i] = 0;
    result_tmp[i] = 0;
  }

  int asidx = this->active_set_idx(k);
  if (asidx < 0) return;

  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {
    int j = *it;
    StateSpaceModel * ssm = this->ssms[j];
    if (!ssm) continue;
    const double * scale = this->scales[j];

    unsigned int state_size = ssm->max_dimension;
    ssm->apply_observation_matrix(X, row_offset,
				  k-this->start_idxs[j],
				  result_tmp, NULL, n);
    // printf("TSSM step %d applying obs matrix on ssm %d state_size %d start_idx %d at row_offset %d n %d scale[%d] %f result[0] %f\n", k, j, state_size, this->start_idxs[j], row_offset, n, k-this->start_idxs[j], scale? scale[k-this->start_idxs[j]] : 1.0, result_tmp[0]);
    if (scale) {
      for (unsigned ii=0; ii < n; ++ii) {
	result[ii] += scale[k-this->start_idxs[j]] * result_tmp[ii];
      }
    } else {
      for (unsigned ii=0; ii < n; ++ii) {
	result[ii] += result_tmp[ii];
      }
    }
    row_offset += state_size;
  }

}

double TransientCombinedSSM::observation_bias(int k) {
  double bias = 0;

  int asidx = this->active_set_idx(k);
  if (asidx < 0) return 0.0;
  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    // skip any null SSMs
    int j = *it;
    StateSpaceModel * ssm = this->ssms[j];
    const double * scale = this->scales[j];
    int kk = k - this->start_idxs[j];

    double b = ssm ? ssm->observation_bias(kk) : 1.0;
    if (scale) b *= scale[kk];
    bias += b;
  }
  return bias;
}

double TransientCombinedSSM::observation_noise(int k) {
  return this->obs_noise;
}

bool TransientCombinedSSM::stationary(int k) {

  matrix_row < matrix<int> > s1 = row(this->active_sets, this->active_set_idx(k));
  if (k > 0) {
    matrix_row < matrix<int> > s2 = row(this->active_sets, this->active_set_idx(k-1));
    for (unsigned i=0; i < s1.size() && !(s1(i) == -1 && s2(i) == -1); ++i) {
      if (s1(i) != s2(i)) return false;
    }
  }

  for (matrix_row < matrix<int> >::const_iterator it = s1.begin();
       it < s1.end() && *it >= 0; ++it) {
    // skip any null SSMs
    int j = *it;
    if (this->scales[j]) return false;

    StateSpaceModel * ssm = this->ssms[j];
    if (ssm && !ssm->stationary(k-this->start_idxs[j])) return false;
  }
  return true;
}

int TransientCombinedSSM::prior_mean(double *result) {

  /*
     TODO: propagate through the forward model whenever
   */

  double * r1 = result;
  int asidx = this->active_set_idx(0);
  if (asidx < 0) return 0;
  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    int j = *it;
    StateSpaceModel * ssm = this->ssms[j];
    if (!ssm) continue;

    int state_size = ssm->max_dimension;
    for(int i=0; i < state_size; ++i) {
      result[i] = 0;
    }
    ssm->prior_mean(result);

    /* if this component starts at a negative time, push the
     * prior through the transition model to get the induced
     * distribution at time zero */
    if (this->start_idxs[j] < 0) {
      double * tmp_result = (double *) malloc(sizeof(double) * ssm->max_dimension);
      if (tmp_result==NULL) {
	printf("memory error, malloc failed!\n");
	exit(-1);
      }
      for (unsigned k=1; k <= -this->start_idxs[j]; ++k) {
	ssm->apply_transition_matrix(result, k, tmp_result);
	ssm->transition_bias(k, tmp_result);
	memcpy(result, tmp_result, ssm->max_dimension);
      }
      free(tmp_result);
    }

    result += state_size;
  }
  return result-r1;
}

int TransientCombinedSSM::prior_vars(double *result) {
  double * r1 = result;
  int asidx = this->active_set_idx(0);
  if (asidx < 0) return 0;
  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    int j = *it;
    StateSpaceModel * ssm = this->ssms[j];
    if (!ssm) continue;
    ssm->prior_vars(result);

    /*printf("ssm %d max dimension %d\n", j, ssm->max_dimension);
    for (double * q = result; q < result+ssm->max_dimension; ++q) {
      printf(" initial var %d = %f\n", q-result, *q);
      }*/

    if (this->start_idxs[j] < 0) {
      matrix<double> P (ssm->max_dimension, ssm->max_dimension);
      matrix<double> P2 (ssm->max_dimension, ssm->max_dimension);
      P.clear();
      for (int i=0; i < ssm->max_dimension; ++i) {
	P(i,i) = result[i];
      }

      for (unsigned k=1; k <= -this->start_idxs[j]; ++k) {
	ssm->apply_transition_matrix(P, 0, k, P2, 0, ssm->max_dimension); // P2 = FP
	noalias(P) = trans(P2); // P = PF'
	ssm->apply_transition_matrix(P, 0, k, P2, 0, ssm->max_dimension); // P2 = FPF'
	P = P2;
	ssm->transition_noise_diag(k, result);
	for (int i=0; i < ssm->max_dimension; ++i) P(i,i) += result[i];
      }
      for (int i=0; i < ssm->max_dimension; ++i) result[i] = P(i,i);
    }

    /*for (double * q = result; q < result+ssm->max_dimension; ++q) {
      printf(" post-prop var %d = %f\n", q-result, *q);
      }*/

    result += ssm->max_dimension;
  }

  /*for (double * q = r1; q < result; ++q) {
    printf("prior var %d = %f\n", q-r1, *q);
    }*/

  return result-r1;
}

void TransientCombinedSSM::init_coef_priors(std::vector<vector<double> > & cmeans,
					    std::vector<vector<double> > & cvars) {
  for (unsigned j=0; j < this->n_ssms; ++j) {
    StateSpaceModel * ssm = this->ssms[j];
    if (ssm && ssm->is_cssm) {
      CompactSupportSSM *cssm = (CompactSupportSSM *) ssm;
      vector<double> mean(cssm->coef_means);
      vector<double> var(cssm->coef_vars);
      cmeans.push_back(mean);
      cvars.push_back(var);
    } else {
      vector<double> mean;
      vector<double> var;
      cmeans.push_back(mean);
      cvars.push_back(var);
    }
  }
}

void TransientCombinedSSM::extract_all_coefs(FilterState &cache, int k,
					     std::vector<vector<double> > & cmeans,
					     std::vector<vector<double> > & cvars) {
  /*
    Assumes cache has a valid, current P matrix.
   */
  int asidx = this->active_set_idx(k);
  if (asidx < 0) return;
  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  unsigned int state_offset = 0;
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    int j = *it;
    StateSpaceModel * ssm = this->ssms[j];
    if (!ssm) continue;
    if (ssm->is_cssm) {
      CompactSupportSSM *cssm = (CompactSupportSSM *) ssm;
      cssm->extract_coefs(cache.xk, cache.P,
			 state_offset, k - this->start_idxs[j],
			 cmeans[j],
			 cvars[j]);
    }
    state_offset += ssm->max_dimension;
  }
}

void TransientCombinedSSM::extract_component_means(double *xk, int k,
						   std::vector<vector<double> > & means) {
  int asidx = this->active_set_idx(k);
  if (asidx < 0) return;
  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  unsigned int state_offset = 0;
  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    int j = *it;
    StateSpaceModel * ssm = this->ssms[j];
    int kk = k - this->start_idxs[j];
    means[j](kk) = ssm ? ssm->apply_observation_matrix(xk + state_offset, kk) : 1.0;
    means[j](kk) += ssm ? ssm->observation_bias(kk) : 0.0;
    if (ssm) state_offset += ssm->max_dimension;
  }
}

void TransientCombinedSSM::extract_component_vars(matrix<double> &P, matrix<double> &P_tmp, int k,
						  std::vector<vector<double> > & vars) {
  int asidx = this->active_set_idx(k);
  if (asidx < 0) return;
  matrix_row < matrix<int> > ssm_indices = row(this->active_sets, asidx);
  unsigned int state_offset = 0;

  double * v_tmp = (double *) malloc(sizeof(double) * P.size1());
  double * v_tmp2 = (double *) malloc(sizeof(double) * P.size1());
  if (!v_tmp || !v_tmp2) {
    printf("memory allocation error in extract_component_vars!");
    exit(-1);
  }

  for (matrix_row < matrix<int> >::const_iterator it = ssm_indices.begin();
       it < ssm_indices.end() && *it >= 0; ++it) {

    int j = *it;
    StateSpaceModel * ssm = this->ssms[j];
    int kk = k - this->start_idxs[j];

    if (ssm) {
      // just be lazy and copy the submatrix for this component into its own matrix.
      subrange(P_tmp, 0, ssm->max_dimension, 0, ssm->max_dimension) = subrange(P, state_offset, state_offset+ssm->max_dimension, state_offset, state_offset+ssm->max_dimension);
      ssm->apply_observation_matrix(P_tmp, 0, kk, v_tmp, v_tmp2, ssm->max_dimension);
      vars[j](kk) = ssm->apply_observation_matrix(v_tmp, kk);
      state_offset += ssm->max_dimension;
      //printf("%d: set var at ssm %d kk %d = %f\n", k, j, kk, vars[j](kk));
    } else {
      vars[j](kk) = 0.0;
    }
  }

  free(v_tmp);
  free(v_tmp2);
}


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/constant/goldbar.hpp>
#include <boost/simd/as.hpp>
#include <stf.hpp>

STF_CASE_TPL( "Check goldbar behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::goldbar;
  using boost::simd::Goldbar;

  STF_TYPE_IS(decltype(Goldbar<T>()), T);
  STF_EQUAL(Goldbar<T>(), T(0));
  STF_EQUAL(goldbar( as(T{}) ),T(0));
}

STF_CASE_TPL( "Check goldbar behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::goldbar;
  using boost::simd::Goldbar;

  STF_TYPE_IS(decltype(Goldbar<T>()), T);
  STF_ULP_EQUAL(Goldbar<T>()+T(1), Goldbar<T>()*Goldbar<T>(), 1);
  STF_LESS(  Goldbar<T>(), T(1));
  auto z = goldbar( as(T{}));
  STF_ULP_EQUAL((z+T(1)),z*z, 1);
}