#include <errno.h>
#include <error.h>
#include <stdio.h>
#include <stdlib.h> /* atexit */
#include <unistd.h> /* dup2 */

#include <libfreenect/libfreenect.h>
#include <libusb-1.0/libusb.h>

#include "util.h"

FILE *depth_stream = NULL;
FILE *video_stream = NULL;
FILE *accel_stream = NULL;
freenect_context *ctx;

void dump_ffmpeg_24(FILE *stream, uint32_t timestamp UNUSED, void *data,
                    size_t data_size)
{
	fwrite(data, data_size, 1, stream);
}

void dump_ffmpeg_pad16(FILE *stream, uint32_t timestamp UNUSED, void *data_anon,
                       size_t data_size)
{
	uint16_t* data = data_anon;
	uint16_t* end = (void*)&((char*)data_anon)[data_size];
	while (data < end) {
		uint32_t z = *data;
		fwrite(((char*)(&z)), 3, 1, stream);
		data = &data[1];
	}
}

void handle_accel(freenect_device *dev UNUSED, freenect_raw_tilt_state* data)
{
	printf("handle accel\n");
	double x, y, z;
	freenect_get_mks_accel(data, &x, &y, &z);
	//fprintf(accel_stream, "x=%f\ty=%f\tz=%f\n", x, y, z);
}

void handle_depth(freenect_device *dev UNUSED, void *depth, uint32_t timestamp)
{
	printf("handle depth\n");
	dump_ffmpeg_pad16(depth_stream, timestamp, depth,
	                  freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM,
	                                           FREENECT_DEPTH_11BIT).bytes);
}

void handle_video(freenect_device *dev, void *rgb, uint32_t timestamp)
{
	printf("handle video\n");
	dump_ffmpeg_24(video_stream, timestamp, rgb,
	               freenect_get_current_video_mode(dev).bytes);
}

void print_mode(const char *name, freenect_frame_mode mode) {
	/* This is just a courtesy function to let the user know the mode
	   if it becomes a bother for maintainability just comment out the
	   code in its body.  It will only break if struct entries go missing.
	*/
	printf("%s Mode: {%d, %d, {%d}, %d, %d, %d, %d, %d, %d, %d}\n", name,
	       mode.reserved, (int)mode.resolution, (int)mode.video_format, mode.bytes, mode.width,
	       mode.height, mode.data_bits_per_pixel, mode.padding_bits_per_pixel,
	       mode.framerate, mode.is_valid);
}

void cleanup() {
	if (ctx)
		freenect_shutdown(ctx);
	if (video_stream)
		fclose(video_stream);
	if (depth_stream)
		fclose(depth_stream);
	if (accel_stream)
		fclose(accel_stream);
}

void usage() {
	printf("Usage: %s [-h] [-v video.rgb24] [-d depth.rgb24] [-a accel.txt]\n", program_invocation_name);
}

int main(int argc, char *argv[]) {
	int res = 0;
	freenect_device *dev;

	atexit(cleanup);

	for (int i = 1; i < argc; i++) {
		if (argv[i][0] == '-' && argv[i][1] != '\0' && argv[i][2] == '\0') {
			switch (argv[i][1]) {
			case 'h':
				usage();
				return EXIT_SUCCESS;
			case 'v':
				i++;
				video_stream = fopen(argv[i], "w");
				if (video_stream == NULL)
					error(EXIT_FAILURE, errno, "fopen: %s", argv[i]);
				break;
			case 'd':
				i++;
				depth_stream = fopen(argv[i], "w");
				if (video_stream == NULL)
					error(EXIT_FAILURE, errno, "fopen: %s", argv[i]);
				break;
			case 'a':
				i++;
				accel_stream = fopen(argv[i], "w");
				if (video_stream == NULL)
					error(EXIT_FAILURE, errno, "fopen: %s", argv[i]);
				break;
			default:
				dup2(2, 1);
				usage();
				return EXIT_FAILURE;
			}
		} else {
			dup2(2, 1);
			usage();
			return EXIT_FAILURE;
		}
	}
					
	res = freenect_init(&ctx, 0);
	if (res) {
		error(EXIT_FAILURE, 0, "freenect_init: %s", libusb_strerror(res));
	}

	freenect_device_flags devices = 0;
	if (video_stream || depth_stream)
		devices |= FREENECT_DEVICE_CAMERA;
	if (accel_stream)
		devices |= FREENECT_DEVICE_MOTOR;

	freenect_select_subdevices(ctx, devices);
	if ((res = freenect_open_device(ctx, &dev, 0))) {
		error(EXIT_FAILURE, 0, "freenect_open_device: %s", libusb_strerror(res));
	}

	if (video_stream) {
		print_mode("Video", freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
		freenect_set_video_mode(dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
		freenect_start_video(dev);
		freenect_set_video_callback(dev, handle_video);
	}

	if (depth_stream) {
		print_mode("Depth", freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
		freenect_set_depth_mode(dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
		freenect_start_depth(dev);
		freenect_set_depth_callback(dev, handle_depth);
	}

	if (accel_stream) {
	}

	while (freenect_process_events(ctx) >= 0) {
		if (accel_stream) {
			freenect_raw_tilt_state* state;
			freenect_update_tilt_state(dev);
			state = freenect_get_tilt_state(dev);
			handle_accel(dev, state);
		}
	}

	freenect_stop_depth(dev);
	freenect_stop_video(dev);
	freenect_close_device(dev);
}