/*
 * Copyright 2010 - 2011, Haiku.
 * Distributed under the terms of the MIT License.
 *
 * Authors:
 *		Clemens Zeidler <haiku@clemens-zeidler.de>
 */


#include "ActiveSetSolver.h"

#include <algorithm>
#include <stdio.h>

#include "LayoutOptimizer.h"


// #define DEBUG_ACTIVE_SOLVER

#ifdef DEBUG_ACTIVE_SOLVER
#define TRACE(x...) printf(x)
#else
#define TRACE(x...) /* nothing */
#endif


using namespace LinearProgramming;
using namespace std;


EquationSystem::EquationSystem(int32 rows, int32 columns)
	:
	fRows(rows),
	fColumns(columns)
{
	fMatrix = allocate_matrix(fRows, fColumns);
	fB = new double[fColumns];
	// better init all values to prevent side cases where not all variables
	// needed to solve the problem, coping theses values to the results could
	// cause problems
	for (int i = 0; i < fColumns; i++)
		fB[i] = 0;
	zero_matrix(fMatrix, fRows, fColumns);

	fRowIndices = new int32[fRows];
	fColumnIndices = new int32[fColumns];
	for (int i = 0; i < fRows; i++)
		fRowIndices[i] = i;
	for (int i = 0; i < fColumns; i++)
		fColumnIndices[i] = i;
}


EquationSystem::~EquationSystem()
{
	free_matrix(fMatrix);
	delete[] fB;
	delete[] fRowIndices;
	delete[] fColumnIndices;
}


void
EquationSystem::SetRows(int32 rows)
{
	fRows = rows;
}


int32
EquationSystem::Rows()
{
	return fRows;
}


int32
EquationSystem::Columns()
{
	return fColumns;
}


bool
EquationSystem::InRange(int32 row, int32 column)
{
	if (row < 0 || row >= fRows)
		return false;
	if (column < 0 || column >= fColumns)
		return false;
	return true;
}


double&
EquationSystem::A(int32 row, int32 column)
{
	return fMatrix[fRowIndices[row]][fColumnIndices[column]];
}


double&
EquationSystem::B(int32 row)
{
	return fB[row];
}


void
EquationSystem::Results(double* results, int32 size)
{
	for (int i = 0; i < size; i++)
		results[i] = 0;
	for (int i = 0; i < fColumns; i++) {
		int32 index = fColumnIndices[i];
		if (index < fRows)
			results[index] = fB[i];
	}
}


void
EquationSystem::SwapColumn(int32 i, int32 j)
{
	swap(fColumnIndices[i], fColumnIndices[j]);
}


void
EquationSystem::SwapRow(int32 i, int32 j)
{
	swap(fRowIndices[i], fRowIndices[j]);
	swap(fB[i], fB[j]);
}


bool
EquationSystem::GaussianElimination()
{
	// basic solve
	for (int i = 0; i < fRows; i++) {
		// find none zero element
		int swapRow = -1;
		for (int r = i; r < fRows; r++) {
			double& value = fMatrix[fRowIndices[r]][fColumnIndices[i]];
			if (fuzzy_equals(value, 0))
				continue;
			swapRow = r;
			break;
		}
		if (swapRow == -1) {
			int swapColumn = -1;
			for (int c = i + 1; c < fColumns; c++) {
				double& value = fMatrix[fRowIndices[i]][fColumnIndices[c]];
				if (fuzzy_equals(value, 0))
					continue;
				swapRow = i;
				swapColumn = c;
				break;
			}
			if (swapColumn == -1) {
				TRACE("can't solve column %i\n", i);
				return false;
			}
			SwapColumn(i, swapColumn);
		}
		if (i != swapRow)
			SwapRow(i, swapRow);

		// normalize
		_EliminateColumn(i, i + 1, fRows - 1);
	}
	return true;
}


bool
EquationSystem::GaussJordan()
{
	if (!GaussianElimination())
		return false;

	for (int32 i = fRows - 1; i >= 0; i--)
		_EliminateColumn(i, 0, i - 1);

	return true;
}


void
EquationSystem::GaussJordan(int32 i)
{
	_EliminateColumn(i, 0, fRows - 1);
}


void
EquationSystem::RemoveLinearlyDependentRows()
{
	double** temp = allocate_matrix(fRows, fColumns);
	bool independentRows[fRows];

	// copy to temp
	copy_matrix(fMatrix, temp, fRows, fColumns);
	int nIndependent = compute_dependencies(temp, fRows, fColumns,
		independentRows);
	if (nIndependent == fRows)
		return;

	// remove the rows
	for (int i = 0; i < fRows; i++) {
		if (!independentRows[i]) {
			int lastDepRow = -1;
			for (int d = fRows - 1; d > i; d--) {
				if (independentRows[d]) {
					lastDepRow = d;
					break;
				}
			}
			if (lastDepRow < 0)
				break;
			SwapRow(i, lastDepRow);
			fRows--;
		}
	}
	fRows = nIndependent;

	free_matrix(temp);
}


