/* 3dv-client/disk_writer.cc
 *
 * Copyright (C) 2013 VisLab
 *
 * This file is part of 3dv-client; 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 "disk_writer.h"

disk_writer::disk_writer(const std::vector<boost::filesystem::path>& paths, data_file_format::types data_file_format, bool autonumber, device_params params, uint8_t guid_type ,uint8_t log_level) :
    m_autonumber(autonumber),
    m_guid_type(guid_type),
    m_log_level(log_level)
{
    for(unsigned int p = 0; p < paths.size(); ++p)
    {
        std::cout << "[II] 3dv-client: creating writing queue for path " << paths[p] << std::endl;
        m_disk_queues.push_back(boost::shared_ptr<disk_queue>(new disk_queue(paths[p], data_file_format, log_level)));
        if(!boost::filesystem::exists(paths[p]))
         {
           std::cout << "[II] 3dv-client: writing to path " << paths[p] << std::endl;
           boost::filesystem::create_directories(paths[p]);
         }
         
      std::string filename = "params.ini";
      boost::filesystem::path path_filename = paths[p] / filename;
   
      std::cout << "[II] 3dv-client: writing INI file " << path_filename << std::endl;
      std::ofstream file (path_filename.string().c_str(), std::ios::out);
      file <<"calibration.u0="<<params.intrinsics.m_u0<<std::endl;
      file <<"calibration.v0="<<params.intrinsics.m_v0<<std::endl;
      file <<"calibration.ku="<<params.intrinsics.m_ku<<std::endl;
      file <<"calibration.kv="<<params.intrinsics.m_kv<<std::endl;
      file <<"calibration.x="<<params.position.m_x<<std::endl;
      file <<"calibration.y="<<params.position.m_y<<std::endl; 
      file <<"calibration.z="<<params.position.m_z<<std::endl;
      file <<"calibration.yaw="<<params.orientation.m_yaw<<std::endl;
      file <<"calibration.pitch="<<params.orientation.m_pitch<<std::endl; 
      file <<"calibration.roll="<<params.orientation.m_roll<<std::endl;
      file <<"calibration.baseline="<<params.baseline<<std::endl;
      file <<"depth mapping.downsample ratio="<<params.downsample<<std::endl;
      file <<"advanced.detection.area.step="<<params.area_step<<std::endl;
      file.close();
    }
}

#ifdef ARCHIVE
disk_writer::disk_writer(const std::vector<boost::filesystem::path>& paths, const std::string& archive_name, archive_file_format::types archive_format, data_file_format::types data_file_format, bool autonumber, device_params params, uint8_t guid_type, uint8_t log_level) :
    m_autonumber(autonumber),
    m_guid_type(guid_type),
    m_log_level(log_level)
{
    for(unsigned int p = 0; p < paths.size(); ++p)
    {
        std::cout << "[II] 3dv-client: creating writing queue for path " << paths[p] << std::endl;
        m_disk_queues.push_back(boost::shared_ptr<disk_queue>(new disk_queue(paths[p], archive_name, archive_format, data_file_format,params,log_level)));
    }
}
#endif

void disk_writer::run()
{
    for(unsigned int q = 0; q < m_disk_queues.size(); ++q)
        m_disk_queue_threads.create_thread(boost::bind(&boost::asio::io_service::run, &m_disk_queues[q]->m_io_service));
    
    m_disk_queue_threads.join_all();
}

void disk_writer::stop()
{
    for(unsigned int q = 0; q < m_disk_queues.size(); ++q)
    {
        delete m_disk_queues[q]->m_io_service_work;
        m_disk_queues[q]->m_io_service_work = NULL;
    }
}

void disk_writer::image_callback(boost::shared_ptr<const lib3dv::image> image, uint32_t guid)
{
    int queue_id = (guid / m_guid_type) % m_disk_queues.size();
  
#ifdef ARCHIVE
    if(m_disk_queues[queue_id]->m_archive)
        m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_image_archive, image, guid, m_guid_type, m_disk_queues[queue_id]->m_archive, m_autonumber, m_log_level));
    else
#endif
        m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_image_file, image, guid, m_guid_type, m_disk_queues[queue_id]->m_path, m_autonumber, m_log_level));
}

void disk_writer::terrain_callback(boost::shared_ptr<const lib3dv::terrain> terrain, uint32_t guid)
{
    int queue_id = (guid / m_guid_type) % m_disk_queues.size();
    
#ifdef ARCHIVE
    if(m_disk_queues[queue_id]->m_archive)
        m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_terrain_archive, terrain, guid, m_guid_type, m_disk_queues[queue_id]->m_archive, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
    else
#endif
        m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_terrain_file, terrain, guid, m_guid_type,m_disk_queues[queue_id]->m_path, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
}

void disk_writer::obstacles_callback(boost::shared_ptr<const std::vector<lib3dv::obstacle> > obstacles, uint32_t guid)
{
    int queue_id = (guid / m_guid_type) % m_disk_queues.size();
    
#ifdef ARCHIVE
    if(m_disk_queues[queue_id]->m_archive)
        m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_obstacles_archive, obstacles, guid, m_guid_type, m_disk_queues[queue_id]->m_archive, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
    else
#endif
        m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_obstacles_file, obstacles, guid, m_guid_type, m_disk_queues[queue_id]->m_path, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
}

void disk_writer::motion_callback(boost::shared_ptr<const lib3dv::motion> motion, uint32_t guid)
{
    int queue_id = (guid / m_guid_type) % m_disk_queues.size();

#ifdef ARCHIVE
    if(m_disk_queues[queue_id]->m_archive)
        m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_motion_archive, motion, guid, m_guid_type, m_disk_queues[queue_id]->m_archive, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
    else
#endif
        m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_motion_file, motion, guid, m_guid_type, m_disk_queues[queue_id]->m_path, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
}

void disk_writer::classification_callback(boost::shared_ptr<const lib3dv::classification> classification, uint32_t guid)
{
    int queue_id = (guid / m_guid_type) % m_disk_queues.size();

#ifdef ARCHIVE
    if(m_disk_queues[queue_id]->m_archive)
        m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_classification_archive, classification, guid, m_guid_type, m_disk_queues[queue_id]->m_archive, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
    else
#endif
        m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_classification_file, classification, guid, m_guid_type, m_disk_queues[queue_id]->m_path, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
}

disk_writer::disk_queue::disk_queue(const boost::filesystem::path& path, data_file_format::types data_file_format, uint8_t log_level) :
    m_io_service_work(new boost::asio::io_service::work(m_io_service)),
    m_path(path),
    m_data_file_format(data_file_format),
    m_log_level(log_level)
#ifdef ARCHIVE
    , m_archive(NULL)
#endif
{}

#ifdef ARCHIVE
disk_writer::disk_queue::disk_queue(const boost::filesystem::path& path, const std::string& archive_name, archive_file_format::types archive_format, data_file_format::types data_file_format,device_params params, uint8_t log_level) : 
    m_io_service_work(new boost::asio::io_service::work(m_io_service)),
    m_path(path),
    m_data_file_format(data_file_format),
    m_log_level(log_level)
{
    m_archive = open_archive(path, archive_name, archive_format,params, log_level);
}
#endif

disk_writer::disk_queue::~disk_queue()
{
    delete m_io_service_work;
#ifdef ARCHIVE
    if(m_archive)
        close_archive(m_archive, m_log_level);
#endif
}