// Daniel Knüttel Aufgabe2

/*
* Copyright(c) 2018 Daniel Knüttel 
*/

/*    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/>.
*
*    Dieses Programm ist Freie Software: Sie können es unter den Bedingungen
*    der GNU General Public License, wie von der Free Software Foundation,
*    Version 3 der Lizenz oder (nach Ihrer Wahl) jeder neueren
*    veröffentlichten Version, weiterverbreiten und/oder modifizieren.
*
*    Dieses Programm wird in der Hoffnung, dass es nützlich sein wird, aber
*    OHNE JEDE GEWÄHRLEISTUNG, bereitgestellt; sogar ohne die implizite
*    Gewährleistung der MARKTFÄHIGKEIT oder EIGNUNG FÜR EINEN BESTIMMTEN ZWECK.
*    Siehe die GNU General Public License für weitere Details.
*
*    Sie sollten eine Kopie der GNU General Public License zusammen mit diesem
*    Programm erhalten haben. Wenn nicht, siehe <http://www.gnu.org/licenses/>.
*/

#include <stdio.h>
#include <stdlib.h>
#include "lib/lrmp.h"

/*
 * General Information:
 *
 * This program will print all the required (N, error) information to stdout,
 * messages to the user are printed to stderr. Typically the output of
 * this program will be piped into plot.py to create some plots. 
 *
 * ./main | python3 plot.py
 *
 * to do so.
 *
 * Compilation:
 *
 * gcc -o main -lm Projekt2_Knuettel_Daniel.c lib/lrmp.c lib/vorrueckwaertsub.c
 *
 *
 * Preprocessor flags:
 *
 * - DEBUG turns on some more output.
 * - NOCRAPMODE enables a reusable interface which is not conformous to the
 *   specification given but allowes to used the algorithms with arbitrary
 *   functions. Also this adds the functions `write_error_(newton|bisek)`.
 * - ALLOW_NEGATGIVE_ERRORS allows negative errors which might be interesting.
 *
 */


#define abs(x) (x < 0 ? (-1)*x : x)



/*
 * \brief Calculate the next interval for the bisection
 *        using the current interval x_minus, x_plus.
 *
 * \param x_minus Lower boundary of the interval. Will be updated to the next
 *                search interval. If the search has been sucessful, this will 
 *                be the result.
 * \param x_plus Upper boundary of the interval.
 *               Will be updated to the next search interval.
 * \param f Used function.
 * \param eps If f(x) < eps, stop.
 *
 * \return 0, if nothing happened, 0xf000, if f(x_min) < eps.
 * */
int bisec_one(double * x_minus
		, double * x_plus
		, double (*f)(double)
		, double eps);


/*
 * \brief Calculate f(x) < eps using bisection. Note that this requires: f(a)f(b) < 0.
 *        For functions not meeting this requirement the behaviour of this function is
 *        undefined. It might however converge to the boundaries a or b.
 *
 * \param f Used function.
 * \param a Lower boundary of the interval to search in.
 * \param b Upper boundary of the interval to search in.
 * \param eps Used precision.
 * \param Nmax Stop searching after Nmax iterations.
 * \param flag Will be set to 0xf000, if the goal has been reached,
 *             to 0xf001, if Nmax has been reached
 *             , to 0xf004 if f(a)*f(b) >= 0.
 *             Any other value indicates an unknown error.
 * \param file The function will not use this parameter, it is there just
 *             for backwards compability.
 *
 * \return The last x_minus which is the result of the bisection, if *flag = 0xf000,
 *         or the closest the function was, when it hit Nmax.
 * */
double bisek(double (*f)(double)
		, double a
		, double b
		, double eps
		, int Nmax
		, int * flag
		, FILE * file);

#ifdef NOCRAPMODE
/*
 * \brief Calculate f(x) < eps using bisek and write the results to file.
 *
 * \param f Used function.
 * \param a Lower boundary of the interval to search in.
 * \param b Upper boundary of the interval to search in.
 * \param eps Used precision.
 * \param Nmax Stop searching after Nmax iterations.
 * \param flag Will be set to 0xf000, if the goal has been reached,
 *             to 0xf001, if Nmax has been reached
 *             , to 0xf004 if f(a)*f(b) >= 0.
 *             Any other value indicates an unknown error.
 * \param file Write ``Nmax,error\n`` to this file.
 * \param x0 The real result for bisek.
 *
 * \return The result of the bisection
 * */
