/*
 * Software License Agreement (BSD License)
 *
 * Point Cloud Library (PCL) - www.pointclouds.org
 * Copyright (c) 2010, Willow Garage, Inc.
 * Copyright (c) 2012-, Open Perception, Inc.
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * * Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided
 * with the distribution.
 * * Neither the name of the copyright holder(s) nor the names of its
 * contributors may be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <thread>

#include <pcl/common/time.h> // for DO_EVERY
#include <pcl/visualization/common/common.h>
#include <vtkRenderWindowInteractor.h>
#include <pcl/visualization/histogram_visualizer.h>

#include <vtkVersion.h>
#include <vtkDoubleArray.h>
#include <vtkTextProperty.h>
#include <vtkRenderer.h>
#include <vtkDataObject.h>
#include <vtkProperty2D.h>
#include <vtkFieldData.h>

using namespace std::chrono_literals;

//////////////////////////////////////////////////////////////////////////////////////////////
pcl::visualization::PCLHistogramVisualizer::PCLHistogramVisualizer () : 
 exit_main_loop_timer_callback_ (vtkSmartPointer<ExitMainLoopTimerCallback>::New ()), 
 exit_callback_ (vtkSmartPointer<ExitCallback>::New ()), 
 stopped_ ()
{
}

//////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Spin once method. Calls the interactor and updates the screen once. */
void
pcl::visualization::PCLHistogramVisualizer::spinOnce (int time)
{
 for (auto &win : wins_)
 {
 DO_EVERY(1.0/win.second.interactor_->GetDesiredUpdateRate (),
 win.second.interactor_->Render ();
 exit_main_loop_timer_callback_->right_timer_id = win.second.interactor_->CreateRepeatingTimer (time);

 // Set the correct interactor for callbacks
 exit_main_loop_timer_callback_->interact = win.second.interactor_;
 win.second.interactor_->Start ();
 win.second.interactor_->DestroyTimer (exit_main_loop_timer_callback_->right_timer_id);
 );
 }
}

//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::visualization::PCLHistogramVisualizer::spin ()
{
 stopped_ = false;
 do
 {
 spinOnce ();
 if (stopped_)
 break;
 std::this_thread::sleep_for(1s);
 }
 while (true);
}

//////////////////////////////////////////////////////////////////////////////////////////////
void 
pcl::visualization::PCLHistogramVisualizer::setBackgroundColor (const double &r, const double &g, const double &b)
{
 /*
 rens_->InitTraversal ();
 vtkRenderer* renderer = NULL;
 int i = 1;
 while ((renderer = rens_->GetNextItem ()) != NULL)
 {
 // Should we add the actor to all renderers?
 if (viewport == 0)
 {
 renderer->SetBackground (r, g, b);
 renderer->Render ();
 }
 else if (viewport == i) // add the actor only to the specified viewport
 {
 renderer->SetBackground (r, g, b);
 renderer->Render ();
 }
 ++i;
 }
 */
 for (auto &win : wins_)
 {
 win.second.ren_->SetBackground (r, g, b);
 win.second.ren_->Render ();
 }
}

//////////////////////////////////////////////////////////////////////////////////////////////
void 
pcl::visualization::PCLHistogramVisualizer::setGlobalYRange (float minp, float maxp)
{
 for (auto &win : wins_)
 {
 win.second.xy_plot_->SetYRange (minp, maxp);
 win.second.xy_plot_->Modified ();
 }
}

//////////////////////////////////////////////////////////////////////////////////////////////
void 
pcl::visualization::PCLHistogramVisualizer::updateWindowPositions ()
{
 int posx = 0, posy = 0;
 for (auto &win : wins_)
 {
 // Get the screen size
 int *scr_size = win.second.win_->GetScreenSize ();
 int *win_size = win.second.win_->GetActualSize ();

 // Update the position of the current window
 win.second.win_->SetPosition (posx, posy);
 win.second.win_->Modified ();
 // If there is space on Y, go on Y first
 if ((posy + win_size[1]) <= scr_size[1]) 
 posy += win_size[1];
 // Go on X
 else
 {
 posy = 0;
 if ((posx + win_size[0]) <= scr_size[0]) 
 posx += win_size[0];
 else
 posx = 0;
 }
 }
}