void
EquationSystem::RemoveUnusedVariables()
{
	for (int c = 0; c < fColumns; c++) {
		bool used = false;
		for (int r = 0; r < fRows; r++) {
			if (!fuzzy_equals(fMatrix[r][fColumnIndices[c]], 0)) {
				used = true;
				break;
			}
		}
		if (used)
			continue;

		//MoveColumnRight(c, fColumns - 1);
		SwapColumn(c, fColumns - 1);
		fColumns--;
		c--;
	}
}


void
EquationSystem::MoveColumnRight(int32 i, int32 target)
{
	int32 index = fColumnIndices[i];
	for (int c = i; c < target; c++)
		fColumnIndices[c] = fColumnIndices[c + 1];
	fColumnIndices[target] = index;
}


void
EquationSystem::Print()
{
	for (int m = 0; m < fRows; m++) {
		for (int n = 0; n < fColumns; n++)
			printf("%.1f ", fMatrix[fRowIndices[m]][fColumnIndices[n]]);
		printf("= %.1f\n", fB[m]);
	}
}


void
EquationSystem::_EliminateColumn(int32 column, int32 startRow, int32 endRow)
{
	double value = fMatrix[fRowIndices[column]][fColumnIndices[column]];
	if (value != 1.) {
		for (int j = column; j < fColumns; j++)
			fMatrix[fRowIndices[column]][fColumnIndices[j]] /= value;
		fB[column] /= value;
	}

	for (int r = startRow; r < endRow + 1; r++) {
		if (r == column)
			continue;
		double q = -fMatrix[fRowIndices[r]][fColumnIndices[column]];
		// don't need to do anything, since matrix is typically sparse this
		// should save some work
		if (fuzzy_equals(q, 0))
			continue;
		for (int c = column; c < fColumns; c++)
			fMatrix[fRowIndices[r]][fColumnIndices[c]]
				+= fMatrix[fRowIndices[column]][fColumnIndices[c]] * q;

		fB[r] += fB[column] * q;
	}
}


namespace {


Constraint*
AddMinConstraint(LinearSpec* spec, Variable* var)
{
	return spec->AddConstraint(1, var, kEQ, 0, 5, 5);
}


Constraint*
AddMaxConstraint(LinearSpec* spec, Variable* var)
{
	static const double kHugeValue = 32000;
	return spec->AddConstraint(1, var, kEQ, kHugeValue, 5, 5);
}

};


ActiveSetSolver::ActiveSetSolver(LinearSpec* linearSpec)
	:
	SolverInterface(linearSpec),

	fVariables(linearSpec->UsedVariables()),
	fConstraints(linearSpec->Constraints())
{

}


ActiveSetSolver::~ActiveSetSolver()
{

}


/* Using algorithm found in:
Solving Inequalities and Proving Farkas's Lemma Made Easy
David Avis and Bohdan Kaluzny
The American Mathematical Monthly
Vol. 111, No. 2 (Feb., 2004), pp. 152-157 */
static bool
solve(EquationSystem& system)
{
	// basic solve
	if (!system.GaussJordan())
		return false;

	bool done = false;
	while (!done) {
		double smallestB = HUGE_VALF;
		int smallestBRow = -1;
		for (int row = 0; row < system.Rows(); row++) {
			if (system.B(row) > 0 || fuzzy_equals(system.B(row), 0))
				continue;

			double bValue = fabs(system.B(row));
			if (bValue < smallestB) {
				smallestB = bValue;
				smallestBRow = row;
			}
		}
		if (smallestBRow == -1) {
			done = true;
			break;
		}

		int negValueCol = -1;
		for (int col = system.Rows(); col < system.Columns(); col++) {
			double value = system.A(smallestBRow, col);
			if (value > 0 || fuzzy_equals(value, 0))
				continue;
			negValueCol = col;
			break;
		}
		if (negValueCol == -1) {
			TRACE("can't solve\n");
			return false;
		}

		system.SwapColumn(smallestBRow, negValueCol);

		// eliminate
		system.GaussJordan(smallestBRow);
	}

	return true;
}


static bool is_soft_inequality(Constraint* constraint)
{
	if (constraint->PenaltyNeg() <= 0. && constraint->PenaltyPos() <= 0.)
		return false;
	if (constraint->Op() != kEQ)
		return true;
	return false;
}


