/*
Copyright (c) 2015 to 2016 by Cornell University and The Regents Of
The University Of California. All Rights Reserved.

Permission to use this Procedural Yarn Fitting and Generation Tool (the "Work")
and its associated copyrights solely for educational, research and non-profit
purposes, without fee is hereby granted, provided that the user agrees as
follows:

Those desiring to incorporate the Work into commercial products or use Work and
its associated copyrights for commercial purposes should contact the Center for
Technology Licensing at Cornell University at

395 Pine Tree Road, Suite 310, Ithaca, NY 14850;
email: ctl-connect@cornell.edu;
Tel: 607-254-4698;
FAX: 607-254-5454

for a commercial license.

IN NO EVENT SHALL CORNELL UNIVERSITY ("CORNELL") OR THE UNIVERSITY OF
CALIFORNIA ("UC") BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL,
INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF
THE USE OF THE WORK AND ITS ASSOCIATED COPYRIGHTS, EVEN IF CORNELL OR UC MAY
HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

THE WORK PROVIDED HEREIN IS ON AN "AS IS" BASIS, AND NEITHER CORNELL NOR UC HAS
ANY OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
MODIFICATIONS. CORNELL AND UC MAKE NO REPRESENTATIONS AND EXTEND NO WARRANTIES
OF ANY KIND, EITHER IMPLIED OR EXPRESS, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, OR
THAT THE USE OF WORK AND ITS ASSOCIATED COPYRIGHTS WILL NOT INFRINGE ANY PATENT,
TRADEMARK OR OTHER RIGHTS.
*/

#pragma once
#include "Util.h" 
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#ifdef MACOSX
#include <malloc/malloc.h>
#else
#include <malloc.h>
#endif

#define MAX_IT      20        /* maximum number of iterations */
#define ALPHA       1.0       /* reflection coefficient */
#define BETA        0.5       /* contraction coefficient */
#define GAMMA       2.0       /* expansion coefficient */
#define NO_VERBOSE

struct SimplexMethod {
	int num_init_guesses;

	double rho_min, rho_max, s_i;
	std::vector<double> *x, *y;

	double init_r, init_theta;
	vec2f init_r_range, init_theta_range;

	SimplexMethod(int num_init_guesses) {
		this->num_init_guesses = num_init_guesses;
		this->init_r_range = vec2f(0.f, 1.f);
		this->init_theta_range = vec2f(0.f, 2.f * pi);
	}

	double migration(double theta, double init_r, double init_theta) 
	{
		double R = rho_min * init_r + (rho_max - rho_min) * init_r * 0.5f * 
			(cosf(s_i * theta + init_theta) + 1);
		return R;
	}

	double objfunc(double abs[]) {
		double _init_r = abs[0];
		double _init_theta = abs[1];
		double _norm = 0;
		for (int i = 0; i < x->size(); i++) {
			_norm += powf((*y)[i] - migration((*x)[i], _init_r, _init_theta), 2.f);
		}
		_norm /= x->size();
		return _norm;
	}

	void feed(std::vector<double> &varX, std::vector<double> &varY, 
		double rho_min, double rho_max, double si) {
		this->x = &varX;
		this->y = &varY;
		this->rho_max = rho_max;
		this->rho_min = rho_min;
		this->s_i = si;

		this->init_r_range = vec2f(0.f, 1.f / rho_max);
	}

	void sample_init_guess(double start[], int id) {
		float init_r_s = init_r_range.x + (init_r_range.y - init_r_range.x) / num_init_guesses * id;
		float init_t_s = init_theta_range.x + (init_theta_range.y - init_theta_range.x) / num_init_guesses * id;
		start[0] = init_r_s;
		start[1] = init_t_s;
	}

	double fit() {
		double min_val = std::numeric_limits<float>::max();
		double start[] = {0.f, 0.f};
		const int dim = 2;
		const double tolerance = 1.0e-7, scale = 1.0;
		for (int i = 0; i < num_init_guesses; i++) 
		{
			sample_init_guess(start, i);
			double val = simplex(start, dim, tolerance, scale/*, NULL*/);
			if (val < min_val) {
				min_val = val;
				this->init_r = start[0];
				this->init_theta = start[1];
			}
		}
		return min_val;
	}

