#include "header.h"
//Youll need to expand your window to view this properlly.  Sorry.
///Compute the Hessian!

// vector * vecin       The inputed vector, cloned into vector alterposi
// matrix * ddf         The Hessian matrix, filled as this hessfunk evaluates.
// double result        The result of the initial function evaluation
//                      Used to calculate the second derivatives.
// double interval      The interval; how far we step from x to calculate slope


void hessfunk(vector * vecin, matrix * ddf, double result, double interval)
{
	int n = vecin->size,row,col;		//The inputed size, and markers for the CURRENT row and col
	vector alterposi = craftvec(n, vecin->elements);	//A vector to store altered values
	vector alternegi = craftvec(n, vecin->elements);	//Likewise, but for negative moves.
	vector thrashdata = craftvecEZ(n);			//A vector to host values for evaluation; ignore it's contents.
	double xposiyposi, xposiynegi, xnegiyposi, xnegiynegi;	//Variables to hold the results of function evaluation
								//The partial derivative formula requires all four.
								//Read as (variable)_(directionshift) (variable)_(directionshift)
	for (row=0; row < n; row++)
	{
		for(col=0; col < n; col++)
		{
			if (row!=col)			//If off diagonal
			{
				alterposi.elements[row] += interval;
				alterposi.elements[col] += interval;
				xposiyposi = sumfunk(&alterposi,&thrashdata);

//				alterposi.elements[row] += interval;
				alterposi.elements[col] -= 2*interval;
				xposiynegi = sumfunk(&alterposi,&thrashdata);

				alterposi.elements[row] -= 2*interval;
				alterposi.elements[col] += 2*interval;
				xnegiyposi = sumfunk(&alterposi,&thrashdata);

//				alterposi.elements[row] += interval;
				alterposi.elements[col] -= 2*interval;
				xnegiynegi = sumfunk(&alterposi,&thrashdata);
	
//http://en.wikipedia.org/wiki/Finite_difference#Finite_difference_in_several_variables
				ddf->elements[row][col] = ( xposiyposi - xposiynegi - xnegiyposi + xnegiynegi )
								 / (4*interval*interval) ;
				alterposi.elements[row] = vecin->elements[row];	//Restore to normal
				alterposi.elements[col] = vecin->elements[col]; //Restore to normal
			}
			else	//row==col; on diagonal
			{
				alterposi.elements[row] += interval;	//using two vectors now!
				alternegi.elements[row] -= interval;	//See, it's alternegi!
				ddf->elements[row][col] = ( sumfunk(&alterposi,&thrashdata)
								- 2*result 
								+ sumfunk(&alternegi,&thrashdata)
								 ) / (interval*interval);
				alterposi.elements[row] = vecin->elements[row];	//Restore to normal
				alternegi.elements[row] = vecin->elements[row];	//Restore to normal
			}//End of Else
		}//End of Cols (this time)
	}//End of Rows
	destroyvec(&alterposi);//Reclaim memory from the vector  alterposi
	destroyvec(&alternegi);//Reclaim memory from the vector  alternegi
	return;
}