/*
 * Copyright (C) 2013 Paul Kocialkowski
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU 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 General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>

#define BIG2LITTLE_ENDIAN(big)	((big & 0xff) << 24 | (big & 0xff00) << 8 | (big & 0xff0000) >> 8 | (big & 0xff000000) >> 24)

int s5c73m3_interleaved_decode(void *data, int size,
	void **yuv_data, int *yuv_size, int yuv_width, int yuv_height,
	void **jpeg_data, int *jpeg_size)
{
	void *yuv_buffer;
	void *jpeg_buffer;
	int yuv_length;
	int jpeg_length;
	unsigned char *yuv_p;
	unsigned char *jpeg_p;
	unsigned char *data_p;
	unsigned int *offset_p;
	int pointers_array_offset;
	int pointers_array_size;
	int yuv_offset_last;
	int yuv_offset;
	int yuv_line_size;
	int gap;
	int i;

	if (data == NULL || size <= 0 || yuv_data == NULL || yuv_size == NULL || yuv_width <= 0 || yuv_height <= 0 || jpeg_data == NULL || jpeg_size == NULL)
		return -EINVAL;

	yuv_buffer = malloc(size);
	jpeg_buffer = malloc(size);
	yuv_length = 0;
	jpeg_length = 0;

	data_p = (unsigned char *) data;
	data_p += size; // End of the first plane (interleaved buffer)
	data_p += 4084; // Fixed offset, make sure metadata is enabled!

	// Read the pointers array offset
	offset_p = (unsigned int *) data_p;
	pointers_array_offset = BIG2LITTLE_ENDIAN(*offset_p);

	// Read the pointers array size, it should be 4 * yuv_height
	data_p += sizeof(pointers_array_offset);
	offset_p = (unsigned int *) data_p;
	pointers_array_size = BIG2LITTLE_ENDIAN(*offset_p);

	printf("Interleaved pointers array at offset 0x%x, %d bytes long\n", pointers_array_offset, pointers_array_size);

	data_p = (unsigned char *) data + pointers_array_offset;
	yuv_p = (unsigned char *) yuv_buffer;
	jpeg_p = (unsigned char *) jpeg_buffer;

	yuv_line_size = yuv_width * 2;
	yuv_offset_last = 0;
	yuv_offset = 0;
	i = 0;

	while (i < pointers_array_size) {
		offset_p = (unsigned int *) data_p;
		yuv_offset = BIG2LITTLE_ENDIAN(*offset_p);

		gap = yuv_offset - yuv_offset_last - yuv_line_size;
		if (gap > 0) {
			data_p = (unsigned char *) data + yuv_offset_last + yuv_line_size;
			memcpy(jpeg_p, data_p, gap);
			jpeg_p += gap;
			jpeg_length += gap;
		}

		yuv_offset_last = yuv_offset;

		data_p = (unsigned char *) data + yuv_offset;
		memcpy(yuv_p, data_p, yuv_line_size);
		yuv_p += yuv_line_size;
		yuv_length += yuv_line_size;

		data_p = (unsigned char *) offset_p;
		data_p += sizeof(yuv_offset);
		i += sizeof(yuv_offset);
	}

	*yuv_data = yuv_buffer;
	*yuv_size = yuv_length;

	*jpeg_data = jpeg_buffer;
	*jpeg_size = jpeg_length;

	return 0;
}

int main(void)
{
	int interleaved_size = 0xA00000;
	int metadata_size = 0x1000;
	int data_fd, yuv_fd, jpeg_fd;
	void *data;
	void *yuv_data = NULL;
	void *jpeg_data = NULL;
	int size;
	int yuv_size = 0;
	int jpeg_size = 0;

	data_fd = open("exynos_camera.raw", O_RDONLY);
	if (data_fd < 0) {
		printf("Unable to open data\n");
		return 1;
	}

	yuv_fd = open("exynos_camera.yvy", O_WRONLY | O_TRUNC | O_CREAT);
	if (yuv_fd < 0) {
		printf("Unable to open yuv\n");
		return 1;
	}

	jpeg_fd = open("exynos_camera.jpg", O_WRONLY | O_TRUNC | O_CREAT);
	if (jpeg_fd < 0) {
		printf("Unable to open jpeg\n");
		return 1;
	}

	size = interleaved_size + metadata_size;
	data = malloc(size);

	read(data_fd, data, size);

	close(data_fd);

	s5c73m3_interleaved_decode(data, interleaved_size, &yuv_data, &yuv_size, 960, 720, &jpeg_data, &jpeg_size);

	if (yuv_data != NULL && yuv_size > 0) {
		write(yuv_fd, yuv_data, yuv_size);
		free(yuv_data);
	}
	close(yuv_fd);

	if (jpeg_data != NULL && jpeg_size > 0) {
		write(jpeg_fd, jpeg_data, jpeg_size);
		free(jpeg_data);
	}
	close(jpeg_fd);

	return 0;
}