/* 3dv-client/display.cc
 *
 * Copyright (C) 2013 VisLab
 *
 * This file is part of lib3dv; 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.
 *
 * 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 Lesser General Public License
 * along with this program; if not, see <http://www.gnu.org/licenses/>.
 */

#include "display.h"
#include "interactor_style_3dv.h"

#include <lib3dv/ipm.h>
#include <lib3dv/pose_utils.h>

#include <pcl/visualization/image_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <boost/lexical_cast.hpp>

#include <limits>
#include <stdint.h>
#include <string>
#include <boost/format.hpp>

#ifdef _MSC_VER
#else
#include <X11/Xlib.h>
#endif

void on_keyboard_event(const pcl::visualization::KeyboardEvent& event, void* data)
{
    display* _this = static_cast<display*>(data);

    if(event.getKeySym() == "p" && event.keyDown())
    {
        _this->toggle_points_display();
    }
    else if(event.getKeySym() == "t" && event.keyDown())
    {
        _this->toggle_terrain_display();        
    }
    else if(event.getKeySym() == "m" && event.keyDown())
    {
        _this->toggle_terrain_gm_display();
    }
    else if(event.getKeySym() == "o" && event.keyDown())
    {
        _this->toggle_obstacles_display();
    }
    else if(event.getKeySym() == "v" && event.keyDown())
    {
        _this->toggle_motion_display();
    }
}

void addCubeCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in, const lib3dv::stixel& stixel, uint8_t r, uint8_t g, uint8_t b){
        //m_3d_viewer->addCube(stixel.m_x - stixel.m_dx, stixel.m_x + stixel.m_dx, stixel.m_y - stixel.m_dy, stixel.m_y + stixel.m_dy, stixel.m_z, stixel.m_z + stixel.m_height, r, g, b, boost::lexical_cast<std::string>(o) + "-" + boost::lexical_cast<std::string>(s));

        float step_stix=0.02;
        pcl::PointXYZRGB padd(r,g,b);

        for(padd.x=stixel.m_x;padd.x<stixel.m_dx+stixel.m_x;padd.x+=step_stix){
            //base
            padd.y = stixel.m_y;
            padd.z = stixel.m_z;
            cloud_in->points.push_back(padd);
            padd.y = stixel.m_y+stixel.m_dy;
            cloud_in->points.push_back(padd);
            //up
            padd.z = stixel.m_height+stixel.m_z;
            cloud_in->points.push_back(padd);
            padd.y = stixel.m_y;
            cloud_in->points.push_back(padd);
        }
        for(padd.y=stixel.m_y;padd.y<stixel.m_dy+stixel.m_y;padd.y+=step_stix){
            //base
            padd.x = stixel.m_x;
            padd.z = stixel.m_z;
            cloud_in->points.push_back(padd);
            padd.x = stixel.m_x+stixel.m_dx;
            cloud_in->points.push_back(padd);
            //up
            padd.z = stixel.m_height+stixel.m_z;
            cloud_in->points.push_back(padd);
            padd.x = stixel.m_x;
            cloud_in->points.push_back(padd);
        }
        for(padd.z = stixel.m_z;padd.z<stixel.m_height+stixel.m_z;padd.z+=step_stix){
            //base
            padd.x = stixel.m_x;
            padd.y = stixel.m_y;
            cloud_in->points.push_back(padd);
            padd.x = stixel.m_x+stixel.m_dx;
            cloud_in->points.push_back(padd);
            padd.y = stixel.m_y+stixel.m_dy;
            cloud_in->points.push_back(padd);
            padd.x = stixel.m_x;
            cloud_in->points.push_back(padd);
        }
}