double write_error_bisek(double (*f)(double)
		, double a
		, double b
		, double eps
		, int Nmax
		, int * flag
		, FILE * file
		, double x0);
#endif



/*
 * \brief Calculate f(x) < eps or a minimum of f.
 *
 * \param x0 The starting point for the calculation
 * \param f The function that should be used
 * \param df The differential of f, must be continuous
 * \param eps The precision
 * \param Nmax The maximal number of iterations
 * \param flag Flag indicating the status of the calculation. 
 *             0xe000: Precision has been hit. f(x) < eps.
 *             0xe001: Nmax has been hit f(x) > eps.
 *             0xe002: Found a minimum of f (<= df(x) = 0)
 * \param file Unused. See write_error_newton. Only for legacy reasons.
 *
 * \return x Such that the condition in flag is met.
 * */

double newton(double x0
		, double (*f)(double)
		, double (*df)(double)
		, double eps
		, int Nmax
		, int * flag
		, FILE * file);


#ifdef NOCRAPMODE
/*
 * \brief Calculate f(x) < eps or a minimum of f. And write the results to file
 *
 * \param x0 The starting point for the calculation
 * \param f The function that should be used
 * \param df The differential of f, must be continuous
 * \param eps The precision
 * \param Nmax The maximal number of iterations
 * \param flag Flag indicating the status of the calculation. 
 *             0xe000: Precision has been hit. f(x) < eps.
 *             0xe001: Nmax has been hit f(x) > eps.
 *             0xe002: Found a minimum of f (<= df(x) = 0)
 * \param file Write "N,error\n" to file. error is calculated from the 
 *             expected value x1.
 * \param x1 Real result of the newton method.
 *
 * \return x Such that the condition in flag is met.
 * */
double write_error_newton(double x0
		, double (*f)(double)
		, double (*df)(double)
		, double eps
		, int Nmax
		, int * flag
		, FILE * file
		, double x1);
#endif

/*
 * \brief Calculate one step of the newton iteration.
 *
 * \param x The current value for the newton iteration. 
 *          For nonzero return values x is unchanged, for
 *          zero return values x is updated according to the newton 
 *          algorithm.
 * \param f The function that should be used
 * \param df The differential of f, must be continuous
 * \param eps The precision
 *
 * \return if f(x) < eps: 0xe000
 *         if df(x) < eps: 0xe002
 *         else: 0x0000
 *
 * */
int newton_one(double * x
		, double (*f)(double)
		, double (*df)(double)
		, double eps);


/*
 * \brief Calculate the multi dimensional newton iteration.
 * 
 * \param N Dimension of the system
 * \param x0 Starting point
 * \param f Used function
 * \param df Differential of f
 * \param fx Result of the calculation
 * \param Nmax The maximal number of iterations
 * \param eps Precision
 *
 * \return If the calculation suceeded (ie |f(x)| < eps): 0xe000
 *         If df(x) is not regulary: 0xe002
 *         If Nmax has been hit: 0xe001
 * */
int newton_mult(int N
		, double * x0
		, void (*f)(double *, double *, int)
		, void (*df)(double *, double **, int)
		, double *fx
		, int Nmax
		, double eps);

/*
 * \brief Calculate one iteration for newton_mult
 *
 * \param N Dimension of the system
 * \param x Current point.
 *          For nonzero retirn values x is unchanged, for
 *          zero return values x is updated according to the newton 
 *          algorithm.
 * \param f Used function
 * \param df Differential of f
 * \param eps Precision
 *
 * \return if f(x) < eps: 0xe000
 *         if df(x) < eps: 0xe002
 *         else: 0x0000
 *
 * */
int newton_mult_one(int N
		, double * x
		, void (*f)(double *, double *, int)
		, void (*df)(double *, double **, int)
		, double eps);

/*
 * \brief Copy from src to dst
 *
 * \param N Dimension of the vector
 * \param dst Destination
 * \param src Source
 *
 * \return 0
 * */
int vector_copy(int N
		, double * dst
		, double * src);

/*
 * \brief Compute the modulus of vector x
 * \param N Dimension of x
 * \param x Vector x
 * \return The modulus
 * */
double modulus(int N
		, double * x);

// The test functions in one dimension.
double g1(double x);
double g2(double x);
double dg1(double x);
double dg2(double x);

// The  test functio in 2D
void g(double * x, double * y, int N);
void dg(double * x, double ** y, int N);


