// Includes, project.
#include "VislabCamera.hpp"

// Includes, qt.imageformat
#include <QDebug>

// Includes, standard.
#include <cassert>
#include <stdexcept>

// Includes lib3dv
#include <lib3dv-1.2.0/lib3dv/device.h>
#include "DiskWriter.h"
#include "Display.h"
#include "VislabImageProcessor.h"

#include <boost/asio/io_service.hpp>
#include <boost/asio/ip/address.hpp>
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/thread.hpp>

using namespace pacpus;

//ComponentFactory<VislabCamera> sFactory("VislabComponent");
/*! Constructor with 2 instantiations of DiskWriter and 2 instatiations of Display*/
    VislabCamera::VislabCamera(const QString& name)
     :diskwriterVideo(name,lib3dv::image::type::RIGHT_RECTIFIED),
     diskwriterDisparite(name,lib3dv::image::type::DSI),
     displayVideo("Video",lib3dv::image::type::RIGHT_RECTIFIED),
     displayDisparite("Disparite",lib3dv::image::type::DSI),
	 outputVideo(lib3dv::image::type::RIGHT_RECTIFIED),
	 outputDisparite(lib3dv::image::type::DSI)
{

}

VislabCamera::~VislabCamera()
{

}

void VislabCamera::open()
{
	
    curr_devices = lib3dv::device::enumerate(boost::asio::ip::address_v4::from_string("192.168.0.1"), 1, error);
    if(error != lib3dv::error::NONE)
        std::cout << "[EE] 3dv-client: error detected: " << error << std::endl;

    if(curr_devices.empty())
    {
        std::cout << "[EE] 3dv-client: no devices found" << std::endl;
        //return lib3dv::error::NONE;
    }
      //disk_writer = boost::shared_ptr< ::disk_writer>(new ::disk_writer(paths, data_format, autonumber, params, guid_type, log_level));
	curr_devices[0].log_level(2);
	curr_devices[0].timeout(boost::posix_time::milliseconds(5000));
    curr_devices[0].connect_image_callback(boost::function<void(boost::shared_ptr< const lib3dv::image>, unsigned int)>(boost::bind(&::VislabImageProcessor::image_callback, &displayVideo, _1, _2)));
    curr_devices[0].connect_image_callback(boost::function<void(boost::shared_ptr< const lib3dv::image>, unsigned int)>(boost::bind(&::VislabImageProcessor::image_callback, &displayDisparite, _1, _2)));
    curr_devices[0].connect_image_callback(boost::function<void(boost::shared_ptr< const lib3dv::image>, unsigned int)>(boost::bind(&::VislabImageProcessor::image_callback, &diskwriterVideo, _1, _2)));
    curr_devices[0].connect_image_callback(boost::function<void(boost::shared_ptr< const lib3dv::image>, unsigned int)>(boost::bind(&::VislabImageProcessor::image_callback, &diskwriterDisparite, _1, _2)));
    curr_devices[0].start_transmission(error);
    if(error != lib3dv::error::NONE)
      std::cout << "[EE] 3dv-client: error detected: " << error << std::endl;
	else 
		std::cout << "[II] 3dv-client: device successfully opened" << std::endl;
}

void VislabCamera::close()
{
  curr_devices[0].stop_transmission(error);
  //curr_devices[0].poweroff(error);
}

void VislabCamera::setVideoOutput(OutputInterface<cv::Mat, VislabComponent>* output)
{
	outputVideo.setOutput(output);
}

void VislabCamera::setDispariteOutput(OutputInterface<cv::Mat, VislabComponent>* output)
{
	outputDisparite.setOutput(output);
}