class SoftInequalityAdder {
public:
	SoftInequalityAdder(LinearSpec* linSpec, ConstraintList& allConstraints)
		:
		fLinearSpec(linSpec)
	{
		for (int32 c = 0; c < allConstraints.CountItems(); c++) {
			Constraint* constraint = allConstraints.ItemAt(c);
			if (!is_soft_inequality(constraint))
				continue;
	
			Variable* variable = fLinearSpec->AddVariable();
			variable->SetRange(0, 20000);

			Constraint* helperConst = fLinearSpec->AddConstraint(1, variable,
				kEQ, 0, constraint->PenaltyNeg(), constraint->PenaltyPos());
			fInequalitySoftConstraints.AddItem(helperConst);
			
			double coeff = -1;
			if (constraint->Op() == kGE)
				coeff = 1;
	
			Constraint* modifiedConstraint = new Constraint(constraint);
			allConstraints.AddItem(modifiedConstraint, c);
			allConstraints.RemoveItemAt(c + 1);
			fModifiedIneqConstraints.AddItem(modifiedConstraint);
			modifiedConstraint->LeftSide()->AddItem(
				new Summand(coeff, variable));
		}
		for (int32 c = 0; c < fInequalitySoftConstraints.CountItems(); c++)
			allConstraints.AddItem(fInequalitySoftConstraints.ItemAt(c));
	}

	~SoftInequalityAdder()
	{
		for (int32 c = 0; c < fModifiedIneqConstraints.CountItems(); c++)
			delete fModifiedIneqConstraints.ItemAt(c);
		for (int32 c = 0; c < fInequalitySoftConstraints.CountItems(); c++) {
			Constraint* con = fInequalitySoftConstraints.ItemAt(c);
			fLinearSpec->RemoveVariable(con->LeftSide()->ItemAt(0)->Var());
				// this also deletes the constraint
		}
	}

private:
	LinearSpec*		fLinearSpec;
	ConstraintList	fModifiedIneqConstraints;
	ConstraintList	fInequalitySoftConstraints;
};


ResultType
ActiveSetSolver::Solve()
{
	
	// make a copy of the original constraints and create soft inequality
	// constraints
	ConstraintList allConstraints(fConstraints);
	SoftInequalityAdder adder(fLinearSpec, allConstraints);

	int32 nConstraints = allConstraints.CountItems();
	int32 nVariables = fVariables.CountItems();

	if (nVariables > nConstraints) {
		TRACE("More variables then constraints! vars: %i, constraints: %i\n",
			(int)nVariables, (int)nConstraints);
		return kInfeasible;
	}

	/* First find an initial solution and then optimize it using the active set
	method. */
	EquationSystem system(nConstraints, nVariables + nConstraints);

	int32 slackIndex = nVariables;
	// setup constraint matrix and add slack variables if necessary
	int32 rowIndex = 0;
	for (int32 c = 0; c < nConstraints; c++) {
		Constraint* constraint = allConstraints.ItemAt(c);
		if (is_soft(constraint))
			continue;
		SummandList* leftSide = constraint->LeftSide();
		system.B(rowIndex) = constraint->RightSide();
		for (int32 sIndex = 0; sIndex < leftSide->CountItems(); sIndex++ ) {
			Summand* summand = leftSide->ItemAt(sIndex);
			double coefficient = summand->Coeff();
			int32 columnIndex = summand->VariableIndex();
			system.A(rowIndex, columnIndex) = coefficient;
		}
		if (constraint->Op() == kLE) {
			system.A(rowIndex, slackIndex) = 1;
			slackIndex++;
		} else if (constraint->Op() == kGE) {
			system.A(rowIndex, slackIndex) = -1;
			slackIndex++;
		}
		rowIndex++;
	}

	system.SetRows(rowIndex);

	system.RemoveLinearlyDependentRows();
	system.RemoveUnusedVariables();

	if (!solve(system))
		return kInfeasible;

	double results[nVariables + nConstraints];
	system.Results(results, nVariables + nConstraints);
	TRACE("base system solved\n");

	LayoutOptimizer optimizer(allConstraints, nVariables);
	if (!optimizer.Solve(results))
		return kInfeasible;

	// back to the variables
	for (int32 i = 0; i < nVariables; i++)
		fVariables.ItemAt(i)->SetValue(results[i]);

	for (int32 i = 0; i < nVariables; i++)
		TRACE("var %f\n", results[i]);

	return kOptimal;
}


bool
ActiveSetSolver::VariableAdded(Variable* variable)
{
	if (fVariableGEConstraints.AddItem(NULL) == false)
		return false;
	if (fVariableLEConstraints.AddItem(NULL) == false) {
		// clean up
		int32 count = fVariableGEConstraints.CountItems();
		fVariableGEConstraints.RemoveItemAt(count - 1);
		return false;	
	}
	return true;
}