//////////////////////////////////////////////////////////////////////////////////////////////
void 
pcl::visualization::PCLHistogramVisualizer::reCreateActor (
 const vtkSmartPointer<vtkDoubleArray> &xy_array, RenWinInteract* renwinupd, const int hsize)
{
 renwinupd->ren_->RemoveActor2D (renwinupd->xy_plot_);
 renwinupd->xy_plot_->RemoveAllDataSetInputConnections ();
 
 double min_max[2];
 xy_array->GetRange (min_max, 1);

 // Create the data structures
 vtkSmartPointer<vtkFieldData> field_values = vtkSmartPointer<vtkFieldData>::New ();
 field_values->AddArray (xy_array);

 vtkSmartPointer<vtkDataObject> field_data = vtkSmartPointer<vtkDataObject>::New ();
 field_data->SetFieldData (field_values);

 renwinupd->xy_plot_->AddDataObjectInput (field_data);
 renwinupd->ren_->AddActor2D (renwinupd->xy_plot_);
 
 renwinupd->xy_plot_->SetYTitle (""); renwinupd->xy_plot_->SetXTitle ("");
 renwinupd->xy_plot_->SetYRange (min_max[0], min_max[1]); 
 renwinupd->xy_plot_->SetXRange (0, hsize - 1);
}
 

////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::visualization::PCLHistogramVisualizer::createActor (
 const vtkSmartPointer<vtkDoubleArray> &xy_array, 
 pcl::visualization::RenWinInteract &renwinint,
 const std::string &id, const int win_width, const int win_height)
{
 // Create the actor
 renwinint.xy_plot_->SetDataObjectPlotModeToColumns ();
 renwinint.xy_plot_->SetXValuesToValue ();

 // Create the data structures
 vtkSmartPointer<vtkFieldData> field_values = vtkSmartPointer<vtkFieldData>::New ();
 field_values->AddArray (xy_array);

 vtkSmartPointer<vtkDataObject> field_data = vtkSmartPointer<vtkDataObject>::New ();
 field_data->SetFieldData (field_values);

 renwinint.xy_plot_->AddDataObjectInput (field_data);
 // Set the plot color
 renwinint.xy_plot_->SetPlotColor (0, 1.0, 0.0, 0.0);

 renwinint.xy_plot_->SetDataObjectXComponent (0, 0); renwinint.xy_plot_->SetDataObjectYComponent (0, 1);
 renwinint.xy_plot_->PlotPointsOn ();
 //renwinint.xy_plot_->PlotCurvePointsOn ();
 //renwinint.xy_plot_->PlotLinesOn ();
 renwinint.xy_plot_->PlotCurveLinesOn ();

 // Get min-max range
 double min_max[2];
 xy_array->GetRange (min_max, 1);

 renwinint.xy_plot_->SetYTitle (""); renwinint.xy_plot_->SetXTitle ("");
 renwinint.xy_plot_->SetYRange (min_max[0], min_max[1]); 
 renwinint.xy_plot_->SetXRange (0, static_cast<double> (xy_array->GetNumberOfTuples () - 1));

 //renwinint.xy_plot_->SetTitle (id.c_str ());
 renwinint.xy_plot_->GetProperty ()->SetColor (0, 0, 0);

 // Adjust text properties
 vtkSmartPointer<vtkTextProperty> tprop = renwinint.xy_plot_->GetTitleTextProperty ();
 renwinint.xy_plot_->AdjustTitlePositionOn ();
 tprop->SetFontSize (8);
 tprop->ShadowOff (); tprop->ItalicOff ();
 tprop->SetColor (renwinint.xy_plot_->GetProperty ()->GetColor ());

 renwinint.xy_plot_->SetAxisLabelTextProperty (tprop);
 renwinint.xy_plot_->SetAxisTitleTextProperty (tprop);
 renwinint.xy_plot_->SetNumberOfXLabels (8);
 renwinint.xy_plot_->GetProperty ()->SetPointSize (3);
 renwinint.xy_plot_->GetProperty ()->SetLineWidth (2);

 renwinint.xy_plot_->SetPosition (0, 0);
 renwinint.xy_plot_->SetWidth (1); renwinint.xy_plot_->SetHeight (1);

 // Create the new window with its interactor and renderer
 renwinint.ren_->AddActor2D (renwinint.xy_plot_);
 renwinint.ren_->SetBackground (1, 1, 1);
 renwinint.win_->SetWindowName (id.c_str ());
 renwinint.win_->AddRenderer (renwinint.ren_);
 renwinint.win_->SetSize (win_width, win_height);
 renwinint.win_->SetBorders (1);
 
 // Create the interactor style
 vtkSmartPointer<pcl::visualization::PCLHistogramVisualizerInteractorStyle> style_ = vtkSmartPointer<pcl::visualization::PCLHistogramVisualizerInteractorStyle>::New ();
 style_->Initialize ();
 renwinint.style_ = style_;
 renwinint.style_->UseTimersOn ();
 renwinint.interactor_ = vtkSmartPointer<vtkRenderWindowInteractor>::New ();
 renwinint.interactor_->SetRenderWindow (renwinint.win_);
 renwinint.interactor_->SetInteractorStyle (renwinint.style_);
 // Initialize and create timer
 renwinint.interactor_->Initialize ();
 renwinint.interactor_->CreateRepeatingTimer (5000L);

 exit_main_loop_timer_callback_->right_timer_id = -1;
 renwinint.interactor_->AddObserver (vtkCommand::TimerEvent, exit_main_loop_timer_callback_);
 exit_callback_->his = this;
 renwinint.interactor_->AddObserver (vtkCommand::ExitEvent, exit_callback_);
}