int main(void)
{

	int i, j;
	int flag;
	double result;

	// Calculate the bisection of g1.
	for(i = 1, j = 0; i < 100 ;i += 1, j++)
	{
#ifdef NOCRAPMODE
		write_error_bisek(g1, -2.5, -100, 1e-7, i, &flag, stdout, -6);
#else
		bisek(g1, -2.5, -100, 1e-7, i, &flag, stdout);
#endif
	}
	printf("XXX\n");
	fprintf(stderr, "wrote %d lines for g1 to /dev/stdout\n", j);
	
	// Calculate the bisection of g2.
	// Note that the output of this will be basically crap, because
	// g2 does not meet the requirements for a bisection.
	for(i = 1, j = 0; i < 100 ;i += 1, j++)
	{
#ifdef NOCRAPMODE
		write_error_bisek(g2, -2.5, -100, 1e-7, i, &flag, stdout, -6);
#else
		bisek(g2, -2.5, -100, 1e-7, i, &flag, stdout);
#endif
	}
	printf("XXX\n");
	fprintf(stderr, "wrote %d lines for g2 to /dev/stdout\n", j);

	// Calculate the newton iteration of g1.
	for(i = 1, j = 0; i < 100 ;i += 1, j++)
	{
#ifdef NOCRAPMODE
		write_error_newton(-100, g1, dg1, 1e-7, i, &flag, stdout, -6);
#else
		newton(-100, g1, dg1, 1e-7, i, &flag, stdout);
#endif
	}
	printf("XXX\n");
	fprintf(stderr, "wrote %d lines for g1 to /dev/stdout\n", j);
	
	// Calculate the newton iteration of g2.
	// Note that the output of this will be basically crap, because
	// g2 does not meet the requirements for the newton iteration.
	for(i = 1, j = 0; i < 100 ;i += 1, j++)
	{
#ifdef NOCRAPMODE
		write_error_newton(-100, g2, dg2, 1e-7, i, &flag, stdout, -6);
#else
		newton(-100, g2, dg2, 1e-7, i, &flag, stdout);
#endif
	}
	printf("XXX\n");
	fprintf(stderr, "wrote %d lines for g2 to /dev/stdout\n", j);

	// Calculate the multi dimensional newton iteration
	

	// Allocate and initialzie the two starting points.
	double * x1 = malloc(sizeof(double) * 2)
		, * x2 = malloc(sizeof(double) * 2);
	x1[0] = -0.5;
	x1[1] = 1.4;
	x2[0] = -0.14;
	x2[1] = 1.47;

	double * x = malloc(sizeof(double) * 2);
	// use x1
	flag = newton_mult(2, x1, g, dg, x, 100, 1e-7);
	fprintf(stderr, "newton_mult -> %x\n", flag);
	// calculate g(x)
	g(x, x1, 2);
	fprintf(stderr, "result: (%.4e, %.4e), f(%.4e, %.4e) = (%.4e, %.4e)\n"
			, x[0], x[1]
			, x[0], x[1]
			, x1[0], x1[1]);

	// use x2
	flag = newton_mult(2, x2, g, dg, x, 100, 1e-7);
	fprintf(stderr, "newton_mult -> %x\n", flag);
	// calculate g(x)
	g(x, x2, 2);
	fprintf(stderr, "result: (%.4e, %.4e), f(%.4e, %.4e) = (%.4e, %.4e)\n"
			, x[0], x[1]
			, x[0], x[1]
			, x2[0], x2[1]);

	free(x);
	free(x1);
	free(x2);


	return 0;
}

double g1(double x)
{
	return (x + 6) * (x*x + 1);
}

double g2(double x)
{
	double result = 1;
	int i;
	for(i = 0; i < 4; i++)
	{
		result *= (x + 6);
	}

	return result;
}
double dg1(double x)
{
	return 3*x*x + 12*x + 1;
}
double dg2(double x)
{
	double result = 4;
	int i;
	for(i = 0; i < 3; i++)
	{
		result *= (x + 6);
	}

	return result;
}
	

int bisec_one(double * x_minus
		, double * x_plus
		, double (*f)(double)
		, double eps)
{
	double a = *x_minus
		, b = *x_plus;
	double x_m = (b + a) / 2.0;
	double y_m = f(x_m);

	if(y_m < 0)
	{
		// We hit precision.
		if(y_m > -1 * eps)
		{
			*x_minus = x_m;
			return 0xf000;
		}
		// Continue.
		*x_minus = x_m;
		*x_plus = b;
		return 0;
	}
	// We hit precision.
	if(y_m < eps)
	{
		*x_minus = x_m;
		return 0xf000;
	}
	// Continue.
	*x_minus = a;
	*x_plus = x_m;
	return 0;
}

