#include <gazebo-2.0/gazebo/math/Pose.hh>
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/math/Matrix4.hh>
#include <stdio.h>
#include <cmath>
#include <stdlib.h>
#include <time.h>
#include <iostream>
#include <ostream>
#include "gazebo/transport/transport.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/msgs/vector3d.pb.h"
#include <gazebo/rendering/rendering.hh>
#include <gazebo/gui/gui.hh>
using namespace std;
using namespace gazebo;

class SystemGUI : public SystemPlugin
{
public:~SystemGUI()
{
        cout<<"in destructor"<<endl;
}

public: void Load(int /*_argc*/, char ** /*_argv*/)
{
        this->connections.push_back(event::Events::ConnectPreRender(boost::bind(&SystemGUI::Update, this)));
        cout<<"Load()"<<endl;
}
private: void Update()
{

        if (!this->userCam)
        {
                // Get a pointer to the active user camera
                this->userCam = gui::get_active_camera();
                cout<<userCam->GetName()<<endl;
                userCam->SetAspectRatio (3);
        }


        userCam->EnableViewController(TRUE);
        userCam->SetFocalPoint(math::Vector3(0,0,0.1));
        userCam->SetViewController("fps",math::Vector3(0,1,0.5));

        scene = rendering::get_scene();
        if (!scene || !scene->GetInitialized())
        return;
}

/// Pointer the user camera.
private: rendering::UserCameraPtr userCam;
rendering::ScenePtr scene;
/// All the event connections.
private: std::vector<event::ConnectionPtr> connections;

};

GZ_REGISTER_SYSTEM_PLUGIN(SystemGUI);