////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
 const pcl::PCLPointCloud2 &cloud, const std::string &field_name,
 const std::string &id, int win_width, int win_height)
{
 // Get the field
 int field_idx = pcl::getFieldIndex (cloud, field_name);
 if (field_idx == -1)
 {
 PCL_ERROR ("[addFeatureHistogram] Invalid field (%s) given!\n", field_name.c_str ());
 return (false);
 }

 auto am_it = wins_.find (id);
 if (am_it != wins_.end ())
 {
 PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
 return (false);
 }

 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
 xy_array->SetNumberOfComponents (2);
 xy_array->SetNumberOfTuples (cloud.fields[field_idx].count);

 // Parse the cloud data and store it in the array
 double xy[2];
 for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
 {
 xy[0] = d;
 float data;
 // TODO: replace float with the real data type
 memcpy (&data, &cloud.data[cloud.fields[field_idx].offset + d * sizeof (float)], sizeof (float));
 xy[1] = data;
 xy_array->SetTuple (d, xy);
 }
 RenWinInteract renwinint;
 createActor (xy_array, renwinint, id, win_width, win_height);

 // Save the pointer/ID pair to the global window map
 wins_[id] = renwinint;
 return (true);
}

////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
 const pcl::PCLPointCloud2 &cloud,
 const std::string &field_name, 
 const pcl::index_t index,
 const std::string &id, int win_width, int win_height)
{
 if (index < 0 || index >= static_cast<pcl::index_t> (cloud.width * cloud.height))
 {
 PCL_ERROR ("[addFeatureHistogram] Invalid point index (%d) given!\n", index);
 return (false);
 }

 // Get the field
 int field_idx = pcl::getFieldIndex (cloud, field_name);
 if (field_idx == -1)
 {
 PCL_ERROR ("[addFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ());
 return (false);
 }

 auto am_it = wins_.find (id);
 if (am_it != wins_.end ())
 {
 PCL_ERROR ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
 return (false);
 }

 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
 xy_array->SetNumberOfComponents (2);
 xy_array->SetNumberOfTuples (cloud.fields[field_idx].count);

 // Compute the total size of the fields
 unsigned int fsize = 0;
 for (const auto &field : cloud.fields)
 fsize += field.count * pcl::getFieldSize (field.datatype);

 // Parse the cloud data and store it in the array
 double xy[2];
 for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
 {
 xy[0] = d;
 float data;
 // TODO: replace float with the real data type
 memcpy (&data, &cloud.data[index * fsize + cloud.fields[field_idx].offset + d * sizeof (float)], sizeof (float));
 xy[1] = data;
 xy_array->SetTuple (d, xy);
 }
 RenWinInteract renwinint;
 createActor (xy_array, renwinint, id, win_width, win_height);

 // Save the pointer/ID pair to the global window map
 wins_[id] = renwinint;
 return (true);
}

//////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram (
 const pcl::PCLPointCloud2 &cloud, const std::string &field_name,
 const std::string &id)
{
 auto am_it = wins_.find (id);
 if (am_it == wins_.end ())
 {
 PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ());
 return (false);
 }
 RenWinInteract* renwinupd = &wins_[id];
 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
 xy_array->SetNumberOfComponents (2);

 // Get the field
 int field_idx = pcl::getFieldIndex (cloud, field_name);
 if (field_idx == -1)
 {
 pcl::console::print_error ("[updateFeatureHistogram] Invalid field (%s) given!", field_name.c_str ());
 return (false);
 }
 xy_array->SetNumberOfTuples (cloud.fields[field_idx].count);

 // Parse the cloud data and store it in the array
 double xy[2];
 for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
 {
 xy[0] = d;
 float data;
 memcpy (&data, &cloud.data[cloud.fields[field_idx].offset + d * sizeof (float)], sizeof (float));
 xy[1] = data;
 xy_array->SetTuple (d, xy);
 }
 reCreateActor(xy_array, renwinupd, cloud.fields[field_idx].count - 1);
 return (true);
}