double 
#ifdef NOCRAPMODE
	bisek
#else
	__inline_bisek
#endif
	(double (*f)(double)
		, double a
		, double b
		, double eps
		, int Nmax
		, int * flag
		, FILE * file)
{
	if(f(a)*f(b) >= 0)
	{
		*flag = 0xf004;
		return 0;
	}
	double x_minus = a
		, x_plus = b;
	if(a > b)
	{
		x_minus = b;
		x_plus = a;
	}
	int i;
	int status;
	for(i = 0; i < Nmax; i++)
	{
		// Calculate the next interval.
		status = bisec_one(&x_minus, &x_plus, f, eps);
		// We have hit precision.
		if(status)
		{
			*flag = status;
			return x_minus;
		}
	}
	// We ran out of iterations.
	*flag = 0xf001;
	return x_minus;
}

double 
#ifdef NOCRAPMODE
	write_error_bisek
#else
	bisek
#endif
	(double (*f)(double)
		, double a
		, double b
		, double eps
		, int Nmax
		, int * flag
		, FILE * file
#ifdef NOCRAPMODE
		, double x0
#endif
		)
{
#ifndef NOCRAPMODE
	double x0 = -6;
#endif
	double result
		, error;

	result = 
#ifdef NOCRAPMODE
		bisek
#else
		__inline_bisek
#endif
			(f, a, b, eps, Nmax, flag, file);
	if(*flag & 0x0f == 0x01)
	{
		fprintf(stderr, "WARNING: bisek(..., eps = %.10e, Nmax = %d, ...) hit Nmax.\n", eps, Nmax);
	}
	if(*flag & 0x0f == 0x04)
	{
		fprintf(stderr, "WARNING: bisek(..., eps = %.10e, Nmax = %d, ...) conditions failed.\n", eps, Nmax);
	}
	if(!(*flag & 0xff00))
	{
		fprintf(stderr, "WARNING: bisek flag is invalid.\n");
	}
	fprintf(stderr, "bisek(<f>, %.10e, %.10e, %.10e, %d, &(%x), <FILE>) -> %.10e\n", a, b, eps, Nmax, *flag, result);

	error = x0 - result;
	fprintf(stderr, "x0 = %.10e, result = %.10e, error = %.10e\n", x0, result, error);
	// FIXME:
	// Shold we allow negative errors? It is mathematically incorrect but
	// more interesting.
#ifndef ALLOW_NEGATGIVE_ERRORS
	if(error < 0)
	{
		error *= -1;
	}
#endif
	
	fprintf(file, "%d,%.10e\n", Nmax, error);
	return result;
}


int newton_one(double * x
		, double (*f)(double)
		, double (*df)(double)
		, double eps)
{
	double y, yd;
	y = f(*x);
	yd = df(*x);
#ifdef DEBUG
	fprintf(stderr, "f(%.4e) -> %.4e; df(%.4e) -> %.4e\n", *x, y, *x, yd);
#endif
	if(abs(y) < eps)
	{
		return 0xe000;
	}
	if(abs(yd) < eps)
	{
		return 0xe002;
	}

	*x -= y / yd;
#ifdef DEBUG
	fprintf(stderr, "set new x: %.4e\n", *x);
#endif
	return 0;
}


double 
#ifdef NOCRAPMODE
	newton
#else
	__inline_newton
#endif
	(double x0
		, double (*f)(double)
		, double (*df)(double)
		, double eps
		, int Nmax
		, int * flag
		, FILE * file)
{
	int i;
	int status = 0;
	double x = x0;

	for(i = 0; i < Nmax && !status; i++)
	{
		status = newton_one(&x, f, df, eps);
	}

	if(status)
	{
		*flag = status;
	}
	else
	{
		*flag = 0xe001;
	}
	return x;

}

double 
#ifdef NOCRAPMODE
	write_error_newton
#else
	newton
