/*
* This file is part of robotreality
*
* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
* http://opensource.cit-ec.de/projects/robotreality
*
* This file may be licensed under the terms of the
* GNU General Public License Version 3 (the ``GPL''),
* or (at your option) any later version.
*
* Software distributed under the License is distributed
* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
* express or implied. See the GPL for the specific language
* governing rights and limitations.
*
* You should have received a copy of the GPL along with this
* program. If not, go to http://www.gnu.org/licenses/gpl.html
* or write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* The development of this software was supported by the
* Excellence Cluster EXC 277 Cognitive Interaction Technology.
* The Excellence Cluster EXC 277 is a grant of the Deutsche
* Forschungsgemeinschaft (DFG) in the context of the German
* Excellence Initiative.
*
*/

#include <XIMU.h>
#include <boost/interprocess/ipc/message_queue.hpp>
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/thread/thread.hpp>
#include <iostream>
#include <vector>
// #include <boost/asio.hpp>
#include <cv.h>
#include <highgui.h>
#include <dc1394/dc1394.h>
#include <signal.h>
#include <sys/stat.h>


using namespace boost::interprocess;
using namespace boost::serialization;
using namespace cv;

//do not touch unless you know what you are doing
string filename_imu_data = "/tmp/head_data.imu";
string filename_images   = "/tmp/grab_%06d";
string filename_mocap_config = "/tmp/mocap_config.mcfg";
string image_ending = "rgb.png";

size_t dataSize;
size_t image_rows;
size_t image_cols;
size_t elem_type;
size_t elem_size;

XIMU *ximu;
FILE *imu_output;


void imu_output_init(string filename){
        printf("> writing imu data to file \"%s\"\n",filename.c_str());

        if (!(imu_output = fopen(filename.c_str(), "w"))){
                printf("> ERROR: cant write data to '%s'\n",filename.c_str());
                exit(-1);
        }

        //get date/time
        struct tm *current;
        time_t now;
        time(&now);
        current = localtime(&now);
        char buf[2048];
        sprintf(buf, "%4d.%02d.%02d - %02d:%02d:%02d", 1900+current->tm_year, current->tm_mon, current->tm_mday,current->tm_hour,current->tm_min,current->tm_sec);
        fprintf(imu_output, "# IMU EULER DATA %s\n",buf);
        fflush(imu_output);
}

void imu_output_write_euler_dataset(int frame_number){
	double euler[3];
	
        //do not write if closed:
        if (imu_output == NULL){
                return;
        }

	//fetch data from imu
	ximu->get_euler(euler);

        fprintf(imu_output, "%d, ",frame_number);
        for(int i=0; i<3; i++){
                fprintf(imu_output, "%f, ", euler[i]);
        }
        fprintf(imu_output, "\n");
        fflush(imu_output);
}
void imu_output_close(){
        if (imu_output != NULL){
                printf("> closed imu datafile\n");
                fclose(imu_output);
        }
}

void worker_thread(){
	char filename[1024];
	
	printf("> worker: thread started, opening for message queue\n");

	message_queue mq (open_only,"message_queue");
	printf("> worker: queue opened\n");
	
	uchar data[dataSize];
	unsigned int priority=0;
	size_t received_size;
	
	Mat frame_rgb;
	int frame=0;
	
	//save to disk
	while(1){
		mq.receive((void*)&data[0], dataSize, received_size, priority);
		if (received_size != dataSize){
			printf("> worker: error, receive size mismatch!\n");
		}else{
			//process image
// 			printf("P");
			Mat m(image_rows, image_cols, elem_type, &data[0]);
// 			cvtColor(m,frame_rgb, CV_BayerRG2RGB);
// 			imshow("test", frame_rgb);
			sprintf(filename, (filename_images + ".%s").c_str(),frame++,"raw.pnm");
			imwrite(filename, m);
		}
	}
}

string get_formated_date(){
        struct tm *current;
        time_t now;
        time(&now);
        current = localtime(&now);

        char buf[256];

        snprintf(buf, 256, "%4d.%02d.%02d - %02d:%02d:%02d", 1900+current->tm_year, current->tm_mon, current->tm_mday,current->tm_hour,current->tm_min,current->tm_sec);
    return string(buf);
}