//////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram (
 const pcl::PCLPointCloud2 &cloud,
 const std::string &field_name, 
 const pcl::index_t index,
 const std::string &id)
{
 if (index < 0 || index >= static_cast<pcl::index_t> (cloud.width * cloud.height))
 {
 PCL_ERROR ("[updateFeatureHistogram] Invalid point index (%d) given!\n", index);
 return (false);
 }
 auto am_it = wins_.find (id);
 if (am_it == wins_.end ())
 {
 PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ());
 return (false);
 }
 RenWinInteract* renwinupd = &wins_[id];
 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
 xy_array->SetNumberOfComponents (2);

 // Get the field
 int field_idx = pcl::getFieldIndex (cloud, field_name);
 if (field_idx == -1)
 {
 pcl::console::print_error ("[updateFeatureHistogram] Invalid field (%s) given!", field_name.c_str ());
 return (false);
 }
 xy_array->SetNumberOfTuples (cloud.fields[field_idx].count);
 
 // Compute the total size of the fields
 unsigned int fsize = 0;
 for (const auto &field : cloud.fields)
 fsize += field.count * pcl::getFieldSize (field.datatype);

 // Parse the cloud data and store it in the array
 double xy[2];
 for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
 {
 xy[0] = d;
 float data;
 memcpy (&data, &cloud.data[index * fsize + cloud.fields[field_idx].offset + d * sizeof (float)], sizeof (float));
 xy[1] = data;
 xy_array->SetTuple (d, xy);
 }
 reCreateActor(xy_array, renwinupd, cloud.fields[field_idx].count - 1);
 return (true);
}

//////////////////////////////////////////////////////////////////////////////
void
pcl::visualization::PCLHistogramVisualizer::ExitMainLoopTimerCallback::Execute (
 vtkObject*, unsigned long event_id, void* call_data)
{
 if (event_id != vtkCommand::TimerEvent)
 return;
 int timer_id = *(reinterpret_cast<int*> (call_data));

 if (timer_id != right_timer_id)
 return;

 // Stop vtk loop and send notification to app to wake it up
 interact->TerminateApp ();
}

//////////////////////////////////////////////////////////////////////////////
void
pcl::visualization::PCLHistogramVisualizer::ExitCallback::Execute (
 vtkObject*, unsigned long event_id, void*)
{
 if (event_id != vtkCommand::ExitEvent)
 return;
 his->stopped_ = true;
}