// from ueyecam_nodelet
#include <ros/package.h>
#include <camera_calibration_parsers/parse.h>
#include <sensor_msgs/fill_image.h>
#include <sensor_msgs/image_encodings.h>

#include <sstream>
#include "std_msgs/String.h"

// code sample image transport
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

// For time synchronizer
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>

#include <iostream>
#include <sstream>
#include <stdio.h>
#include <cstring>

using namespace sensor_msgs;
using namespace message_filters;

unsigned int k(0);

/*****************************************************/
/***                  SAVE_PPM                     ***/
/*****************************************************/
void save_ppm(char *filename, char *im, unsigned int width, unsigned int height)
{
    FILE *f;
    if(!(f=fopen(filename,"wb"))) {
        fprintf(stderr,"Could not open `%s'",filename);
        exit(1);
    }

    fprintf(f,"P6\n%d %d\n255\n",width,height);
    fwrite(im,1,width*height*3,f);
    fclose(f);
}
/*****************************************************/
/***               SAVE_PPM_NAMED                  ***/
/*****************************************************/
void save_ppm_named(char *basename, int k,char *endname,cv_bridge::CvImagePtr  cv_ptr_1)
{
    std::stringstream sstm;
    std::string nameFileL;
    sstm << basename << std::setfill('0') << std::setw(5) << k << endname;
    nameFileL = sstm.str();
    save_ppm((char*)nameFileL.c_str(), (char*)cv_ptr_1->image.data, cv_ptr_1->image.cols, cv_ptr_1->image.rows);
}
/*****************************************************/
/***              IMAGE CALLBACK                   ***/
/*****************************************************/
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
        cv_bridge::CvImagePtr cv_ptr_1 = cv_bridge::toCvCopy(msg, "bgr8");	
	save_ppm_named((char*)"/tmp/images/ueye_", k,(char*)"_mono.ppm",cv_ptr_1);
        cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
        cv::waitKey(30);
        k++;
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}
/*****************************************************/
/***              STEREO CALLBACK                  ***/
/*****************************************************/
void stereoCallback(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
    // Solve all of perception here...
    try
    {
        cv_bridge::CvImagePtr cv_ptr_1 = cv_bridge::toCvCopy(image1, "bgr8");
        cv_bridge::CvImagePtr cv_ptr_2 = cv_bridge::toCvCopy(image2, "bgr8");
        // save left image
	save_ppm_named((char*)"/tmp/images/ueye_", k,(char*)"_left.ppm",cv_ptr_1);
        // save right image
	save_ppm_named((char*)"/tmp/images/ueye_", k,(char*)"_right.ppm",cv_ptr_2);
        // show image
        cv::imshow("view_left", cv_ptr_1->image);
        cv::imshow("view_right", cv_ptr_2->image);
        cv::waitKey(30);
        k++;
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", image1->encoding.c_str());
    }
}
/*****************************************************/
/***                    MAIN                       ***/
/*****************************************************/
int main(int argc, char **argv)
{
    // create folder where we save images
    system("mkdir /tmp/images");

    ros::init(argc, argv, "listenerUeye");
    ros::NodeHandle nh;
    bool stereo(true);

    if(!stereo){
	ROS_INFO("ueye_cam_test in moncular mode"); 
        cv::namedWindow("view");
        cv::startWindowThread();

        image_transport::ImageTransport it(nh);
        image_transport::Subscriber sub = it.subscribe("/camera/image_raw", 1, imageCallback);

        ros::spin();

        cv::destroyWindow("view");
    }
    else{
	ROS_INFO("ueye_cam_test in binocular mode");
        cv::namedWindow("view_left");
        cv::namedWindow("view_right");
        cv::startWindowThread();

        message_filters::Subscriber<Image> image_left_sub(nh, "/stereo/left/image_raw", 1);
        message_filters::Subscriber<Image> image_right_sub(nh, "/stereo/right/image_raw", 1);
        TimeSynchronizer<Image, Image> sync(image_left_sub, image_right_sub, 10);
        sync.registerCallback(boost::bind(&stereoCallback, _1, _2));

        ros::spin();

        cv::destroyWindow("view_left");
        cv::destroyWindow("view_right");
    }
}