void write_mocap_config(int frame){
	printf("> creating mocap config file '%s'\n", filename_mocap_config.c_str());
	
	FILE *fh = fopen(filename_mocap_config.c_str(), "w");
	if (!fh){
		printf("> ERROR: can not open file for writing\n");
		exit(1);
	}
	fprintf(fh, "#mocap config for grabbed frames %s\n",get_formated_date().c_str());
	fprintf(fh, "free_running[bool] = 0\n");
	fprintf(fh, "grab_from_cam[bool] = 0\n");
	fprintf(fh, "rotate_resultview[bool] = 0\n");
	fprintf(fh, "show_output[bool] = 1\n");
	fprintf(fh, "data_output_class[int] = 0\n");
	fprintf(fh, "frame_number[int] = 0\n");
	fprintf(fh, "frame_number_end[int] = %d\n", frame);
	fprintf(fh, "head_zero_rotation[double] = -90\n");	
	fprintf(fh, "image_source_filename[string] = %s.%s\n", filename_images.c_str(), image_ending.c_str());
	fprintf(fh, "imu_data_source[string] = %s\n", filename_imu_data.c_str());
	fprintf(fh, "colorsample_vector_b[vector<int>] = 78:127:86:\n");
        fprintf(fh, "colorsample_vector_g[vector<int>] = 106:102:165:\n");
        fprintf(fh, "colorsample_vector_id[vector<int>] = 0:0:1:\n");
        fprintf(fh, "colorsample_vector_r[vector<int>] = 147:144:88:\n");

	
	fclose(fh);
	printf("> done. you can now open the results by running robotreality_mocap %s\n", filename_mocap_config.c_str());
	
}

void do_convert(){
	char filename[1024];
	char filename_out[1024];
	struct stat file_stats;
	Mat frame_raw;
	Mat frame_rgb;
	
	//ok now we have to convert all images:
	printf("> converting files (do not abort!)\n");
	int frame=0;
	bool images_to_process = true;
	
	while(images_to_process){
		sprintf(filename, (filename_images + ".%s").c_str(),frame,"raw.pnm");
		sprintf(filename_out, (filename_images + ".%s").c_str(),frame,image_ending.c_str());
		
		if (stat(filename, &file_stats) != -1){
			printf("> converting file %s to %s\n",filename,filename_out);
			frame_raw = imread(filename,0);
			cvtColor(frame_raw, frame_rgb, CV_BayerRG2RGB);
			imwrite(filename_out, frame_rgb);
			frame++;
		}else{
			 images_to_process = false;
		}
	}
	
	printf("> done. converted %d frames\n",frame);
	
	write_mocap_config(frame);
}

void do_grabbing(int device){
	//unprocessed cam frame
	Mat frame_raw;

	//init logfile
	imu_output_init(filename_imu_data);
	
	//open camera
	printf("> opening cam\n");
	VideoCapture *camera = new VideoCapture(device);
	
	if(!camera->isOpened()){
		printf("> ERROR: failed to open cam %d\n",device);
		exit(1);
	}

	if (!camera->set(CV_CAP_PROP_MODE, DC1394_VIDEO_MODE_FORMAT7_0)){
		printf("> ERROR: setting mode 7:0 failed\n");
		exit(1);
	}
	
	if ((!camera->set(CV_CAP_PROP_FRAME_HEIGHT, 524)) || (!camera->set(CV_CAP_PROP_FRAME_WIDTH, 652)) ){
		printf("> ERROR: setting size (mode 7:0 failed\n");
		exit(1);
	}
	
	if (!camera->set(CV_CAP_PROP_FPS, 0.0)){
		printf("> ERROR: set 0fps (=auto for mode7) failed\n");
		exit(1);
	}


	printf("> grabbing first frame & setting up message queue\n");
	while(!camera->grab()){}
	if(!camera->retrieve(frame_raw, 0)){
		printf("> ERROR: retrieve failed [this should NEVER fail!]\n");
		exit(1);
	}
		
	//set up mq: erase previous message queue
	message_queue::remove("message_queue");
	
	//create a message_queue.
	image_rows = frame_raw.rows;
	image_cols = frame_raw.cols;
	elem_size  = frame_raw.elemSize();
	elem_type  = frame_raw.type();
	dataSize = image_rows * image_cols * elem_size;
	int storage_size = 4000;
	
	message_queue mq(create_only,"message_queue",storage_size,dataSize); //2000 frames capacity
	printf("> queue object size %d -> storage needed = %d MB\n",(int)dataSize,(int)dataSize*storage_size/1024/1024);

	printf("> entering grab loop\n");
	
	printf("> starting worker thread (saves images)\n");
	boost::thread* thread = new boost::thread(worker_thread);

	
	int count=0;
	int frame = 0;
	while(1){
		if (!camera->grab()){
			printf("> ERROR: single frame grab failed [high cpu load? run as root?]\n");
		}else{
// 			printf("grab\n");
			if (!camera->retrieve(frame_raw, 0)){
				printf("> ERROR: retrieve failed [this should NEVER fail!]\n");
			}else{
				//ok, we got a frame, push it to save/worker thread:
				try{
					imu_output_write_euler_dataset(frame);
					if (!mq.try_send(frame_raw.data, dataSize, 0)){
						printf("> ERROR: queue full! [system too slow? saving to network? use /tmp!]\n");
					}
				}catch(interprocess_exception &ex){
					std::cerr << "> ERROR: queue problems: " << ex.what() << std::endl;
				}
			}
			
			frame++;
			if (count++ > 10){
				printf("> grabbed %d frames, queuesize=%4d\n",frame,(int)mq.get_num_msg());
				count = 0;
			}
		}
	}
	
	imu_output_close();
}

