#ifndef ENVIRONMENT_H
#define ENVIRONMENT_H

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "car/ussDataMsg.h"
#include "car/camDataMsg.h"

using namespace boost::numeric::ublas;
namespace car
{
class Environment : public nodelet::Nodelet
{
public:
    virtual void onInit();

    Environment(ros::NodeHandle & nh, std::string & name);

    Environment();

    ~Environment();

private:
    ros::NodeHandle nh_;
    std::string name_;
    void readConfigFile();

    float distance;
    float relativeSpeed;

    ros::Publisher environmentData;
    ros::Subscriber ussData;
    ros::Subscriber camData;

    void ussDataCallback(const ussDataMsg::ConstPtr & inMsg);
    void camDataCallback(const camDataMsg::ConstPtr & inMsg);

    //Notation like in Kalamanfilter <Wikipedia>
    vector<double> x {2};          // current state (distance, velocity)^T
    matrix<double> P {2, 2};       // covariance matrix
    matrix<double> F {2, 2};       // dynamic model
    matrix<double> Q {2, 2};       // process error
    vector<double> H {2};          // mesure vector (refers to distance)
    vector<double> R {1};          // measurement error
    
    void predict();
    void update(vector<double> mesVec);
};
}
#endif