/* Trabalho 3 - Foto 3D
 * Ricardo Costa
 * rcosta@inf.puc-rio.br
 */

#define _CRT_SECURE_NO_WARNINGS

#include <stdio.h>
#include <memory.h>
#include "image.h"
#include "BCSLtable.h"
#include "stereo.h"

#define ROUND(_) (int)( (_) + 0.5f )

Image* imgSubtract(Image* image1, Image* image2)
{
	int w = imgGetWidth(image1);
	int h = imgGetHeight(image1);
	Image * imgRet = imgCreate(w, h);

	for ( int y = 0; y < h; y++ )
	{
		for ( int x = 0; x < w; x++ ) 
		{
			float rgb1[3];
			float rgb2[3];
			float diff[3];
			imgGetPixel3fv(image1, x, y, rgb1);
			imgGetPixel3fv(image2, x, y, rgb2);
			diff[0] = rgb1[0] - rgb2[0];
			diff[1] = rgb1[1] - rgb2[1];
			diff[2] = rgb1[2] - rgb2[2];
			imgSetPixel3fv(imgRet, x, y, diff);
		}
	}
	return imgRet;
}

int getPixelColorCode(float pixel[])
{
	if ( pixel[0] == pixel[1] && pixel[0] == pixel[2] && pixel[1] == pixel[2] )
	{
		return 0;
	}
	if ( pixel[0] >= pixel[1] && pixel[0] >= pixel[2] )
	{
		return 1;
	}
	if ( pixel[1] >= pixel[0] && pixel[1] >= pixel[2] )
	{
		return 2;
	}
	return 3;
}

Image* imgFindTransitionsV(Image* image1, Image* image2)
{
	int w = imgGetWidth(image1);
	int h = imgGetHeight(image1);
	Image * imgRet = imgCreate(w, h);

	for ( int y = 0; y < h; y++ )
	{
		float rgb[3];
		imgGetPixel3fv(image1, 0, y, rgb);
		int lastColor1 = getPixelColorCode(rgb);
		for ( int x = 1; x < w; x++ ) 
		{
			imgGetPixel3fv(image1, x, y, rgb);
			int currentColor1 = getPixelColorCode(rgb);
			if ( lastColor1 == 0 || currentColor1 == 0 )
			{
				lastColor1 = currentColor1;
				continue;
			}			
			if ( currentColor1 != lastColor1 )
			{

				imgGetPixel3fv(image2, x-1, y, rgb);
				int lastColor2 = getPixelColorCode(rgb);
				imgGetPixel3fv(image2, x, y, rgb);
				int currentColor2 = getPixelColorCode(rgb);
				if ( lastColor2 == 0 || currentColor2 == 0 )
				{
					lastColor2 = currentColor2;
					continue;
				}	
				int stripeCode = getBcslStripeCode(3, lastColor1, currentColor1, lastColor2, currentColor2);
				float outColor[3] = { 1.0f, 1.0f, stripeCode / 255.0f };
				imgSetPixel3fv(imgRet, x, y, outColor);
				lastColor1 = currentColor1;
			}
		}
	}
	return imgRet;
}

Image* imgFindTransitionsH(Image* image1, Image* image2)
{
	int w = imgGetWidth(image1);
	int h = imgGetHeight(image1);
	Image * imgRet = imgCreate(w, h);

	for ( int x = 0; x < w; x++ )
	{
		float rgb[3];
		imgGetPixel3fv(image1, x, 0, rgb);
		int lastColor1 = getPixelColorCode(rgb);
		for ( int y = 1; y < h; y++ ) 
		{
			imgGetPixel3fv(image1, x, y, rgb);
			int currentColor1 = getPixelColorCode(rgb);
			if ( lastColor1 == 0 || currentColor1 == 0 )
			{
				lastColor1 = currentColor1;
				continue;
			}			
			if ( currentColor1 != lastColor1 )
			{
				imgGetPixel3fv(image2, x, y-1, rgb);
				int lastColor2 = getPixelColorCode(rgb);
				imgGetPixel3fv(image2, x, y, rgb);
				int currentColor2 = getPixelColorCode(rgb);
				if ( lastColor2 == 0 || currentColor2 == 0 )
				{
					lastColor2 = currentColor2;
					continue;
				}	
				int stripeCode = getBcslStripeCode(3, lastColor1, currentColor1, lastColor2, currentColor2);
				float outColor[3] = { 1.0f, 1.0f, stripeCode / 255.0f };
				imgSetPixel3fv(imgRet, x, y, outColor);
				lastColor1 = currentColor1;
			}
		}
	}
	return imgRet;
}