display::display(const lib3dv::calibration::camera_intrinsics& intrinsics, const lib3dv::calibration::position& position, const lib3dv::calibration::orientation& orientation, float baseline, uint8_t downsample, double area_step, uint8_t log_level) :
    m_downsample(downsample),
    m_area_step(area_step),
    m_log_level(log_level),
    m_image_viewers_closed(0),
    m_3d_viewer_closed(false),
    m_show_points(true),
    m_show_terrain(true),
    m_show_terrain_gm(true),
    m_show_obstacles(true),
    m_show_motion(true),
    m_show_classification(true),
    m_stopped(false),
    m_updated(false)
{


    lib3dv::calibration::camera_intrinsics downsampled_intrinsics = intrinsics;
    
    downsampled_intrinsics.m_u0 /= downsample;
    downsampled_intrinsics.m_v0 /= downsample;
    downsampled_intrinsics.m_ku /= downsample;
    downsampled_intrinsics.m_kv /= downsample;

    m_ipm = boost::shared_ptr<lib3dv::inverse_perspective_mapping>(new lib3dv::inverse_perspective_mapping(downsampled_intrinsics, position, orientation, baseline));
    m_pose_utils = boost::shared_ptr<lib3dv::pose_utils>(new lib3dv::pose_utils(position, orientation));

    m_me_orientations.first.m_yaw = m_me_orientations.first.m_pitch = m_me_orientations.first.m_roll = 0;
    m_me_orientations.second.m_yaw = m_me_orientations.second.m_pitch = m_me_orientations.second.m_roll = 0;

    m_td_orientations.first = m_td_orientations.second = boost::posix_time::time_duration(0,0,0);

    #ifdef _MSC_VER
    screen_width=GetSystemMetrics(SM_CXSCREEN);
    screen_height=GetSystemMetrics(SM_CYSCREEN); 
    #else
    Display* disp = XOpenDisplay(NULL);
    Screen*  scrn = DefaultScreenOfDisplay(disp);
    screen_height = scrn->height;
    screen_width  = scrn->width;
    XCloseDisplay(disp);
    #endif
}

void display::update(boost::shared_ptr<const lib3dv::image > image)
{
    boost::mutex::scoped_lock l(m_update_mutex);
    m_image_buffers[image->m_type - 1].second = image;
    m_updated = true;
    m_update_condition.notify_all();
}

void display::update(boost::shared_ptr<const lib3dv::terrain> terrain)
{
    boost::mutex::scoped_lock l(m_update_mutex);
    m_terrain_buffers.second = terrain;
    m_updated = true;
    m_update_condition.notify_all();
}

void display::update(boost::shared_ptr<const std::vector<lib3dv::obstacle> > obstacles)
{
    boost::mutex::scoped_lock l(m_update_mutex);
    m_obstacles_buffers.second = obstacles;
    m_updated = true;
    m_update_condition.notify_all();
}

void display::update(boost::shared_ptr<const lib3dv::motion> motion)
{
    boost::mutex::scoped_lock l(m_update_mutex);
    m_motion_buffers.second = motion;
    m_updated = true;
    m_update_condition.notify_all();
}

void display::update(boost::shared_ptr<const lib3dv::classification> classification)
{
    boost::mutex::scoped_lock l(m_update_mutex);
    m_classification_buffers.second = classification;
    m_updated = true;
    m_update_condition.notify_all();
}

float r_coefficeints[] = {1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 1.0f};
float g_coefficeints[] = {0.0f, 0.5f, 1.0f, 1.0f, 0.5f, 0.0f, 0.5f, 0.0f, 0.0f};
float b_coefficeints[] = {0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.5f};

std::string image_names[] = {"Left rectified", "Right rectified", "Left raw", "Right raw", "DSI"};