#endif
		(double x0
		, double (*f)(double)
		, double (*df)(double)
		, double eps
		, int Nmax
		, int * flag
		, FILE * file
#ifdef NOCRAPMODE
		, double x1
#endif
		)
{
#ifndef NOCRAPMODE
	double x1 = -6;
#endif
	double result, error;

	result = 
#ifdef NOCRAPMODE
		newton
#else
		__inline_newton
#endif
			(x0, f, df, eps, Nmax, flag, file);

	if(*flag & 0xff)
	{
		fprintf(stderr, "WARNING: newton(..., eps = %.4e, Nmax = %d, ...) hit Nmax or min.\n", eps, Nmax);
	}
	if(!(*flag & 0xff00))
	{
		fprintf(stderr, "WARNING: newton flag is invalid.\n");
	}
	fprintf(stderr, "newton(%.4e, <f>, <df>, %.4e, %d, &(%x), <FILE>) -> %.4e\n", x0, eps, Nmax, *flag, result);

	error = x1 - result;
	fprintf(stderr, "x1 = %.4e, result = %.4e, error = %.4e\n", x1, result, error);
	// FIXME:
	// Shold we allow negative errors? It is mathematically incorrect but
	// more interesting.
#ifndef ALLOW_NEGATGIVE_ERRORS
	if(error < 0)
	{
		error *= -1;
	}
#endif
	
	fprintf(file, "%d,%.10e\n", Nmax, error);
	return result;
}

int newton_mult(int N
		, double * x0
		, void (*f)(double *, double *, int)
		, void (*df)(double *, double **, int)
		, double *fx
		, int Nmax
		, double eps)
{
	int i, status = 0;
	double * x = malloc(sizeof(double) * N);
	vector_copy(N, x, x0);

	for(i = 0; i < Nmax && !status; i++)
	{
		status = newton_mult_one(N, x, f, df, eps);
	}

	vector_copy(N, fx, x);
	free(x);
	if(status)
	{
		return status;
	}
	return 0xe001;

}

int newton_mult_one(int N
		, double * x
		, void (*f)(double *, double *, int)
		, void (*df)(double *, double **, int)
		, double eps)
{
	int i, status;
	double * b;
	int * p;
	double ** A;

	fprintf(stderr, "newton_mult_one(..., (%.4e, %4e, ...), ...)\n", x[0], x[1]);

	p = malloc(sizeof(int) * N);
	b = malloc(sizeof(double) * N);


	// f(x) = b
	f(x, b, N);

	// check if the precision is already met.
	if(modulus(N, b) < eps)
	{
		free(p);
		free(b);
		return 0xe000;
	}

	// Allocate A and calculate df(x) = A
	A = malloc(sizeof(double *) * N);
	for(i = 0; i < N; i++)
	{
		A[i] = malloc(sizeof(double) * N);
	}
	df(x, A, N);

	// We have to solve (As = -f(x) = -b), so multiply result b by -1.
	for(i = 0; i < N; i++)
	{
		b[i] *= -1;
	}

	// Try to solve
	status = Solve(N, A, b, p);
	if(status)
	{
		// df(x) is not regular.
		// Basically we are in a minimum or a saddle point.
		status = 0xe002;
		// Clean up & return
		goto exit;
	}

	for(i = 0; i < N; i++)
	{
		// FIXME:
		// Does Solve invert the swaps automatically 
		// or do I have to take care of that?
		// Crappy docs!
		x[i] += b[i];
	}


exit:
	// Clean up & go.
	for(i = 0; i < N; i++)
	{
		free(A[i]);
	}
	free(A);
	free(b);
	free(p);
	return status;
}

int vector_copy(int N
		, double * dst
		, double * src)
{
	int i;
	for(i = 0; i < N; i++)
	{
		dst[i] = src[i];
	}
	return 0;
}

double modulus(int N, double * x)
{
	int i;
	double result = 0;
	for(i = 0; i < N; i++)
	{
		result += x[i]*x[i];
	}
	return sqrt(result);

}

void g(double * x, double * y, int N)
{
	// Well idk. Should not happen.
	// At least prevent SegFault.
	if(N < 2)
	{
		return;
	}
	y[0] = (x[0] + 3)*(x[1]*x[1]*x[1] - 7) + 18;
	y[1] = sin(x[1]*exp(x[0]) -1);
}

void dg(double * x, double ** y, int N)
{
	if(N < 2)
	{
		return;
	}

	y[0][0] = x[1]*x[1]*x[1] - 7;
	y[0][1] = 3*(x[0] + 3)*x[1]*x[1];
	y[1][0] = cos(x[1]*exp(x[0]) - 1)*exp(x[0])*x[1];
	y[1][1] = cos(x[1]*exp(x[0]) - 1)*exp(x[0]);
}