/**********************************************************
 *
 * Solve a quadratic function f(X) = X' A X + b X
 * subject to a set of separation constraints cs
 *
 * Tim Dwyer, 2006
 **********************************************************/

#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <stdio.h>
#include <float.h>
#include <cassert>
#include <libvpsc/solve_VPSC.h>
#include <libvpsc/variable.h>
#include <libvpsc/constraint.h>
#include "gradient_projection.h"
#include <iostream>
#include <2geom/math-utils.h>
#include <math.h>

using namespace std;
using namespace vpsc;
//#define CONMAJ_LOGGING 1

static void dumpVPSCException(char const *str, IncSolver* solver) {
    cerr<<str<<endl;
    unsigned m;
    Constraint** cs = solver->getConstraints(m);
    for(unsigned i=0;i<m;i++) {
        cerr << *cs[i] << endl;
    }
}
/*
 * Use gradient-projection to solve an instance of
 * the Variable Placement with Separation Constraints problem.
 * Uses sparse matrix techniques to handle pairs of dummy
 * vars.
 */
unsigned GradientProjection::solve(double * b) {
	unsigned i,j,counter;
	if(max_iterations==0) return 0;

	bool converged=false;

    IncSolver* solver=NULL;

    solver = setupVPSC();
    //cerr << "in gradient projection: n=" << n << endl;
    for (i=0;i<n;i++) {
        assert(!IS_NAN(place[i]));
        assert(IS_FINITE(place[i]));
        vars[i]->desiredPosition=place[i];
    }
    try {
        solver->satisfy();
    } catch (char const *str) {
        dumpVPSCException(str,solver);
	}

    for (i=0;i<n;i++) {
        place[i]=vars[i]->position();
    }
    for (DummyVars::iterator it=dummy_vars.begin();it!=dummy_vars.end();++it){
        (*it)->updatePosition();
    }
    	
	for (counter=0; counter<max_iterations&&!converged; counter++) {
		converged=true;		
		// find steepest descent direction
        //  g = 2 ( b - Ax )
		for (i=0; i<n; i++) {
			old_place[i]=place[i];
			g[i] = b[i];
			for (j=0; j<n; j++) {
				g[i] -= A[i][j]*place[j];
			}
            g[i] *= 2.0;
		}		
        for (DummyVars::iterator it=dummy_vars.begin();it!=dummy_vars.end();++it){
            (*it)->computeDescentVector();
        }
        // compute step size: alpha = ( g' g ) / ( 2 g' A g )
        //   g terms for dummy vars cancel out so don't consider
		double numerator = 0, denominator = 0, r;
		for (i=0; i<n; i++) {
			numerator += g[i]*g[i];
			r=0;
			for (j=0; j<n; j++) {
				r += A[i][j]*g[j];
			}
			denominator -= 2.0 * r*g[i];
		}
		double alpha = numerator/denominator;

        // move to new unconstrained position
		for (i=0; i<n; i++) {
			place[i]-=alpha*g[i];
            assert(!IS_NAN(place[i]));
            assert(IS_FINITE(place[i]));
            vars[i]->desiredPosition=place[i];
		}
        for (DummyVars::iterator it=dummy_vars.begin();it!=dummy_vars.end();++it){
            (*it)->steepestDescent(alpha);
        }

        //project to constraint boundary
        try {
            solver->satisfy();
        } catch (char const *str) {
            dumpVPSCException(str,solver);
        }
        for (i=0;i<n;i++) {
            place[i]=vars[i]->position();
        }
        for (DummyVars::iterator it=dummy_vars.begin();it!=dummy_vars.end();++it){
            (*it)->updatePosition();
        }
        // compute d, the vector from last pnt to projection pnt
		for (i=0; i<n; i++) {
			d[i]=place[i]-old_place[i];
		}	
		// now compute beta, optimal step size from last pnt to projection pnt
        //   beta = ( g' d ) / ( 2 d' A d )
		numerator = 0, denominator = 0;
		for (i=0; i<n; i++) {
			numerator += g[i] * d[i];
			r=0;
			for (j=0; j<n; j++) {
				r += A[i][j] * d[j];
			}
			denominator += 2.0 * r * d[i];
		}
        for (DummyVars::iterator it=dummy_vars.begin();it!=dummy_vars.end();++it){
            (*it)->betaCalc(numerator,denominator);
        }
		double beta = numerator/denominator;

        // beta > 1.0 takes us back outside the feasible region
        // beta < 0 clearly not useful and may happen due to numerical imp.
        if(beta>0&&beta<1.0) {
            for (i=0; i<n; i++) {
                place[i]=old_place[i]+beta*d[i];
            }
            for (DummyVars::iterator it=dummy_vars.begin();it!=dummy_vars.end();++it){
                (*it)->feasibleDescent(beta);
            }
        }
		double test=0;
		for (i=0; i<n; i++) {
			test += fabs(place[i]-old_place[i]);
		}
        for (DummyVars::iterator it=dummy_vars.begin();it!=dummy_vars.end();++it){
            test += (*it)->absoluteDisplacement();
        }
		if(test>tolerance) {
			converged=false;
		}
	}
    destroyVPSC(solver);
	return counter;
}
// Setup an instance of the Variable Placement with Separation Constraints
// for one iteration.
// Generate transient local constraints --- such as non-overlap constraints 
// --- that are only relevant to one iteration, and merge these with the
// global constraint list (including alignment constraints,
// dir-edge constraints, containment constraints, etc).
IncSolver* GradientProjection::setupVPSC() {
    Constraint **cs;
    //assert(lcs.size()==0);
    
    for(DummyVars::iterator dit=dummy_vars.begin();
            dit!=dummy_vars.end(); ++dit) {
        (*dit)->setupVPSC(vars,lcs);
    }
    Variable** vs = new Variable*[vars.size()];
    for(unsigned i=0;i<vars.size();i++) {
        vs[i]=vars[i];
    }
    if(nonOverlapConstraints) {
        Constraint** tmp_cs=NULL;
        unsigned m=0;
        if(k==HORIZONTAL) {
            Rectangle::setXBorder(0.0001);
            m=generateXConstraints(n,rs,vs,tmp_cs,true); 
            Rectangle::setXBorder(0);
        } else {
            m=generateYConstraints(n,rs,vs,tmp_cs); 
        }
        for(unsigned i=0;i<m;i++) {
            lcs.push_back(tmp_cs[i]);
        }
    }
    cs = new Constraint*[lcs.size() + gcs.size()];
    unsigned m = 0 ;
    for(vector<Constraint*>::iterator ci = lcs.begin();ci!=lcs.end();++ci) {
        cs[m++] = *ci;
    }
    for(vector<Constraint*>::iterator ci = gcs.begin();ci!=gcs.end();++ci) {
        cs[m++] = *ci;
    }
    return new IncSolver(vars.size(),vs,m,cs);
}
void GradientProjection::clearDummyVars() {
    for(DummyVars::iterator i=dummy_vars.begin();i!=dummy_vars.end();++i) {
        delete *i;
    }
    dummy_vars.clear();
}
void GradientProjection::destroyVPSC(IncSolver *vpsc) {
    if(acs) {
        for(AlignmentConstraints::iterator ac=acs->begin(); ac!=acs->end();++ac) {
            (*ac)->updatePosition();
        }
    }
    unsigned m,n;
    Constraint** cs = vpsc->getConstraints(m);
    const Variable* const* vs = vpsc->getVariables(n);
    delete vpsc;
    delete [] cs;
    delete [] vs;
    for(vector<Constraint*>::iterator i=lcs.begin();i!=lcs.end();++i) {
            delete *i;
    }
    lcs.clear();
    //cout << " Vars count = " << vars.size() << " Dummy vars cnt=" << dummy_vars.size() << endl;
    vars.resize(vars.size()-dummy_vars.size()*2);
    for(DummyVars::iterator i=dummy_vars.begin();i!=dummy_vars.end();++i) {
        DummyVarPair* p = *i;
        delete p->left;
        delete p->right;
    }
}

// vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=4:softtabstop=4 :