bool
ActiveSetSolver::VariableRemoved(Variable* variable)
{
	fVariableGEConstraints.RemoveItemAt(variable->GlobalIndex());
	fVariableLEConstraints.RemoveItemAt(variable->GlobalIndex());
	return true;
}


bool
ActiveSetSolver::VariableRangeChanged(Variable* variable)
{
	double min = variable->Min();
	double max = variable->Max();
	int32 variableIndex = variable->GlobalIndex();

	Constraint* constraintGE = fVariableGEConstraints.ItemAt(variableIndex);
	Constraint* constraintLE = fVariableLEConstraints.ItemAt(variableIndex);
	if (constraintGE == NULL && min > -20000) {
		constraintGE = fLinearSpec->AddConstraint(1, variable, kGE, 0);
		if (constraintGE == NULL)
			return false;
		constraintGE->SetLabel("Var Min");
		fVariableGEConstraints.RemoveItemAt(variableIndex);
		fVariableGEConstraints.AddItem(constraintGE, variableIndex);
	}
	if (constraintLE == NULL && max < 20000) {
		constraintLE = fLinearSpec->AddConstraint(1, variable, kLE, 20000);
		if (constraintLE == NULL)
			return false;
		constraintLE->SetLabel("Var Max");
		fVariableLEConstraints.RemoveItemAt(variableIndex);
		fVariableLEConstraints.AddItem(constraintLE, variableIndex);
	}

	if (constraintGE)
		constraintGE->SetRightSide(min);
	if (constraintLE)
		constraintLE->SetRightSide(max);
	return true;
}


bool
ActiveSetSolver::ConstraintAdded(Constraint* constraint)
{
	return true;
}


bool
ActiveSetSolver::ConstraintRemoved(Constraint* constraint)
{
	return true;
}


bool
ActiveSetSolver::LeftSideChanged(Constraint* constraint)
{
	return true;
}


bool
ActiveSetSolver::RightSideChanged(Constraint* constraint)
{
	return true;
}


bool
ActiveSetSolver::OperatorChanged(Constraint* constraint)
{
	return true;
}


bool
ActiveSetSolver::PenaltiesChanged(Constraint* constraint)
{
	return true;
}


bool
ActiveSetSolver::SaveModel(const char* fileName)
{
	return false;
}


ResultType
ActiveSetSolver::FindMaxs(const VariableList* variables)
{
	return _FindWithConstraintsNoSoft(variables, AddMaxConstraint);
}


ResultType
ActiveSetSolver::FindMins(const VariableList* variables)
{
	return _FindWithConstraintsNoSoft(variables, AddMinConstraint);
}


void
ActiveSetSolver::GetRangeConstraints(const Variable* var,
	const Constraint** _min, const Constraint** _max) const
{
	int32 variableIndex = var->GlobalIndex();

	if (_min)
		*_min = fVariableGEConstraints.ItemAt(variableIndex);
	if (_max)
		*_max = fVariableLEConstraints.ItemAt(variableIndex);
}


void
ActiveSetSolver::_RemoveSoftConstraint(ConstraintList& list)
{
	ConstraintList allConstraints = fLinearSpec->Constraints();
	for (int i = 0; i < allConstraints.CountItems(); i++) {
		Constraint* constraint = allConstraints.ItemAt(i);
		// soft eq an ineq constraint?
		if (constraint->PenaltyNeg() <= 0. && constraint->PenaltyPos() <= 0.)
			continue;

		if (RemoveConstraint(constraint, false, false) == true)
			list.AddItem(constraint);
	}
}


void
ActiveSetSolver::_AddSoftConstraint(const ConstraintList& list)
{
	for (int i = 0; i < list.CountItems(); i++) {
		Constraint* constraint = list.ItemAt(i);
		// at least don't leak it
		if (AddConstraint(constraint, false) == false)
			delete constraint;
	}
}


ResultType
ActiveSetSolver::_FindWithConstraintsNoSoft(const VariableList* variables,
	AddConstraintFunc constraintFunc)
{
	ConstraintList softConstraints;
	_RemoveSoftConstraint(softConstraints);

	ConstraintList constraints;
	for (int32 i = 0; i < variables->CountItems(); i++)
		constraints.AddItem(constraintFunc(fLinearSpec, variables->ItemAt(i)));

	ResultType result = Solve();
	for (int32 i = 0; i < constraints.CountItems(); i++)
		fLinearSpec->RemoveConstraint(constraints.ItemAt(i));

	_AddSoftConstraint(softConstraints);

	if (result != kOptimal)
		TRACE("Could not solve the layout specification (%d). ", result);

	return result;
}