int main(int argc, char** argv){
	printf("\n");
	printf(",---------------------------------------------.\n");
	printf("| robotreality camera frame grabber                   |\n");
	printf("`---------------------------------------------´\n\n");
	printf("NOTE: intended to be used with an FIREWIRE ptGrey FireflyMV 13S2C!\n\n");

	if (argc <= 1){
		printf("\nusage:\n");
		printf("         grab data        : %s /dev/ttyUSB0\n",argv[0]);
		printf("         grab images only : %s NONE",argv[0]);
		printf("         convert          : %s --convert\n",argv[0]);
		printf("\n\n");
		exit(-1);
	}
	
	if (strcmp(argv[1], "--convert")==0){
		do_convert();
	}else{
		if (strcmp(argv[1], "NONE")==0){
			ximu = new XIMU("/dev/null");
			printf("> WARNING: commandline parameter set to ignore IMU data!\n> [[CAUTION: this is not what you normally want!]]\n");
		}else{
			ximu = new XIMU(argv[1]);
			if(!ximu->get_device_detected()){
				printf("> ERROR: no XIMU was found...\n");
				exit(1);
			}
		}
		do_grabbing(0);
	}
	
	return 0;
}


#if 0
#include <cv.h>
#include <highgui.h>

using namespace cv;

int main(){
CvCapture* capture = NULL;
IplImage *frame = NULL;
IplImage *result = NULL;
IplImage *bayer = NULL; 

CvFont font;
double hScale=0.5;
double vScale=0.5;
int lineWidth=2;

cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, hScale, vScale, 0, lineWidth);

capture = cvCreateCameraCapture(0);



if (capture) {
	printf("> got capture\n");
	cvNamedWindow("Capture", CV_WINDOW_AUTOSIZE);
	cvSetCaptureProperty(capture, CV_CAP_PROP_FPS, 60.0);
}


while(true){
	frame = cvQueryFrame(capture);
	if( !frame ){
		printf("did not get frame\n");
		break;
	}
printf("FRAME\n");
//	result = cvCloneImage(frame);

	cvSetImageCOI(frame,1);

	bayer = cvCreateImage( cvGetSize(frame), IPL_DEPTH_8U, 1);
	cvCopy(frame,bayer,NULL);

	result=cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
	cvCvtColor(bayer,result,CV_BayerRG2RGB); //CV_BayerGB2RGB); 

//	cvSetImageCOI(frame,1);
//	cvCvtColor(frame,result,CV_BayerGB2RGB); 


	//cvPutText(result, text_fps, cvPoint(0,20), &font, cvScalar(255,255,0));
	cvShowImage("Capture", result);
	if(cvWaitKey(10) != -1){
		printf("keypress\n");
		break;
	}
}

}
#endif