	void constrain(double x[], int n) {
		x[0] = clamp(x[0], init_r_range.x, init_r_range.y);
		x[1] = x[1] - std::floor(x[1]/(2*pi))*2*pi;
	}

	double simplex(double start[],int n, double EPSILON, double scale/*, void (*constrain)(double[],int n)*/)
	{

		int vs;         /* vertex with smallest value */
		int vh;         /* vertex with next smallest value */
		int vg;         /* vertex with largest value */

		int i,j,m,row;
		int k;   	      /* track the number of function evaluations */
		int itr;	      /* track the number of iterations */

		double **v;     /* holds vertices of simplex */
		double pn,qn;   /* values used to create initial simplex */
		double *f;      /* value of function at each vertex */
		double fr;      /* value of function at reflection point */
		double fe;      /* value of function at expansion point */
		double fc;      /* value of function at contraction point */
		double *vr;     /* reflection - coordinates */
		double *ve;     /* expansion - coordinates */
		double *vc;     /* contraction - coordinates */
		double *vm;     /* centroid - coordinates */
		double min;

		double fsum,favg,s,cent;

		/* dynamically allocate arrays */

		/* allocate the rows of the arrays */
		v =  (double **) malloc ((n+1) * sizeof(double *));
		f =  (double *) malloc ((n+1) * sizeof(double));
		vr = (double *) malloc (n * sizeof(double));
		ve = (double *) malloc (n * sizeof(double));  
		vc = (double *) malloc (n * sizeof(double));  
		vm = (double *) malloc (n * sizeof(double));  

		/* allocate the columns of the arrays */
		for (i=0;i<=n;i++) {
			v[i] = (double *) malloc (n * sizeof(double));
		}

		/* create the initial simplex */
		/* assume one of the vertices is 0,0 */

		pn = scale*(sqrtf(n+1)-1+n)/(n*sqrtf(2));
		qn = scale*(sqrtf(n+1)-1)/(n*sqrtf(2));

		for (i=0;i<n;i++) {
			v[0][i] = start[i];
		}

		for (i=1;i<=n;i++) {
			for (j=0;j<n;j++) {
				if (i-1 == j) {
					v[i][j] = pn + start[j];
				}
				else {
					v[i][j] = qn + start[j];
				}
			}
		}

		//if (constrain != NULL) {
			constrain(v[j],n);
		//} 
		/* find the initial function values */
		for (j=0;j<=n;j++) {
			f[j] = objfunc(v[j]);
		}

		k = n+1;
#ifndef NO_VERBOSE
		/* print out the initial values */
		printf("Initial Values\n");
		for (j=0;j<=n;j++) {
			for (i=0;i<n;i++) {
				printf("%f %f\n",v[j][i],f[j]);
			}
		}
#endif


		/* begin the main loop of the minimization */
		for (itr=1;itr<=MAX_IT;itr++) {     
			/* find the index of the largest value */
			vg=0;
			for (j=0;j<=n;j++) {
				if (f[j] > f[vg]) {
					vg = j;
				}
			}

			/* find the index of the smallest value */
			vs=0;
			for (j=0;j<=n;j++) {
				if (f[j] < f[vs]) {
					vs = j;
				}
			}

			/* find the index of the second largest value */
			vh=vs;
			for (j=0;j<=n;j++) {
				if (f[j] > f[vh] && f[j] < f[vg]) {
					vh = j;
				}
			}

			/* calculate the centroid */
			for (j=0;j<=n-1;j++) {
				cent=0.0;
				for (m=0;m<=n;m++) {
					if (m!=vg) {
						cent += v[m][j];
					}
				}
				vm[j] = cent/n;
			}

			/* reflect vg to new vertex vr */
			for (j=0;j<=n-1;j++) {
				/*vr[j] = (1+ALPHA)*vm[j] - ALPHA*v[vg][j];*/
				vr[j] = vm[j]+ALPHA*(vm[j]-v[vg][j]);
			}
			//if (constrain != NULL) {
				constrain(vr,n);
			//}
			fr = objfunc(vr);
			k++;

			if (fr < f[vh] && fr >= f[vs]) {
				for (j=0;j<=n-1;j++) {
					v[vg][j] = vr[j];
				}
				f[vg] = fr;
			}

			/* investigate a step further in this direction */
			if ( fr <  f[vs]) {
				for (j=0;j<=n-1;j++) {
					/*ve[j] = GAMMA*vr[j] + (1-GAMMA)*vm[j];*/
					ve[j] = vm[j]+GAMMA*(vr[j]-vm[j]);
				}
				//if (constrain != NULL) {
					constrain(ve,n);
				//}
				fe = objfunc(ve);
				k++;

				/* by making fe < fr as opposed to fe < f[vs], 			   
				Rosenbrocks function takes 63 iterations as opposed 
				to 64 when using double variables. */

				if (fe < fr) {
					for (j=0;j<=n-1;j++) {
						v[vg][j] = ve[j];
					}
					f[vg] = fe;
				}
				else {
					for (j=0;j<=n-1;j++) {
						v[vg][j] = vr[j];
					}
					f[vg] = fr;
				}
			}

			/* check to see if a contraction is necessary */
			if (fr >= f[vh]) {
				if (fr < f[vg] && fr >= f[vh]) {
					/* perform outside contraction */
					for (j=0;j<=n-1;j++) {
						/*vc[j] = BETA*v[vg][j] + (1-BETA)*vm[j];*/
						vc[j] = vm[j]+BETA*(vr[j]-vm[j]);
					}
					//if (constrain != NULL) {
						constrain(vc,n);
					//}
					fc = objfunc(vc);
					k++;
				}
				else {
					/* perform inside contraction */
					for (j=0;j<=n-1;j++) {
						/*vc[j] = BETA*v[vg][j] + (1-BETA)*vm[j];*/
						vc[j] = vm[j]-BETA*(vm[j]-v[vg][j]);
					}
					//if (constrain != NULL) {
						constrain(vc,n);
					//}
					fc = objfunc(vc);
					k++;
				}


				if (fc < f[vg]) {
					for (j=0;j<=n-1;j++) {
						v[vg][j] = vc[j];
					}
					f[vg] = fc;
				}
				/* at this point the contraction is not successful,
				we must halve the distance from vs to all the 
				vertices of the simplex and then continue.
				10/31/97 - modified to account for ALL vertices. 
				*/
				else {
					for (row=0;row<=n;row++) {
						if (row != vs) {
							for (j=0;j<=n-1;j++) {
								v[row][j] = v[vs][j]+(v[row][j]-v[vs][j])/2.0;
							}
						}
					}
					//if (constrain != NULL) {
						constrain(v[vg],n);
					//}
					f[vg] = objfunc(v[vg]);
					k++;
					//if (constrain != NULL) {
						constrain(v[vh],n);
					///}
					f[vh] = objfunc(v[vh]);
					k++;


				}
			}
#ifndef NO_VERBOSE
			/* print out the value at each iteration */
			printf("Iteration %d\n",itr);
			for (j=0;j<=n;j++) {
				for (i=0;i<n;i++) {
					printf("%f %f\n",v[j][i],f[j]);
				}
			}
#endif

			/* test for convergence */
			fsum = 0.0;
			for (j=0;j<=n;j++) {
				fsum += f[j];
			}
			favg = fsum/(n+1);
			s = 0.0;
			for (j=0;j<=n;j++) {
				s += pow((f[j]-favg),2.0)/(n);
			}
			s = sqrt(s);
			if (s < EPSILON) break;
		}
		/* end main loop of the minimization */

		/* find the index of the smallest value */
		vs=0;
		for (j=0;j<=n;j++) {
			if (f[j] < f[vs]) {
				vs = j;
			}
		}
#ifndef NO_VERBOSE
		printf("The minimum was found at\n"); 
#endif
		for (j=0;j<n;j++) {
#ifndef NO_VERBOSE
			printf("%e\n",v[vs][j]);
#endif
			start[j] = v[vs][j];
		}
		min=objfunc(v[vs]);
		k++;
#ifndef NO_VERBOSE
		printf("%d Function Evaluations\n",k);
		printf("%d Iterations through program\n",itr);
#endif
		free(f);
		free(vr);
		free(ve);
		free(vc);
		free(vm);
		for (i=0;i<=n;i++) {
			free (v[i]);
		}
		free(v);
		return min;
	}

};