Image* imgFindPoints(Image* imageV, Image* imageH, int coords[36][36*2][2])
{
	int w = imgGetWidth(imageV);
	int h = imgGetHeight(imageV);
	Image * imgRet = imgCreate(w, h);

	for ( int x = 0; x < w; x++ )
	{
		for ( int y = 0; y < h; y++ ) 
		{
			float rgb1[3];
			float rgb2[3];
			imgGetPixel3fv(imageV, x, y, rgb1);
			imgGetPixel3fv(imageH, x, y, rgb2);
			if ( rgb1[0] > 0.001f && rgb2[0] > 0.001f )
			{
				int code1 = ROUND(rgb1[2] * 255.f);
				int code2 = ROUND(rgb2[2] * 255.f);
				int vhalf = (y > h / 2) ? 36 : 0;
				code2 += vhalf;
				coords[code1][code2][0] = x;
				coords[code1][code2][1] = y;
				float rgbout[3] = { 1.0f, rgb1[2], rgb2[2] };
				imgSetPixel3fv(imgRet, x, y, rgbout);
			}
		}
	}
	return imgRet;
}

int main (void)
{
	printf("Trabalho 3 - Foto 3D\nRicardo Costa\n\n");

	FILE* outputFull = fopen("pontos.txt", "wt");

	for ( int angle = 0; angle <= 330; angle += 30 )
	{
		char outfile[256];
		sprintf(outfile, "pontos_%d.txt", angle);
		FILE* output = fopen(outfile, "wt");

		char filename[256];
		int coordsLeft[36][36*2][2];
		int coordsRight[36][36*2][2];

		memset(coordsLeft, -1, sizeof(coordsLeft));
		memset(coordsRight, -1, sizeof(coordsRight));

		printf("Processando imagens (angulo %d)...\n", angle);

	/* Câmera Esquerda - Padrão 1 - Vertical */
		sprintf(filename, "cavalo\\Cavalo Positivo 1 Camera 1 Rotacao %03d Vertical.bmp", angle);
		Image* imgLeftPos1v = imgReadBMP(filename);
		sprintf(filename, "cavalo\\Cavalo Negativo 1 Camera 1 Rotacao %03d Vertical.bmp", angle);
		Image* imgLeftNeg1v = imgReadBMP(filename);
		Image* imgLeft1v = imgSubtract(imgLeftPos1v, imgLeftNeg1v);
		imgDestroy(imgLeftPos1v);
		imgDestroy(imgLeftNeg1v);

	/* Câmera Esquerda - Padrão 2 - Vertical */
		sprintf(filename, "cavalo\\Cavalo Positivo 2 Camera 1 Rotacao %03d Vertical.bmp", angle);
		Image* imgLeftPos2v = imgReadBMP(filename);
		sprintf(filename, "cavalo\\Cavalo Negativo 2 Camera 1 Rotacao %03d Vertical.bmp", angle);
		Image* imgLeftNeg2v = imgReadBMP(filename);
		Image* imgLeft2v = imgSubtract(imgLeftPos2v, imgLeftNeg2v);
		imgDestroy(imgLeftPos2v);
		imgDestroy(imgLeftNeg2v);

	/* Câmera Esquerda - Faixas de Transição - Vertical */
		Image* imgLeftStripesV = imgFindTransitionsV(imgLeft1v, imgLeft2v);
		imgDestroy(imgLeft1v);
		imgDestroy(imgLeft2v);
		sprintf(filename, "output\\cam1v_%d.bmp", angle);
		//imgWriteBMP(filename, imgLeftStripesV);

	/* Câmera Esquerda - Padrão 1 - Horizontal */
		sprintf(filename, "cavalo\\Cavalo Positivo 1 Camera 1 Rotacao %03d Horizontal.bmp", angle);
		Image* imgLeftPos1h = imgReadBMP(filename);
		sprintf(filename, "cavalo\\Cavalo Negativo 1 Camera 1 Rotacao %03d Horizontal.bmp", angle);
		Image* imgLeftNeg1h = imgReadBMP(filename);
		Image* imgLeft1h = imgSubtract(imgLeftPos1h, imgLeftNeg1h);
		imgDestroy(imgLeftPos1h);
		imgDestroy(imgLeftNeg1h);

	/* Câmera Esquerda - Padrão 2 - Horizontal */
		sprintf(filename, "cavalo\\Cavalo Positivo 2 Camera 1 Rotacao %03d Horizontal.bmp", angle);
		Image* imgLeftPos2h = imgReadBMP(filename);
		sprintf(filename, "cavalo\\Cavalo Negativo 2 Camera 1 Rotacao %03d Horizontal.bmp", angle);
		Image* imgLeftNeg2h = imgReadBMP(filename);
		Image* imgLeft2h = imgSubtract(imgLeftPos2h, imgLeftNeg2h);
		imgDestroy(imgLeftPos2h);
		imgDestroy(imgLeftNeg2h);

	/* Câmera Esquerda - Faixas de Transição - Horizontal */
		Image* imgLeftStripesH = imgFindTransitionsH(imgLeft1h, imgLeft2h);
		imgDestroy(imgLeft1h);
		imgDestroy(imgLeft2h);
		sprintf(filename, "output\\cam1h_%d.bmp", angle);
		//imgWriteBMP(filename, imgLeftStripesH);

	/* Câmera Esquerda - Pontos de Transição */
		Image* imgLeftPoints = imgFindPoints(imgLeftStripesV, imgLeftStripesH, coordsLeft);
		imgDestroy(imgLeftStripesV);
		imgDestroy(imgLeftStripesH);
		sprintf(filename, "output\\cam1_points_%d.bmp", angle);
		//imgWriteBMP(filename, imgLeftPoints);


	/* Câmera Direita - Padrão 1 - Vertical */
		sprintf(filename, "cavalo\\Cavalo Positivo 1 Camera 2 Rotacao %03d Vertical.bmp", angle);
		Image* imgRightPos1v = imgReadBMP(filename);
		sprintf(filename, "cavalo\\Cavalo Negativo 1 Camera 2 Rotacao %03d Vertical.bmp", angle);
		Image* imgRightNeg1v = imgReadBMP(filename);
		Image* imgRight1v = imgSubtract(imgRightPos1v, imgRightNeg1v);
		imgDestroy(imgRightPos1v);
		imgDestroy(imgRightNeg1v);

	/* Câmera Direita - Padrão 2 - Vertical */
		sprintf(filename, "cavalo\\Cavalo Positivo 2 Camera 2 Rotacao %03d Vertical.bmp", angle);
		Image* imgRightPos2v = imgReadBMP(filename);
		sprintf(filename, "cavalo\\Cavalo Negativo 2 Camera 2 Rotacao %03d Vertical.bmp", angle);
		Image* imgRightNeg2v = imgReadBMP(filename);
		Image* imgRight2v = imgSubtract(imgRightPos2v, imgRightNeg2v);
		imgDestroy(imgRightPos2v);
		imgDestroy(imgRightNeg2v);

	/* Câmera Direita - Faixas de Transição - Vertical */
		Image* imgRightStripesV = imgFindTransitionsV(imgRight1v, imgRight2v);
		imgDestroy(imgRight1v);
		imgDestroy(imgRight2v);
		sprintf(filename, "output\\cam2v_%d.bmp", angle);
		//imgWriteBMP(filename, imgRightStripesV);

	/* Câmera Direita - Padrão 1 - Horizontal */
		sprintf(filename, "cavalo\\Cavalo Positivo 1 Camera 2 Rotacao %03d Horizontal.bmp", angle);
		Image* imgRightPos1h = imgReadBMP(filename);
		sprintf(filename, "cavalo\\Cavalo Negativo 1 Camera 2 Rotacao %03d Horizontal.bmp", angle);
		Image* imgRightNeg1h = imgReadBMP(filename);
		Image* imgRight1h = imgSubtract(imgRightPos1h, imgRightNeg1h);
		imgDestroy(imgRightPos1h);
		imgDestroy(imgRightNeg1h);

	/* Câmera Direita - Padrão 2 - Horizontal */
		sprintf(filename, "cavalo\\Cavalo Positivo 2 Camera 2 Rotacao %03d Horizontal.bmp", angle);
		Image* imgRightPos2h = imgReadBMP(filename);
		sprintf(filename, "cavalo\\Cavalo Negativo 2 Camera 2 Rotacao %03d Horizontal.bmp", angle);
		Image* imgRightNeg2h = imgReadBMP(filename);
		Image* imgRight2h = imgSubtract(imgRightPos2h, imgRightNeg2h);
		imgDestroy(imgRightPos2h);
		imgDestroy(imgRightNeg2h);

	/* Câmera Direita - Faixas de Transição - Horizontal */
		Image* imgRightStripesH = imgFindTransitionsH(imgRight1h, imgRight2h);
		imgDestroy(imgRight1h);
		imgDestroy(imgRight2h);
		sprintf(filename, "output\\cam2h_%d.bmp", angle);
		//imgWriteBMP(filename, imgRightStripesH);

	/* Câmera Direita - Pontos de Transição */
		Image* imgRightPoints = imgFindPoints(imgRightStripesV, imgRightStripesH, coordsRight);
		imgDestroy(imgRightStripesV);
		imgDestroy(imgRightStripesH);
		sprintf(filename, "output\\cam2_points_%d.bmp", angle);
		//imgWriteBMP(filename, imgRightPoints);

	/* Reconstrução Estéreo */
		printf("Gerando lista de pontos (angulo = %d)...\n", angle);
		double cx = 1024.0 / 2.0;
		double cy = 768.0 / 2.0;
		double focalLeft = 927.0580085;
		Vector eyeLeft = algVector(51.7638, 50.0, 200.0, 1.0);
		Matrix rotLeft = algMatrix4x4(0.9681, 0.0, -0.250563, 0.0, 
			-0.0698833, 0.960319, -0.270008, 0.0,
			0.24062, 0.278905, 0.929685, 0.0,
			0.0, 0.0, 0.0, 1.0);
		double focalRight = 927.0580085;
		Vector eyeRight = algVector(-51.7638, 50.0, 200.0, 1.0);
		Matrix rotRight = algMatrix4x4( 0.9681, 0.0, 0.250563, 0.0,
			0.0698833, 0.960319, -0.270008, 0.0,
			-0.24062, 0.278905, 0.929685, 0.0,
			0.0, 0.0, 0.0, 1.0);

		Matrix vertexRot = algMatrixRotate(-angle, 0., 1., 0.);

		for ( int i = 0; i < 36; i++ )
		{
			for ( int j = 0; j < 36*2; j++ )
			{
				if ( coordsLeft[i][j][0] != -1 && coordsRight[i][j][0] != -1 )
				{
					int error;
					Vector point = stereoReconstruct(
						coordsLeft[i][j][0], coordsLeft[i][j][1], focalLeft, rotLeft, eyeLeft, cx, cy,
						coordsRight[i][j][0], coordsRight[i][j][1], focalRight, rotRight, eyeRight, cx, cy, 
						&error);
					fprintf(output, "%f %f %f\n", point.x, point.y, point.z);
					point = algTransf(vertexRot, point);
					fprintf(outputFull, "%f %f %f\n", point.x, point.y, point.z);
				}
			}
		}

		fclose(output);
	}

	fclose(outputFull);

	printf("Programa finalizado com sucesso.\n");
	
	return 0;
}