/* 3dv-client/playback.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 "playback.h"

#ifdef _MSC_VER
#include <conio.h>
#else
#include "3dv_conio.h"  
#endif


uint8_t playback(std::vector<std::string> playback_value,uint8_t log_level)
{
    if(playback_value.size() > 0)
    {
        std::stringstream ss;
        uint32_t framenumber;
        uint32_t begin_framenumber;
        uint32_t first=999999;
        uint32_t last=0;
        uint32_t myframe=0;
        std::string path = playback_value[0]+"/";
        boost::filesystem::path path_filename =path;
    
    
        if(boost::filesystem::exists(path_filename)==1)
        {
            if(boost::filesystem::is_directory(path_filename)) 
            {
                typedef std::vector<boost::filesystem::path> vec;             
                vec v;                               
                copy(boost::filesystem::directory_iterator(path_filename), boost::filesystem::directory_iterator(), back_inserter(v));
                sort(v.begin(), v.end());            
                for(vec::const_iterator it (v.begin()); it != v.end(); ++it)
                {
                    std::vector<std::string> tokens;
                    boost::filesystem::path linep=*it;
                    std::string line=linep.stem().string();
                    boost::split(tokens, line, boost::is_any_of("-"));
                    if(tokens[0]=="dsi")
                    {
                        std::string stringnumber=tokens[1];
                        uint32_t number=atoi(stringnumber.c_str());
                        if(number<first)
                            first=number;
                        if(number>last)
                            last=number;
                    }
                }
            }
            else
            {
            std::cerr<<"[EE] 3dv-client:"<< path_filename <<" exists, but is neither a regular file nor a directory\n";
            return lib3dv::error::NONE;
            }
            
            framenumber=first;
            if(playback_value.size() > 1)
            {
                ss << playback_value[1];
                ss >> begin_framenumber;
                framenumber= begin_framenumber;
            }
    
            std::cout << "[II] 3dv-client: playback " << path <<" from frame " <<framenumber<<std::endl;
    
            if(begin_framenumber>last)
                begin_framenumber=last;
            else if(begin_framenumber<first) 
                begin_framenumber=first;
      
    
            boost::thread display_thread;
            boost::shared_ptr<display> display;
            device_params params;
    
            std::string filenameini = path+"params.ini";
            std::cout << "[II] 3dv-client: loading INI file: " << filenameini << std::endl;
            std::ifstream file (filenameini.c_str(), std::ios::in);
            std::string line;
    
            if (file.is_open())
            {
                while( getline (file,line))
                {
                    std::vector<std::string> tokens;
                    boost::split(tokens, line, boost::is_any_of("="));
    
                    if(tokens[0] == "calibration.u0")
                        params.intrinsics.m_u0 = boost::lexical_cast<float>(tokens[1].c_str());
                    else if(tokens[0] == "calibration.v0")
                         params.intrinsics.m_v0 = boost::lexical_cast<float>(tokens[1].c_str());
                    else if(tokens[0] == "calibration.ku")
                         params.intrinsics.m_ku = boost::lexical_cast<float>(tokens[1].c_str());
                    else if(tokens[0] == "calibration.kv")
                         params.intrinsics.m_kv = boost::lexical_cast<float>(tokens[1].c_str());
                    else if(tokens[0] == "calibration.x")
                        params.position.m_x = boost::lexical_cast<float>(tokens[1].c_str());
                    else if(tokens[0] == "calibration.y")
                        params.position.m_y = boost::lexical_cast<float>(tokens[1].c_str());
                    else if(tokens[0] == "calibration.z")
                        params.position.m_z = boost::lexical_cast<float>(tokens[1].c_str());
                    else if(tokens[0] == "calibration.yaw")
                        params.orientation.m_yaw = boost::lexical_cast<float>(tokens[1].c_str());
                    else if(tokens[0] == "calibration.pitch")
                        params.orientation.m_pitch = boost::lexical_cast<float>(tokens[1].c_str());
                    else if(tokens[0] == "calibration.roll")
                        params.orientation.m_roll = boost::lexical_cast<float>(tokens[1].c_str());
                    else if(tokens[0] == "calibration.baseline")
                        params.baseline = boost::lexical_cast<float>(tokens[1].c_str());
                    else if(tokens[0] == "depth mapping.downsample ratio")
                        params.downsample = boost::lexical_cast<uint32_t>(tokens[1].c_str());
                    else if(tokens[0] =="advanced.detection.area.step")
                        params.area_step = boost::lexical_cast<double>(tokens[1].c_str());
                }
                file.close();
            }
            else 
                std::cout << "[WW] 3dv-client::Unable to open INI file\n"; 
    
            display = boost::shared_ptr< ::display>(new ::display(params.intrinsics, params.position, params.orientation, params.baseline, params.downsample, params.area_step,log_level));
            display_thread = boost::thread(boost::bind(&::display::run, display.get()));
    
            std::string filename;
    
            boost::posix_time::ptime  last_timestamp;
            boost::posix_time::ptime  now_timestamp;
            boost::posix_time::ptime  old_timestamp;
    
            
            #ifdef _MSC_VER
            #else
            set_conio_terminal_mode();
            #endif
    
            char p='h';
            int play=1;
            float speed=1;
            int skip=1;
            int change=1;
            bool loop=false;
            bool enable_classification=true;
            std::cout.precision(2);
    
            while(1)
            {
                while(p!='g')
                {
                    while (!kbhit())
                    {
                        if (p=='h')
                        {
                               std::cout <<"\n\r[II] 3dv-client: playback menu:\n\n\r"
                               "         h              : display the help\n\r"
                               "         q              : quit playback\n\r"
                               "\n\r"
                               "       up-arrow, s      : start playback\n\r"
                               "     down-arrow, r      : start reverse playback\n\r"
                               "    right-arrow         : go to the next frame\n\r"
                               "     left-arrow         : go to the previous frame\n\r"
                               "         p              : pause playback\n\r"
                               "         l              : enable/disable loop playback\n\r"
                               "         c              : enable/disable classification output\n\r"
                               "\n\r"
                               "         home           : go to the first frame\n\r"
                               "         end            : go to the last frame\n\r"
                               "         g [nframe]     : go to frame nframe\n\r"
                               "\n\r"
                               "        +/-             : increment/decrement playback speed \n\r"
                               " [1|2|3|4|5|6|7|8|9|0]  : set speed  [1x|2x|3x|4x|5x|0.1x|0.25x|0.5x|0.75x|max] \n\r"
                               "    [F1|F2|F3|F4]       : set skip frame to [1|2|5|10] (NOTE: you may want to increase the speed as well)\n\n\r";
                               "\n\r";
                        }
                        else if (p=='s')
                            {play=1;}
                        else if (p=='p')
                            {play=0;}
                        else if (p=='r')
                            {play=-1;}
                        else if (p=='l')
                            {loop=!loop;}
                        else if (p=='c')
                            {enable_classification=!enable_classification;}
                        else if (p=='0')
                            {speed=0;}
                        else if (p=='1')
                            {speed=1;}
                        else if (p=='2')
                            {speed=2;}
                        else if (p=='3')
                            {speed=3;}
                        else if (p=='4')
                            {speed=4;}
                        else if (p=='5')
                            {speed=5;}
                        else if (p=='6')
                            {speed=0.10;}
                        else if (p=='7')
                            {speed=0.25;}
                        else if (p=='8')
                            {speed=0.50;}
                        else if (p=='9')
                            {speed=0.75;}
                        else if (p=='+')
                        {
                           if (speed<=20) speed*=2;
                           else if(speed==0) speed=1;
                        } 
                        else if (p=='-')
                        {
                            if (speed>=0.1) speed/=2;
                            else if(speed==0) speed=1;
                        }
                        #ifdef _MSC_VER
                        else if (p=='H')
                            {play=1;} 
                        else if (p=='P')
                            {play=-1;}
                        else if (p=='M')
                            {framenumber+=skip;play=2;}
                        else if (p=='K')
                            {framenumber-=skip;play=-2;}
                        else if (p=='G')
                            {framenumber=first;play=0;} 
                        else if (p=='O')
                            {framenumber=last;play=0;}                               
                        else if (p==';')
                            {skip=1;}    
                        else if (p=='<')
                            {skip=2;}  
                        else if (p=='=')
                            {skip=5;}   
                        else if (p=='>')
                            {skip=10;}                            
                        #else
                        else if (p=='A')
                            {play=1;} 
                        else if (p=='B')
                            {play=-1;}
                        else if (p=='C')
                            {framenumber+=skip;play=2;}
                        else if (p=='D')
                            {framenumber-=skip;play=-2;}
                        else if (p=='H')
                            {framenumber=first;play=0;} 
                        else if (p=='F')
                            {framenumber=last;play=0;}                               
                        else if (p=='P')
                            {skip=1;}    
                        else if (p=='Q')
                            {skip=2;}  
                        else if (p=='R')
                            {skip=5;}   
                        else if (p=='S')
                            {skip=10;}
                        #endif
                        else if (p=='q')
                        {
                            if(display)
                            {
                                display->stop();
                                display_thread.join();
                            }
                            std::cout << "\n\r[II] 3dv-client: operation completed\n\r";        
                            return lib3dv::error::NONE;
                        }
                        else
                        {
                            change=0;
                        }
                               
                        if(loop)
                        {
                            if(framenumber>last)
                            {
                                framenumber=first;
                                std::cout<<"\r\n";
                                change=1;
                            }
                            else if(framenumber<first)
                            {
                                framenumber=last;
                                std::cout<<"\r\n";
                                change=1;
                            }
                        }
                        else 
                        {
                            if(framenumber>last)
                                framenumber=last;
                            else if(framenumber<first)
                                framenumber=first;
                        }
    
                        if(change)
                        {
                            if(speed==0.0)
                            {
                                std::cout<<"\r\t\t\t\t\t\tskip="<<skip<<" \r";
                                std::cout<<"\r\t\t\t\tspeed=max    \r";
                            }
                            else 
                            {
                                std::cout<<"\r\t\t\t\t\t\tskip="<<skip<<" \r";
                                std::cout<<"\r\t\t\t\tspeed="<<speed<<"x    \r";
                            }
                        }
    
                        if(play==1 )
                            std::cout<<"\r > ";
                        else if(play==-1 )
                            std::cout<<"\r < ";
                        else if(play==-2 )
                            std::cout<<"\r<--";
                        else if(play==2 )
                             std::cout<<"\r-->";
                        else if(play==0 )
                             std::cout<<"\r = ";
    
                        if(framenumber==last)
                            std::cout<<"E";
                        else if(framenumber==first)
                            std::cout<<"B";
                        else
                            std::cout<<" ";
    
                        std::string str_nframe=(boost::format("%06i") %framenumber).str(); 
                        if(myframe!=framenumber || change) 
                            std::cout<<"\t"<<str_nframe<<":";
                        filename=path+"dsi-"+str_nframe+".pgm";
                        if (boost::filesystem::exists(filename)==1)
                        {
                            boost::shared_ptr<lib3dv::image> dsi_image = boost::shared_ptr<lib3dv::image>(new lib3dv::image()); 
                            pnm_load<uint16_t>(dsi_image,filename.c_str(),lib3dv::image::type::DSI); 
                            boost::posix_time::time_duration steptime;
                            if(play==1)
                                steptime= dsi_image->m_timestamp-last_timestamp;
                            else if(play==-1)
                                steptime=last_timestamp- dsi_image->m_timestamp;
                            
                            last_timestamp= dsi_image->m_timestamp;
                            now_timestamp=boost::posix_time::microsec_clock::local_time();
                            boost::posix_time::time_duration codetime=now_timestamp-old_timestamp;
                            if((play==1 || play==-1) && speed!=0 && framenumber!=first && framenumber!=begin_framenumber) 
                            {
                                int32_t sleep=steptime.total_milliseconds()/speed-codetime.total_milliseconds();
                                if (sleep>0)
                                {
                                    boost::this_thread::sleep(boost::posix_time::milliseconds(sleep));
                                }
                            }
                            old_timestamp=boost::posix_time::microsec_clock::local_time();
    
                            if (myframe!=framenumber)
                                display->update(dsi_image);
                            if(myframe!=framenumber || change) 
                                 std::cout<<"d";
                        }
                        
                        for(int i=0;i<6;i++)
                        {
                            std::string image_type;
                            if (i<2)
                                image_type="left_rectified-";
                            else if (i<4) 
                                image_type="right_rectified-";
                            if (i%2==0)
                                filename=path+image_type+str_nframe+".ppm";
                            else
                                filename=path+image_type+str_nframe+".pgm";
                            if (i==4)
                                filename=path+"left_raw-"+str_nframe+".pgm";
                            if (i==5)
                                filename=path+"right_raw-"+str_nframe+".pgm";
                            if (boost::filesystem::exists(filename)==1)
                            {
                                boost::shared_ptr<lib3dv::image> output_image = boost::shared_ptr<lib3dv::image>(new lib3dv::image());
                                pnm_load<uint8_t>(output_image,filename.c_str(),(lib3dv::image::type::types)(i/2+1+(i/5)));
                                if(myframe!=framenumber || change)
                                    std::cout<<"i";
                                if (myframe!=framenumber)
                                    display->update(output_image);       
                            }
                        }
    
                        filename=path+"terrain-"+str_nframe+".txt";
                        boost::shared_ptr<lib3dv::terrain> output_terrain= boost::shared_ptr<lib3dv::terrain>(new  lib3dv::terrain());   
                        if (boost::filesystem::exists(filename)==1)
                        {                             	         
                            std::ifstream file(filename.c_str(), std::ios::in);
                            if(file)
                            {
                                boost::archive::text_iarchive archive(file);
                                archive >> *output_terrain;
                            }
                            
                            display->update(output_terrain);
                            if (myframe!=framenumber || change)
                                std::cout<<"t";
                        }
                        else
                            display->update(output_terrain);
    
                        filename=path+"obstacles-"+str_nframe+".txt";
                        boost::shared_ptr< std::vector<lib3dv::obstacle> > output_obstacles=boost::shared_ptr< std::vector<lib3dv::obstacle> >(new std::vector<lib3dv::obstacle>);
                        if (boost::filesystem::exists(filename)==1)
                        {   
                            boost::shared_ptr< std::vector<lib3dv::obstacle> > output_obstacles=boost::shared_ptr< std::vector<lib3dv::obstacle> >(new std::vector<lib3dv::obstacle>);
                            std::ifstream fileo(filename.c_str(), std::ios::in);
                            if(fileo)
                            {
                                boost::archive::text_iarchive archive(fileo);
                                archive >> *output_obstacles;
                            }
                            display->update( output_obstacles);
                            if (myframe!=framenumber || change)
                                std::cout<<"o";
                        }
                        else 
                            display->update( output_obstacles);
                        
                        filename=path+"motion-"+str_nframe+".txt";
                        boost::shared_ptr<lib3dv::motion> output_motion=boost::shared_ptr<lib3dv::motion>(new lib3dv::motion);
                        if (boost::filesystem::exists(filename)==1 )
                        {   
                            boost::shared_ptr<lib3dv::motion> output_motion=boost::shared_ptr<lib3dv::motion>(new lib3dv::motion);
                            std::ifstream filem(filename.c_str(), std::ios::in);
                            if(filem)
                            {
                                boost::archive::text_iarchive archive(filem);
                                archive >> *output_motion;
                            }
                            if (myframe!=framenumber)
                                display->update(output_motion);
                            if (myframe!=framenumber || change)
                                std::cout<<"m";
                        }
                        else
                        {
                            display->update(output_motion);
                        }
                        
                        filename=path+"classification-"+str_nframe+".txt";
                        boost::shared_ptr<lib3dv::classification> output_classificat=boost::shared_ptr<lib3dv::classification>(new lib3dv::classification);
                        if (boost::filesystem::exists(filename)==1 )
                        {   

                            std::ifstream filec(filename.c_str(), std::ios::in);
                            if(filec && enable_classification)
                            {
                                boost::archive::text_iarchive archive(filec);
                                archive >> *output_classificat;
                            }
                            if (myframe!=framenumber)
                                display->update(output_classificat);
                            if (myframe!=framenumber || change)
                                std::cout<<"c";
                        }
                        else
                            display->update(output_classificat);     

                        if (myframe!=framenumber || change)
                        {   
                            std::cout<<"         \r";
                        }
                        std::cout.flush();
                        change=1;
                        myframe=framenumber;
                        if(play==1)
                            framenumber+=skip;
                        if(play==-1)
                            framenumber-=skip;
                        p='a';
                    }
                    p=getch();
                }
                std::cout<<"\n\rInsert the framenumber and press enter:";
                 
                #ifdef _MSC_VER
                std::cin>>framenumber;
                #else
                reset_terminal_mode();
                std::cin>>framenumber;
                set_conio_terminal_mode();
                #endif
         
                play=0;
                std::cout<<"GO->\t"<<(boost::format("%06i") %framenumber).str();
                change=1;
                p='a';
            }//end while(1)
            
        }
        else
        {
            std::cerr << "[EE] 3dv-client: the path :"<< path<<" doesn't exist"<< std::endl;
            return lib3dv::error::NONE;
        }
        
    }
    else
    {
        std::cerr << "[EE] 3dv-client: at least 1 argument must be provided to --playback" << std::endl;
        return lib3dv::error::NONE;
    }         
}