#include <stdlib.h> 
#include <stdio.h>
#include <cmath>
#include <iostream>
#include <fstream>
#include "vision_utilities.h"

using namespace std;

int p2(char *inputFilename, char *im1fname, char *im2fname, char *im3fname, char *im4fname, char *im5fname, char *outputFilename)
{
	/* Step 1. File input */
	ifstream fin(inputFilename);
	int centroid_x = 0;
	int centroid_y = 0;
	double radius;
	fin>>centroid_x>>centroid_y>>radius;
	fin.close();

	Image **im = new Image*[5];
	for (int k = 0; k < 5; k++) 
		im[k] = new Image;//DO NOT FORGET THIS!!!OR U WILL GET SEGMENTATION FAULT
	readImage(im[0], im1fname);
	readImage(im[1], im2fname);
	readImage(im[2], im3fname);
	readImage(im[3], im4fname);
	readImage(im[4], im5fname);
	int NRows = getNRows(im[0]);//# of rows
	int NCols = getNCols(im[0]);//# of cols
	
	/* Step 2. Find the brightest point */
	int brightest_x[5] = {0,0,0,0,0};
	int brightest_y[5] = {0,0,0,0,0};
	int maxMagnitude[5] = {0,0,0,0,0};
	double nx[5];
	double ny[5];
	double nz[5];
	for (int k = 0; k < 5; k++)
	{
		for (int i = 0; i < NRows; i++)
		{
			for (int j = 0; j < NCols; j++)
			{
				if (getPixel(im[k],i,j) > maxMagnitude[k])
				{
					maxMagnitude[k] = getPixel(im[k],i,j);
					brightest_x[k] = j;
					brightest_y[k] = i;
				}				
			}
		}


	/* Step 3.a. Compute its normal vector */
		nx[k] = brightest_x[k] - centroid_x;
		ny[k] = brightest_y[k] - centroid_y;
		nz[k] = -sqrt(MAX(radius*radius-nx[k]*nx[k]-ny[k]*ny[k],0));

	/* Step 3.b. Scale it using its magnitude */
		nx[k] = nx[k]/radius*maxMagnitude[k];
		ny[k] = ny[k]/radius*maxMagnitude[k];
		nz[k] = nz[k]/radius*maxMagnitude[k];
	}

	/* Step 4. File output */
	ofstream fout(outputFilename);
	for (int k = 0; k < 5; k++)
	{
		fout<<nx[k]<<" "<<ny[k]<<" "<<nz[k]<<"\n";
		//cout<<maxMagnitude[k]<<"\n";//for test
	}
	fout.close();
	printf("U got it\n");
	return 0;
}

int main(int argc, char **argv) 
{ 
	if (argc <= 7){ 
		printf("Usage: %s <input parameters file> <image1> <image2> <image3> <image4> <image5> <output directions file>\n", argv[0]); 
		return 0; 
	} 
	char *inputFilename = argv[1]; 
	char *im1fname = argv[2];
	char *im2fname = argv[3];
	char *im3fname = argv[4];
	char *im4fname = argv[5];
	char *im5fname = argv[6];
	char *outputFilename = argv[7];
	p2(inputFilename,im1fname,im2fname,im3fname,im4fname,im5fname,outputFilename);
	return 0; 
}
