/*
 * eqsolvlib.js - Multidimensional Equation Solver library
 * Copyright (C) 2009  Clifford Wolf <clifford@clifford.at>
 *
 * 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 2 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.
 *
 * See eqsolver.html for usage examples and documentation
 *
 */

function eqsolver(code, print, dbg, initData, initFuncs, initMacros)
{
	function debug(text)
	{
		if (dbg) {
			text = text.replace(/\r?\n/g, "\r\n");
			print(text);
		}
	}

	function error(text)
	{
		print("Error: " + text);
		throw Error(text);
	}

	function fmt(text)
	{
		var args = arguments;
		function replace_arg(_dummy, i) { return i > 0 ? args[i] : "{"; };
		return text.replace(/\{([0-9]+)\}/g, replace_arg);
	}

	function fmtnum(x, len)
	{
		var t;
		var a = Math.abs(x);
		if (a < 1000000 && a > 1000) {
			t = x.toFixed(0);
		} else if (a > 0.0001 || a == 0) {
			t = x.toFixed(5);
		} else {
			t = x.toPrecision(4);
		}
		while (t.length < len) {
			t = " " + t;
		}
		return t;
	}

	function fmtstr(t, len)
	{
		while (t.length < len) {
			t += " ";
		}
		return t;
	}

	function Matrix(d)
	{
		this.dim_y = 0;
		this.dim_x = 0;
		this.dat = new Array();
		this.set = function(y, x, v) {
			while (y >= this.dim_y) {
				this.dat[this.dim_y] = new Array();
				this.dim_y++;
			}
			if (x >= this.dim_x)
				this.dim_x = x+1;
			this.dat[y][x] = v;
		}
		this.get = function(y, x) {
			return this.dat[y][x] ? this.dat[y][x] : 0;
		}
		this.clone = function() {
			var ret = new Matrix();
			for (var y=0; y<this.dim_y; y++)
			for (var x=0; x<this.dim_x; x++)
				ret.set(y, x, this.get(y, x));
			return ret;
		}
		this.add = function(a) {
			if (this.dim_x != a.dim_x || this.dim_y != a.dim_y) {
				error(fmt("Can not add matrizes with different sizes: {1}x{2} != {3}x{4}",
						this.dim_x, this.dim_y, a.dim_x, a.dim_y));
			}
			var ret = new Matrix();
			for (var y=0; y<this.dim_y; y++)
			for (var x=0; x<this.dim_x; x++)
				ret.set(y, x, this.get(y, x) + a.get(y, x));
			return ret;
		}
		this.smul = function(a) {
			var ret = new Matrix();
			for (var y=0; y<this.dim_y; y++)
			for (var x=0; x<this.dim_x; x++)
				ret.set(y, x, this.get(y, x) * a);
			return ret;
		}
		this.mul = function(a) {
			if (this.dim_x != a.dim_y)
				error(fmt("Can not multiply with incompatible sizes: {1}x{2} != {3}x{4}",
						this.dim_x, this.dim_y, a.dim_x, a.dim_y));
			var ret = new Matrix();
			for (var x=0; x<a.dim_x; x++)
			for (var y=0; y<this.dim_y; y++) {
				var sum = 0;
				for (var z=0; z<a.dim_y; z++)
					sum += this.get(y, z) * a.get(z, x);
				ret.set(y, x, sum);
			}
			return ret;
		}
		this.transpose = function() {
			var ret = new Matrix();
			for (var y=0; y<this.dim_y; y++)
			for (var x=0; x<this.dim_x; x++)
				ret.set(x, y, this.get(y, x));
			return ret;
		}
		this.gaussJordan = function() {
			var y = 0;
			var ret = this.clone();
			for (var x = 0; x < ret.dim_x; x++)
			{
				var pivot_y = y;
				for (var y1 = y+1; y1 < ret.dim_y; y1++)
					if (Math.abs(ret.get(pivot_y, x)) < Math.abs(ret.get(y1, x)))
						pivot_y = y1;
				if (Math.abs(ret.get(pivot_y, x)) == 0) {
					print(fmt("WARNING: Singularity in Gauss-Jordan elimination in column {1}!\n", x+1));
				} else {
					for (var x1 = x; x1 < ret.dim_x; x1++) {
						var temp = ret.get(pivot_y, x1);
						ret.set(pivot_y, x1, ret.get(y, x1));
						ret.set(y, x1, temp);
					}
					var div = ret.get(y, x);
					for (var x1 = x; x1 < ret.dim_x; x1++)
						ret.set(y, x1, ret.get(y, x1) / div);
					for (var y1 = 0; y1 < ret.dim_y; y1++) {
						if (y1 == y)
							continue;
						for (var x1 = ret.dim_x-1; x1 >= x; x1--)
							ret.set(y1, x1, ret.get(y1, x1) - ret.get(y1, x) * ret.get(y, x1));
					}
					if (++y >= ret.dim_y)
						break;
				}
			}
			return ret;
		}
		this.inv = function() {
			if (this.dim_x != this.dim_y)
				error(fmt("Inversion is only posible on a quadratic matrix!\n"));
			var tmp = this.clone();
			for (var y=0; y<this.dim_y; y++)
			for (var x=0; x<this.dim_x; x++)
				tmp.set(y, x+this.dim_x, y == x);
			tmp = tmp.gaussJordan();
			var ret = new Matrix();
			for (var y=0; y<this.dim_y; y++)
			for (var x=0; x<this.dim_x; x++)
				ret.set(y, x, tmp.get(y, x+this.dim_x));
			return ret;
		}
		this.dump = function() {
			var ret = "";
			for (var y=0; y<this.dim_y; y++) {
				if (this.dim_y == 1)
					ret += "[ ";
				else if (y == 0)
					ret += "/ ";
				else if (y == this.dim_y-1)
					ret += "\\ ";
				else
					ret += "| ";
				for (var x=0; x<this.dim_x; x++) {
					var v = fmtnum(this.get(y, x), 10);
					ret += " " + v;
				}
				if (this.dim_y == 1)
					ret += " ]\n";
				else if (y == 0)
					ret += " \\\n";
				else if (y == this.dim_y-1)
					ret += " /\n";
				else
					ret += " |\n";
			}
			return ret;
		}
		if (typeof(d) != 'undefined') {
			for (var y=0; y<d.length; y++)
			for (var x=0; x<d[y].length; x++)
				this.set(y, x, d[y][x]);
		}
	}

	function calcJacobiMatrix()
	{
		var D = new Matrix();
		var x = 0;
		for (var id in data) {
			var y = 0;
			for (var i in eqlist) {
				var orig_data = data[id];
				// 3.4527e-04 = sqrt() of a single prec float machine epsilon (2^-23)
				var delta = Math.abs(orig_data) < 1e-10 ? 1e-12 : Math.abs(orig_data * 3.4527e-04);
				data[id] = orig_data - delta;
				var v1 = eqlist[i]();
				data[id] = orig_data + delta;
				var v2 = eqlist[i]();
				var diff = (v2 - v1) / (2*delta);
				if (!isFinite(diff) || isNaN(diff))
					diff = 0;
				data[id] = orig_data;
				D.set(y, x, diff * eqwght[i]);
				y++;
			}
			x++;
		}
		return D;
	}

	function calcResVect()
	{
		var r = new Matrix();
		var y = 0;
		for (var i in eqlist)
			r.set(y++, 0, eqlist[i]() * eqwght[i]);
		return r;
	}

	function calcError()
	{
		var err = 0;
		for (var i in eqlist) {
			var x = eqlist[i]() * eqwght[i];
			err += x * x;
		}
		return err;
	}

	var data = new Object();
	var userfunc = new Object();
	var eqlist = new Array();
	var eqwght = new Array();
	var macros = new Object();
	var ranges = new Object();
	var eqtxt = new Array();

	var config_scalehack = 0;
	var config_tryhard = 0;
	var config_showindep = 0;
	var config_weight = 1;

	for (id in initFuncs)
		userfunc[id] = initFuncs[id];

	for (id in initMacros)
		macros[id] = initMacros[id];

	while (code.match(/([^\n]*)\n/))
	{
		var line = RegExp.$1;
		code = code.replace(/[^\n]*\n/, "");
		line = line.replace(/[ \t]/g, "");
		line = line.replace(/#.*/, "");

		if (line == "")
			continue;

		if (line == "$scalehack") {
			config_scalehack = 1;
			continue;
		}

		if (line == "$tryhard") {
			config_tryhard = 1;
			continue;
		}

		if (line == "$showindep") {
			config_showindep = 1;
			continue;
		}

		if (line.match(/^\$weight:(.*)$/)) {
			with (Math) {
				config_weight = +eval(RegExp.$1);
			}
			continue;
		}

		if (line.match(/^\$range:(.*):(.*):(.*)$/)) {
			ranges[RegExp.$1] = [ RegExp.$2, RegExp.$3 ];
			continue;
		}

		if (line.match(/^\$function:([a-zA-Z_][a-zA-Z0-9_]*)(.*)$/)) {
			var fname = RegExp.$1, fargs = RegExp.$2;
			var body = "";
			while (code.match(/[ \t]*([^\n]*)\n/)) {
				line = RegExp.$1;
				code = code.replace(/[^\n]*\n/, "");
				if (line.match(/^\$endfunction *$/))
					break;
				body += line + " ";
			}
			userfunc[fname] = [ fargs, body ];
			continue;
		}

		if (line.match(/^([a-zA-Z_][a-zA-Z0-9_]*):=([a-zA-Z0-9_+\-*\/\(\),.]+)$/)) {
			var from = RegExp.$1, to = RegExp.$2;
			if (from in initMacros) {
				debug(fmt("Ignoring macro '{1}', using version from initMacros.\n", from));
				continue;
			}
			while (from in macros && macros[from].match(/^[a-zA-Z_][a-zA-Z0-9_]*$/)) {
				debug(fmt("Conflicting macro '{1}', next in chain: '{2}'\n", from, macros[from]));
				from = macros[from];
			}
			if (from in macros) {
				eqtxt.push([ from, to, "=", config_weight ]);
				debug(fmt("Conflicting macro '{1}', adding as equation: {1} = {2}\n", from, to));
			} else {
				macros[from] = to;
				debug(fmt("New macro '{1}': {2}\n", from, to));
			}
			continue;
		}

		if (line.match(/^([a-zA-Z_][a-zA-Z0-9_]*)\?=(.*)$/)) {
			with (Math) {
				data[RegExp.$1] = +eval(RegExp.$2);
			}
			debug(fmt("Initial value of '{1}': {2}\n", RegExp.$1, +RegExp.$2));
			continue;
		}

		if (line.match(/^([a-zA-Z0-9_+\-*\/(),.]+)([<>=])([a-zA-Z0-9_+\-*\/(),.]+)$/)) {
			eqtxt.push([ RegExp.$1, RegExp.$3, RegExp.$2, config_weight ]);
			if (RegExp.$2 == "=")
				debug(fmt("New equation with weight {3}: {1} = {2}\n", RegExp.$1, RegExp.$3, config_weight.toPrecision(3)));
			else
				debug(fmt("New inequation with weight {4}: {1} {2} {3}\n", RegExp.$1, RegExp.$2, RegExp.$3, config_weight.toPrecision(3)));
			continue;
		}

		error(fmt("Syntax Error: {1}\n", line));
	}

	function replace_macro(token) {
		if (token in macros) {
			var reptext = macros[token];
			macros[token] = "_RECURSION_ON_MACRO_" + token + "_";
			var ret = "(" + reptext.replace(/[a-zA-Z0-9_]+/g, replace_macro) + ")";
			macros[token] = reptext;
			return ret;
		}
		return token;
	}

	function process_token(token) {
		if (token.match(/^([0-9\.]+)(Y|Z|E|P|T|G|M|k|m|n|p|f|a|z|y)([0-9]+)/)) {
			var r1 = RegExp.$1, r2 = RegExp.$2, r3 = RegExp.$3;
			if (r2 == "Y") return (+r1 + +("0." + r3)) * pow(1000,8);
			if (r2 == "Z") return (+r1 + +("0." + r3)) * pow(1000,7);
			if (r2 == "E") return (+r1 + +("0." + r3)) * pow(1000,6);
			if (r2 == "P") return (+r1 + +("0." + r3)) * pow(1000,5);
			if (r2 == "T") return (+r1 + +("0." + r3)) * pow(1000,4);
			if (r2 == "G") return (+r1 + +("0." + r3)) * pow(1000,3);
			if (r2 == "M") return (+r1 + +("0." + r3)) * pow(1000,2);
			if (r2 == "k") return (+r1 + +("0." + r3)) * pow(1000,1);
			if (r2 == "m") return (+r1 + +("0." + r3)) * pow(1000,-1);
			if (r2 == "u") return (+r1 + +("0." + r3)) * pow(1000,-2);
			if (r2 == "n") return (+r1 + +("0." + r3)) * pow(1000,-3);
			if (r2 == "p") return (+r1 + +("0." + r3)) * pow(1000,-4);
			if (r2 == "f") return (+r1 + +("0." + r3)) * pow(1000,-5);
			if (r2 == "a") return (+r1 + +("0." + r3)) * pow(1000,-6);
			if (r2 == "z") return (+r1 + +("0." + r3)) * pow(1000,-7);
			if (r2 == "y") return (+r1 + +("0." + r3)) * pow(1000,-8);
			error(fmt("Can't parse number with si-prefix: {1}\n", token));
		}
		if (token.match(/^[0-9]/)) {
			return token;
		}
		if (token in userfunc) {
			return "userfunc." + token;
		}
		if (token in Math) {
			return "Math." + token;
		}
		if (!(token in data)) {
			data[token] = token in initData ? initData[token] : 1;
		}
		return "data." + token;
	}

	var firstUserFunc = 1;
	for (var fname in userfunc) {
		if (firstUserFunc)
			debug("\n");
		firstUserFunc = 0;
		var body = userfunc[fname][1].replace(/[a-zA-Z0-9_]+/g, replace_macro);
		var cmd = fmt("function _func{1}{ with (Math) { with (data) { {2} } } }", userfunc[fname][0], body);
		debug(fmt("New user function '{1}': {2}\n", fname, cmd));
		eval(cmd);
		userfunc[fname] = _func;
	}

	debug("\n");
	for (i in eqtxt)
	{
		var expr1 = eqtxt[i][0];
		var expr2 = eqtxt[i][1];
		var op = eqtxt[i][2];
		var wght = eqtxt[i][3];

		expr1 = expr1.replace(/[a-zA-Z0-9_]+/g, replace_macro);
		expr2 = expr2.replace(/[a-zA-Z0-9_]+/g, replace_macro);

		expr1 = expr1.replace(/[a-zA-Z0-9_.]+/g, process_token);
		expr2 = expr2.replace(/[a-zA-Z0-9_.]+/g, process_token);

		var cmd;
		if (op == "=") {
			cmd = fmt("function _func() { return ({1}) - ({2}); }", expr1, expr2);
		}
		else if (op == "<") {
			cmd = fmt("function _func() { return Math.max(({1}) - ({2}), 0); }", expr1, expr2);
		}
		else if (op == ">") {
			cmd = fmt("function _func() { return Math.min(({1}) - ({2}), 0); }", expr1, expr2);
		}
		debug(fmt("Residual function: {1}\n", cmd));
		eval(cmd);
		eqlist.push(_func);
		eqwght.push(wght);
	}

	var it;
	var lastErr = -1;
	var thisErr = calcError();
	var bestErr = thisErr;
	var bestData = data;
	var stallCounter = 0;
	for (it=0; it < 100 && thisErr != lastErr && thisErr != 0; it++)
	{
		debug(fmt("\n**** Gauss-Newton-Fitting (iteration {1}) ****\n\n", it+1));

		var D = calcJacobiMatrix();
		debug("Jacobi Matrix:\n");
		debug(D.dump() + "\n");

		var r = calcResVect();
		debug("Residuals Vector:\n");
		debug(r.dump() + "\n");

		if (it == 0 && D.dim_y < D.dim_x)
			print("WARNING: Equation system seams to be underdefined.\n");

		var Dt = D.transpose();
		var aoff = Dt.mul(D).inv().mul(Dt).mul(r);

		debug("Offset Vector:\n");
		debug(aoff.dump() + "\n");

		var y = 0;
		var old_data = data;
		data = new Object();
		for (var id in old_data) {
			data[id] = old_data[id] - aoff.get(y++, 0);
			if (id in ranges) {
				if (ranges[id][0] != "" && data[id] < +ranges[id][0])
					data[id] = +ranges[id][0];
				if (ranges[id][1] != "" && data[id] > +ranges[id][1])
					data[id] = +ranges[id][1];
			}
			debug(fmt("{1} = {2}\n", id, fmtnum(data[id], 10)));
		}

		lastErr = thisErr;
		thisErr = calcError();

		debug("\nLast square error:     " + fmtnum(lastErr, 10) + "\n");
		debug("Old best square error: " + fmtnum(bestErr, 10) + "\n");
		debug("Current square error:  " + fmtnum(thisErr, 10) + "\n");

		if (config_scalehack && lastErr > 0 && thisErr > 0 && thisErr*2 > lastErr) {
			debug("\nInsufficient improvement in square error. Trying scale hack..\n");
			var scaleValues = [ -100, -10, -1, -0.1, 0, 0.1, 0.15, 0.22, 0.33, 0.47, 0.68,
			                    1.0, 1.5, 2.2, 3.3, 4.7, 6.8, 10, 15, 22, 33, 47, 68, 100 ];
			var bestErr = thisErr;
			var bestScaleIdx = -1;
			data = new Object();
			for (i in scaleValues) {
				var y = 0;
				if (config_tryhard && scaleValues[i] == 0)
					continue;
				for (var id in old_data)
					data[id] = old_data[id] - aoff.get(y++, 0)*scaleValues[i];
				thisErr = calcError();
				if (thisErr < bestErr) {
					bestErr = thisErr;
					bestScaleIdx = i;
				}
				debug(fmt("scaling by {1}: {2}\n", fmtnum(scaleValues[i]), fmtnum(thisErr, 10)));
			}
			var finalScale = 1;
			if (bestScaleIdx >= 0) {
				finalScale = scaleValues[bestScaleIdx];
				debug(fmt("Using scaling factor {1}.\n\n", scaleValues[bestScaleIdx].toFixed(1)));
				debug("\nNew square error: " + fmtnum(thisErr, 0) + "\n");
			} else {
				debug("\nNo improvement from scaling hack.\n");
			}
			var y = 0;
			for (var id in old_data) {
				data[id] = old_data[id] - aoff.get(y++, 0)*finalScale;
				debug(fmt("{1} = {2}\n", fmtstr(id, 10), fmtnum(data[id], 10)));
			}
			thisErr = calcError();
		}

		if (!config_tryhard && lastErr > 0 && thisErr > 0 && thisErr*1.0001 > bestErr) {
			if (++stallCounter < 5) {
				debug(fmt("\nImprovement in square error is minimal ({1}/5).\n", stallCounter));
			} else {
				debug("\nImprovement in square error stays minimal. We are done.\n");
				break;
			}
		} else
			stallCounter = 0;

		if (bestErr > thisErr) {
			bestData = data;
			bestErr = thisErr;
		}
	}

	data = bestData;

	var ret = new Object();
	ret.data = data;
	ret.func = userfunc;
	ret.alldata = new Object();
	for (id in bestData)
		ret.alldata[id] = bestData[id];
	for (id in macros) {
		var expr = macros[id];
		expr = expr.replace(/[a-zA-Z0-9_]+/g, replace_macro);
		expr = expr.replace(/[a-zA-Z0-9_.]+/g, process_token);
		ret.alldata[id] = eval(expr);
	}

	debug("\n**** Results ****\n\n");

	var results = new Array();
	for (var id in data) {
		results.push(fmt("{1} = {2}\n", fmtstr(id, 10), fmtnum(data[id], 10)));
	}
	results.sort();
	for (var i in results) {
		print(results[i]);
	}
	print("\n");

	if (config_showindep) {
		var results = new Array();
		for (var id in ret.alldata) {
			results.push(fmt("{1} = {2}\n", fmtstr(id, 10), fmtnum(ret.alldata[id], 10)));
		}
		results.sort();
		for (var i in results) {
			print(results[i]);
		}
		print("\n");
	}

	print(fmt("Number of iterations: {1}\n", it));
	print(fmt("Remaining error: {1}\n", fmtnum(Math.sqrt(bestErr), 0)));

	return ret;
}