void display::run()
{
    do
    {
        boost::this_thread::interruption_point();

        {
            boost::mutex::scoped_lock l(m_update_mutex);

            while(!m_updated && !m_stopped)
                m_update_condition.wait(l);

            if(m_stopped)
                break;
            m_updated = false;
            
            for(unsigned int i = 0; i < lib3dv::image::type::NUM - 1; ++i)            
                if(m_image_buffers[i].second)
                {
                    std::swap(m_image_buffers[i].first, m_image_buffers[i].second);
                    m_image_buffers[i].second.reset();
                }
            
            if(m_terrain_buffers.second)
            {
                std::swap(m_terrain_buffers.first, m_terrain_buffers.second);
                m_terrain_buffers.second.reset();
            }
            
            if(m_obstacles_buffers.second)
            {
                std::swap(m_obstacles_buffers.first, m_obstacles_buffers.second);
                m_obstacles_buffers.second.reset();
            }

            if(m_motion_buffers.second)
            {
                std::swap(m_motion_buffers.first, m_motion_buffers.second);
                m_motion_buffers.second.reset();
            }

            if(m_classification_buffers.second)
            {   
                std::swap(m_classification_buffers.first, m_classification_buffers.second);
                m_classification_buffers.second.reset();
            }

        }
        
        for(unsigned int i = 0; i < lib3dv::image::type::NUM - 1; ++i)
            if(m_image_buffers[i].first && !m_image_viewers_closed[i])
            {
                if(m_image_buffers[i].first->m_type == lib3dv::image::type::RIGHT_RECTIFIED)
                    m_right_rectified = m_image_buffers[i].first;
                
                if(m_image_buffers[i].first->m_type == lib3dv::image::type::DSI)
                    m_dsi = m_image_buffers[i].first;

                if(!m_image_viewers[i])
                    m_image_viewers[i] = boost::shared_ptr<pcl::visualization::ImageViewer>(new pcl::visualization::ImageViewer());

                m_image_viewers[i]->setWindowTitle(image_names[i] + " - " + boost::posix_time::to_simple_string(m_image_buffers[i].first->m_timestamp));
        
                switch(m_image_buffers[i].first->m_format)
                {
                    case (lib3dv::image::format::MONO8) :
                    case (lib3dv::image::format::BAYER8_BGGR) :
                    case (lib3dv::image::format::BAYER8_GRBG) :
                    case (lib3dv::image::format::BAYER8_RGGB) :
                    case (lib3dv::image::format::BAYER8_GBRG) :
                    {
                        m_image_viewers[i]->showMonoImage(m_image_buffers[i].first->m_buffer.data(), m_image_buffers[i].first->m_width, m_image_buffers[i].first->m_height);
                        if(m_classification_buffers.first && m_show_classification && m_image_buffers[i].first->m_type == lib3dv::image::type::RIGHT_RECTIFIED)
                        {
                            m_image_viewers[i]->removeLayer("rectangles");
                            m_image_viewers[i]->removeLayer("car");
                            m_image_viewers[i]->removeLayer("pedestrian");
                            int offset=m_image_buffers[i].first->m_height+27-screen_height;
                            if (offset<0)
                                offset=0;
                            for(int s=0; s < m_classification_buffers.first->m_candidates.size(); s++)
                            {
                                const lib3dv::candidate& cand = m_classification_buffers.first->m_candidates[s];
                                if (cand.m_category==lib3dv::category_type::PEDESTRIAN)
                                {
                                    m_image_viewers[i]->addRectangle(cand.m_x0,cand.m_x1,m_image_buffers[i].first->m_height-cand.m_y0,m_image_buffers[i].first->m_height-cand.m_y1,0.0,0.0,1.0,"rectangles",1.0);                                
                                    m_image_viewers[i]->addText(cand.m_x0+2, cand.m_y1-offset,"Conf="+(boost::format("%0.2f") %cand.m_confidence).str(),  0.0, 0.0, 1.0, "pedestrian",1.0);
                                }
                                else if (cand.m_category==lib3dv::category_type::CAR)
                                {
                                    m_image_viewers[i]->addRectangle(cand.m_x0,cand.m_x1,m_image_buffers[i].first->m_height-cand.m_y0,m_image_buffers[i].first->m_height-cand.m_y1,1.0,0.0,0.0,"rectangles",1.0);                                
                                    m_image_viewers[i]->addText(cand.m_x0+2, cand.m_y1-offset,"Conf="+(boost::format("%0.2f") %cand.m_confidence).str(), 1.0, 0.0, 0.0, "car",1.0);
                                }
                            } 

                        }                        
                        break;
                    }
                    case (lib3dv::image::format::MONO16) :
                    case (lib3dv::image::format::BAYER16_BGGR) :
                    case (lib3dv::image::format::BAYER16_GRBG) :
                    case (lib3dv::image::format::BAYER16_RGGB) :
                    case (lib3dv::image::format::BAYER16_GBRG) :
                    {
                        if(m_image_buffers[i].first->m_type == lib3dv::image::type::DSI)
                            m_image_viewers[i]->showShortImage(reinterpret_cast<const uint16_t*>(m_image_buffers[i].first->m_buffer.data()), m_image_buffers[i].first->m_width, m_image_buffers[i].first->m_height, 0, 0x7F00);
                        else
                            m_image_viewers[i]->showShortImage(reinterpret_cast<const uint16_t*>(m_image_buffers[i].first->m_buffer.data()), m_image_buffers[i].first->m_width, m_image_buffers[i].first->m_height, 0, (1 << m_image_buffers[i].first->m_bpp) - 1, true);
                        break;
                    }

                    case (lib3dv::image::format::RGB24) :
                    {
                        m_image_viewers[i]->showRGBImage(m_image_buffers[i].first->m_buffer.data(), m_image_buffers[i].first->m_width, m_image_buffers[i].first->m_height);
                        if(m_classification_buffers.first && m_show_classification && m_image_buffers[i].first->m_type == lib3dv::image::type::RIGHT_RECTIFIED)
                        {
                            m_image_viewers[i]->removeLayer("rectangles");
                            m_image_viewers[i]->removeLayer("car");
                            m_image_viewers[i]->removeLayer("pedestrian");
                            int offset=m_image_buffers[i].first->m_height+27-screen_height;
                            if (offset<0)
                                offset=0;
                            for(int s=0; s < m_classification_buffers.first->m_candidates.size(); s++)
                            {
                                const lib3dv::candidate& cand = m_classification_buffers.first->m_candidates[s];
                                if (cand.m_category==lib3dv::category_type::PEDESTRIAN)
                                {
                                    m_image_viewers[i]->addRectangle(cand.m_x0,cand.m_x1,m_image_buffers[i].first->m_height-cand.m_y0,m_image_buffers[i].first->m_height-cand.m_y1,0.0,0.0,1.0,"rectangles",1.0);                                
                                    m_image_viewers[i]->addText(cand.m_x0+2, cand.m_y1-offset,"Conf="+(boost::format("%0.2f") %cand.m_confidence).str(), 0.0, 0.0, 1.0, "pedestrian",1.0);
                                }
                                else if (cand.m_category==lib3dv::category_type::CAR)
                                {
                                    m_image_viewers[i]->addRectangle(cand.m_x0,cand.m_x1,m_image_buffers[i].first->m_height-cand.m_y0,m_image_buffers[i].first->m_height-cand.m_y1,1.0,0.0,0.0,"rectangles",1.0);                                
                                    m_image_viewers[i]->addText(cand.m_x0+2, cand.m_y1-offset,"Conf="+(boost::format("%0.2f") %cand.m_confidence).str(), 1.0, 0.0, 0.0, "car",1.0);
                                }
                            } 

                        }
                        break;
                    }
                }
                
                m_image_viewers[i]->spinOnce();
                
                if(m_image_viewers[i]->wasStopped())
                {
                    m_image_viewers[i]->close();
                    m_image_viewers[i].reset();
                    m_image_viewers_closed[i] = true;
                }
                
                boost::mutex::scoped_lock l(m_update_mutex);
                m_image_buffers[i].first.reset();
            }
            
        if((m_terrain_buffers.first || m_obstacles_buffers.first || m_dsi) && !m_3d_viewer_closed)
        {
            if(!m_3d_viewer)
            {
                int argc = 0;
                
                m_3d_viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer(argc, (char**)NULL, std::string("Point Cloud"), interactor_style_3dv::New()));

                //FIXME: get camera params from device calibration
                m_3d_viewer->setBackgroundColor (0, 0, 0);
                m_3d_viewer->addCoordinateSystem (1.0);
                m_3d_viewer->initCameraParameters();
                m_3d_viewer->setCameraPosition(-3.0, 0.0, 6.0, 1.03093, -0.140037, 1.86098, 0.715357, 0.012124, 0.698653);
                m_3d_viewer->setCameraFieldOfView(0.8575);
                m_3d_viewer->setCameraClipDistances(-100, 300);
                m_3d_viewer->setShowFPS(false);
                m_3d_viewer->registerKeyboardCallback(on_keyboard_event, (void*)this);
            }
            
            if(m_dsi && m_show_points)
            {

                std::vector<lib3dv::point3> world_points;
                world_points.reserve(m_dsi->m_width * m_dsi->m_height);

                (*m_ipm)(m_dsi, world_points);
                
                if(m_right_rectified)
                {
                    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
                    cloud->reserve(world_points.size());
                    
                    if(m_right_rectified->m_format == lib3dv::image::format::MONO8)
                    {
                        const uint8_t* buffer = m_right_rectified->m_buffer.data();

                        for(unsigned int i = 0; i < m_dsi->m_height; ++i)
                            for(unsigned int j = 0; j < m_dsi->m_width; ++j)
                            {
                                const uint32_t dsi_px = i * m_dsi->m_width + j;
                                const uint32_t right_px = m_downsample * (i * (m_dsi->m_width * m_downsample) + j);
                                
                                const lib3dv::point3& wp = world_points[dsi_px];
                                
                                if(wp.m_x < std::numeric_limits<float>::max() && wp.m_y < std::numeric_limits<float>::max() && wp.m_z < std::numeric_limits<float>::max())
                                {
                                    uint8_t val = buffer[right_px];
                                    pcl::PointXYZRGB p(val, val, val);

                                    p.x = wp.m_x;
                                    p.y = wp.m_y;
                                    p.z = wp.m_z;

                                    cloud->points.push_back(p);
                                }
                            }
                    }
                    
                    if(m_right_rectified->m_format == lib3dv::image::format::RGB24)
                    {
                        const uint8_t* buffer = m_right_rectified->m_buffer.data();

                        for(unsigned int i = 0; i < m_dsi->m_height; ++i)
                            for(unsigned int j = 0; j < m_dsi->m_width; ++j)
                            {
                                const uint32_t dsi_px = i * m_dsi->m_width + j;
                                const uint32_t right_px = m_downsample * (i * (m_dsi->m_width * 3 * m_downsample) + j * 3);
                                
                                const lib3dv::point3& wp = world_points[dsi_px];
                                
                                if(wp.m_x < std::numeric_limits<float>::max() && wp.m_y < std::numeric_limits<float>::max() && wp.m_z < std::numeric_limits<float>::max())
                                {
                                    uint8_t r = buffer[right_px];
                                    uint8_t g = buffer[right_px + 1];
                                    uint8_t b = buffer[right_px + 2];
                                    
                                    pcl::PointXYZRGB p(r, g, b);

                                    p.x = wp.m_x;
                                    p.y = wp.m_y;
                                    p.z = wp.m_z;

                                    cloud->points.push_back(p);
                                }
                            }
                    }
                        
                    if(!m_3d_viewer->updatePointCloud(cloud, "cloud"))
                    {
                        m_3d_viewer->addPointCloud(cloud, "cloud");
                        m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
                    }

                    m_right_rectified.reset();
                }
                else
                {
//                    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//                    cloud->reserve(world_points.size());
//
//                    for(unsigned int w = 0; w < world_points.size(); ++w)
//                    {
//                        const lib3dv::point3& wp = world_points[w];
//
//                        if(wp.m_x < std::numeric_limits<float>::max() && wp.m_y < std::numeric_limits<float>::max() && wp.m_z < std::numeric_limits<float>::max())
//                        {
//                            pcl::PointXYZ p;
//
//                            p.x = wp.m_x;
//                            p.y = wp.m_y;
//                            p.z = wp.m_z;
//
//                            cloud->points.push_back(p);
//                        }
//                    }
//
//                    if(!m_3d_viewer->updatePointCloud(cloud, "cloud"))
//                    {
//                        m_3d_viewer->addPointCloud(cloud, "cloud");
//                        m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
//                    }
                }
                
                m_dsi.reset();
            }
            else if(!m_show_points)
                m_3d_viewer->removePointCloud("cloud");
            
            //m_3d_viewer->removeAllShapes();

            std::vector< std::vector<lib3dv::point3> > tcolumn_points;

            if(m_terrain_buffers.first && m_show_terrain)
            {

                unsigned int dim_x;
                unsigned int dim_y;
                std::vector<unsigned long> matrix_terrain_index;

                if(m_show_terrain_gm && m_terrain_buffers.first->m_data.size()>4)
                {
                    //std::cout << "terrain_size: "<< m_terrain_buffers.first->m_data.size() << std::endl;
                    float x_min=std::numeric_limits<float>::max(),x_max=-std::numeric_limits<float>::max(),y_min=std::numeric_limits<float>::max(),y_max=-std::numeric_limits<float>::max();
                    for(unsigned int t = 0; t < m_terrain_buffers.first->m_data.size(); ++t){
                        //std::cout<<"(x,y)"<<m_terrain_buffers.first->m_data[t].m_x<<" "<<m_terrain_buffers.first->m_data[t].m_y<<std::endl;
                        if(m_terrain_buffers.first->m_data[t].m_x>x_max)
                            x_max = m_terrain_buffers.first->m_data[t].m_x;
                        if(m_terrain_buffers.first->m_data[t].m_x<x_min)
                            x_min = m_terrain_buffers.first->m_data[t].m_x;
                        if(m_terrain_buffers.first->m_data[t].m_y>y_max)
                            y_max = m_terrain_buffers.first->m_data[t].m_y;
                        if(m_terrain_buffers.first->m_data[t].m_y<y_min)
                            y_min = m_terrain_buffers.first->m_data[t].m_y;
                    }
                    dim_x=(x_max-x_min)/m_area_step + 1;
                    dim_y=(y_max-y_min)/m_area_step + 1;
                    //std::cout << "x_min:"<<x_min<< " | x_max:"<<x_max << " | y_min:"<<y_min<<" | y_max:"<<y_max<<std::endl;
                    //std::cout << "dim_x:"<<dim_x<< " dim_y:"<<dim_y<<std::endl;
                    matrix_terrain_index.resize(dim_x*dim_y,std::numeric_limits<unsigned long>::max());
                    
                    for(unsigned int t = 0; t < m_terrain_buffers.first->m_data.size(); ++t)
                    {
                        lib3dv::point3 ptt = m_terrain_buffers.first->m_data[t];
                        unsigned int index=((unsigned int)((ptt.m_x-x_min)/m_area_step))*dim_y + (unsigned int)((ptt.m_y-y_min)/m_area_step);
                        if(index < dim_x*dim_y)
                            matrix_terrain_index[index] = t;
                    }
                    /*
                    for(int w=0;w<dim_x;w++){
                        for(int q=0;q<dim_y;q++){
                            unsigned int index=q+w*dim_y;
                            if(matrix_terrain[index].m_z>-100){
                                std::cout<<"("<<matrix_terrain[index].m_x<<","<<matrix_terrain[index].m_y<<")\t";
                                //std::cout <<"o\t";
                            }
                            else
                                std::cout <<"x\t\t";
                        }
                        std::cout << std::endl;
                    }*/
                }
                pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

                cloud->reserve(m_terrain_buffers.first->m_data.size());

                for(unsigned int t = 0; t < m_terrain_buffers.first->m_data.size(); ++t)
                {
                    
                    pcl::PointXYZRGB p(0,255,0);
                    
                    p.x = m_terrain_buffers.first->m_data[t].m_x;
                    p.y = m_terrain_buffers.first->m_data[t].m_y;
                    p.z = m_terrain_buffers.first->m_data[t].m_z;
                    
                    cloud->points.push_back(p);
                }


                
                if (m_show_terrain_gm && m_terrain_buffers.first->m_data.size()>4)
                {                    
                    for (int c = 0; c < dim_x; c++)
                    {
                        for (int cc = 0; cc < dim_y; cc++)
                        {
                            pcl::PointXYZRGB p1,p2;
                            const std::vector<lib3dv::point3>& terr_data= m_terrain_buffers.first->m_data;
                            if(matrix_terrain_index[cc+c*dim_y]!=std::numeric_limits<unsigned long>::max())
                            {
                                //valid point
                                unsigned int p1_index = matrix_terrain_index[cc+c*dim_y];
                                p1.x = terr_data[p1_index].m_x;
                                p1.y = terr_data[p1_index].m_y;
                                p1.z = terr_data[p1_index].m_z;
                                
                                if(cc<dim_y-1)
                                    if(matrix_terrain_index[cc+1+c*dim_y]!=std::numeric_limits<unsigned long>::max() )
                                    {
                                        int p2_index=matrix_terrain_index[cc+1+c*dim_y];
                                        p2.x = terr_data[p2_index].m_x;
                                        p2.y = terr_data[p2_index].m_y;
                                        p2.z = terr_data[p2_index].m_z;
                                        std::string tid = "TRLH-"+ boost::lexical_cast<std::string>(c)+"-"+ boost::lexical_cast<std::string>(cc);
                                        pcl::PointXYZRGB pmed(0,255,0);
                                        float step_terr = 0.02;
                                        pmed.x=p1.x;
                                        pmed.z=p1.z;
                                        float dz = step_terr*(p2.z-p1.z)/(p2.y-p1.y);
                                        
                                        for(pmed.y=p1.y+step_terr, pmed.z=p1.z+dz;pmed.y<p2.y;pmed.y+=step_terr,pmed.z+=dz){
                                            cloud->points.push_back(pmed);
                                        }
                                    }
                                
                                if(c<dim_x-1)
                                    if(matrix_terrain_index[cc+(c+1)*dim_y]!=std::numeric_limits<unsigned long>::max() )
                                    {
                                        int p2_index=matrix_terrain_index[cc+(c+1)*dim_y];
                                        std::string tid = "TRLV-"+ boost::lexical_cast<std::string>(c)+"-"+ boost::lexical_cast<std::string>(cc);
                                        p2.x = terr_data[p2_index].m_x;
                                        p2.y = terr_data[p2_index].m_y;
                                        p2.z = terr_data[p2_index].m_z;
                                        pcl::PointXYZRGB pmed(0,255,0);
                                        float step_terr = 0.02;
                                        pmed.y=p1.y;
                                        pmed.z=p1.z;
                                        float dz = step_terr*(p2.z-p1.z)/(p2.x-p1.x);
                                        for(pmed.x=p1.x+step_terr, pmed.z=p1.z+dz;pmed.x<p2.x;pmed.x+=step_terr,pmed.z+=dz){
                                            cloud->points.push_back(pmed);
                                        }
                                   }

                            }
                        }
                    }


                }
                if(!m_3d_viewer->updatePointCloud(cloud, "terrain"))
                {
                    m_3d_viewer->addPointCloud(cloud, "terrain");
                    m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "terrain");
                }

            } else if(!m_show_terrain)
            {
                m_3d_viewer->removePointCloud("terrain");
            }



            if(m_obstacles_buffers.first && m_show_obstacles)
            {
                pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
                for(unsigned int o = 0; o < m_obstacles_buffers.first->size(); ++o)
                {
                    const lib3dv::obstacle& obstacle = (*m_obstacles_buffers.first)[o];

                    uint8_t c = obstacle.m_guid % 9;
                    float r = r_coefficeints[c];
                    float g = g_coefficeints[c];
                    float b = b_coefficeints[c];

                    for(unsigned int s = 0; s < obstacle.m_data.size(); ++s)
                    {
                        const lib3dv::stixel& stixel = obstacle.m_data[s];
                        //m_3d_viewer->addCube(stixel.m_x - stixel.m_dx, stixel.m_x + stixel.m_dx, stixel.m_y - stixel.m_dy, stixel.m_y + stixel.m_dy, stixel.m_z, stixel.m_z + stixel.m_height, r, g, b, boost::lexical_cast<std::string>(o) + "-" + boost::lexical_cast<std::string>(s));
                        addCubeCloud(cloud,stixel,r*255,g*255,b*255);
                    }
                }
                if(!m_3d_viewer->updatePointCloud(cloud, "obstacle"))
                {
                    m_3d_viewer->addPointCloud(cloud, "obstacle");
                    m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "obstacle");
                }

            } else if(!m_show_obstacles){
                m_3d_viewer->removePointCloud("obstacle");
            }


            if(m_motion_buffers.first && m_show_motion)
            {   
                if(m_motion_buffers.first->m_poses.size()==0)
                {
                    m_me_orientations.first.m_yaw = m_me_orientations.first.m_pitch = m_me_orientations.first.m_roll = 0;
                    m_td_orientations.first = boost::posix_time::time_duration(0,0,0);
                    m_3d_viewer->removeShape("orientation");
                    m_3d_viewer->removeShape("orientation_rate");
                    m_3d_viewer->removeShape("orientation_acc");
                 }
                 else
                 {
                     std::swap(m_td_orientations.first, m_td_orientations.second);
                     std::swap(m_me_orientations.first, m_me_orientations.second);
                     
                     boost::posix_time::time_duration& prev_pose_td = m_td_orientations.first;
                     boost::posix_time::time_duration& pose_td = m_td_orientations.second;
                     
                     float & yaw = m_me_orientations.second.m_yaw;
                     float & pitch = m_me_orientations.second.m_pitch;
                     float & roll = m_me_orientations.second.m_roll;
                     
                     float & prev_yaw = m_me_orientations.first.m_yaw;
                     float & prev_pitch = m_me_orientations.first.m_pitch;
                     float & prev_roll = m_me_orientations.first.m_roll;
                     
                     
                     for (std::vector<lib3dv::pose>::const_iterator p = m_motion_buffers.first->m_poses.begin(); p != m_motion_buffers.first->m_poses.end(); p++)
                         if ( p->m_type == lib3dv::pose::type::RELATIVE_POSE )
                         {
                             m_pose_utils->extract_orientation(*p,yaw,pitch,roll);
                             pose_td = p->m_timestamp - boost::posix_time::ptime(boost::gregorian::date(1970,1,1));
                         }
                     
                     
                     float dt = std::fabs((double)pose_td.total_microseconds() - (double)prev_pose_td.total_microseconds())/1e6;
                     
                     std::string orientation_acc = "yaw_acc: " + boost::lexical_cast<std::string>((yaw - prev_yaw)/pow(dt,2.0f))
                                                               + ", pitch_acc: " + boost::lexical_cast<std::string>((pitch - prev_pitch)/pow(dt,2.0f)) +
                                                               + ", roll_acc: " +  boost::lexical_cast<std::string>((roll - prev_roll)/pow(dt,2.0f)) + " [rad/s^2]";
                     
                     std::string orientation_rate = "yaw rate: " + boost::lexical_cast<std::string>((yaw - prev_yaw)/dt)
                                                                 + ", pitch rate: " + boost::lexical_cast<std::string>((pitch - prev_pitch)/dt) +
                                                                 + ", roll rate: " +  boost::lexical_cast<std::string>((roll - prev_roll)/dt) + " [rad/s]";
                     
                     std::string orientation = "yaw: " + boost::lexical_cast<std::string>(yaw)
                                                       + ", pitch: " + boost::lexical_cast<std::string>(pitch) +
                                                       + ", roll: " +  boost::lexical_cast<std::string>(roll) + " [rad]";
                     m_3d_viewer->removeShape("orientation");
                     m_3d_viewer->removeShape("orientation_rate");
                     m_3d_viewer->removeShape("orientation_acc");
                     if (!m_3d_viewer->updateText(orientation, 5, 25, 1.0, 1.0, 1.0, "orientation"))
                         m_3d_viewer->addText(orientation, 5, 25, 1.0, 1.0, 1.0, "orientation");
                     
                     if (!m_3d_viewer->updateText(orientation_rate, 5, 15, 1.0, 1.0, 1.0, "orientation_rate"))
                         m_3d_viewer->addText(orientation_rate, 5, 15, 1.0, 1.0, 1.0, "orientation_rate");
                     
                     if (!m_3d_viewer->updateText(orientation_acc, 5, 5, 1.0, 1.0, 1.0, "orientation_acc"))
                         m_3d_viewer->addText(orientation_acc, 5, 5, 1.0, 1.0, 1.0, "orientation_acc");
                 }
            } 
            else if (!m_show_motion)
            {
                m_3d_viewer->removeShape("angles");
                m_3d_viewer->removeShape("orientation");
                m_3d_viewer->removeShape("orientation_rate");
                m_3d_viewer->removeShape("orientation_acc");
            }
            m_3d_viewer->spinOnce();
            
            if(m_3d_viewer->wasStopped())
            {
                m_3d_viewer->close();
                m_3d_viewer.reset();
                m_3d_viewer_closed = true;
            }

            boost::mutex::scoped_lock l(m_update_mutex);
            m_terrain_buffers.first.reset();
            m_obstacles_buffers.first.reset();
            m_motion_buffers.first.reset();
        }        
    } while(true);
    
    for(unsigned int i = 0; i < lib3dv::image::type::NUM - 1; ++i)
        if(m_image_viewers[i])
        {
            m_image_viewers[i]->close();
            m_image_viewers[i].reset();
        }
        
    if(m_3d_viewer)
    {
        m_3d_viewer->close();
        m_3d_viewer.reset();
    }
}


void display::stop()
{
    boost::mutex::scoped_lock l(m_update_mutex);
    m_stopped = true;
    m_update_condition.notify_all();
}