/* This file is part of the source code for the following publication:
 * Assembling Self-Supporting Structures, Deuss et al., SIGGRAPH Asia 2014
 *
 * Copyright (C) 2014 Mario Deuss <mario.deuss@epfl.ch>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/. */

#ifndef SELF_ASSEMBLY_ANALYZER_H
#define SELF_ASSEMBLY_ANALYZER_H

#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <vector>
#include <memory>
#include "Assembly.h"
#include "QP.h"
#include "AnalyzerParameter.h"
#include "Statistics.h"


/** A class performing a static equilibrium analysis on a given Assembly 
	using the equilibrium model of Livesley, extended to pick a sparse 
	set of chains necessary for equlibrium.
*/

namespace SelfAssembly{
class EquilibriumForces;

class Analyzer{
	
public:
	typedef Eigen::Vector3d Force;
	typedef std::vector<Force> Forces;
	
	typedef Eigen::Triplet<double> Triplet;
	typedef std::vector<Triplet> Triplets;
	
	typedef Eigen::Matrix< double,6,Eigen::Dynamic > Matrix6Xd;
	typedef Eigen::Matrix< double,6,1> Vector6d;
	
	typedef unsigned int Idx;
	typedef std::vector<Idx> Idxs;

	typedef double W;
	typedef std::vector<W> Ws;
	
	typedef QP::Cones Cones;

	typedef Eigen::VectorXd VXd;
	
public:
	Analyzer(const Assembly* _a, //Assembly object to perform analysis on
			 QP& _qp, 			 //Quadratic/conic program solver
			 const SelfAssembly::AnalyzerParameter& ap, //parameter bundle
			 bool segmentation=false,	//Quasi-arch extraction
			 bool _debug=false,			//Debug output
			 int _report=0);			//Output level
	virtual ~Analyzer(){;}
		
public:
	bool staticEquilibrium(const EquilibriumForces* _initF=NULL, //initial forces, used if given QP solver can take an initial point
							EquilibriumForces* _resultF=NULL, 	 //resulting forces explaining the equilibrium
						   const EquilibriumForces *_lastF=NULL);//previous forces, useful if objective contains temporal smoothness term
	
	//Weight on squared chain force magnitude, experimental
	double wCLsq() const;
	void setWCLsq(double wCLsq);
	
	//Weight on chain force magnitude, corresponds to (1-lambda) in equation 5
	double wCL1() const;
	double wCcL1() const { return wCcL1_; }
	
	//Weight on squared tangential part of internal forces, experimental
	double wBLsqTang( Idx iv ) const;
	void setWBLsqTang(double w);
	void setWBLsqTang( Idx iv, double w);

	//Weight on squared normal part of internal forces, experimental
	double wBLsqNorm( Idx iv ) const;
	void setWBLsqNorm(double w);
	void setWBLsqNorm( Idx iv, double w);

	//Weight for the quasi-arch term, corresponds to lambda in equation 5
	double wSeg( Idx ii ) const;
	void setWSeg( Idx ii, double w);

	//Check if there still exists a equilibrium constraint when forcing all values below
	//the numerical zero to be precisely zero
	bool checkRounding() const;
	void setCheckRounding( bool b );
	bool lastRoundingCheckSuccessfull(){ return lastRoundingCheckSuccessfull_;}
	
	//Convert the QP variable vector x to EquilibriumForces
	void xToForces(const VXd& x, EquilibriumForces* _resultF);

	//Reweighting parameter for chains and quasi-arches (interfaces)
	void setReweightChains( bool r){ reweightChains_=r;}
	void setReweightInterfaces( bool r){ reweightInterfaces_=r;}

	//Reweighting functions for chains and quasi-arches.
	void reweightChains(const VXd& x, const VXd &fr, VXd &f, Statistics* s = NULL);
	void reweightInterfaces(const VXd& x, const VXd &fr, VXd &f, Statistics* s = NULL);
	double reweight( double v, double p);

protected:
	
	//total number of variables
	int nvars() const { return 3*totalVertices_.back()+nEC_+ (segmentation_?nEI_:0);}

	//Total number of vertices of all interfaces before ifsi in a_
	unsigned int totalVertices( Idx ifsi ) const;
	
	//Fills the matrix m into the sparse triplets ts, offset by row and col
	void fillBlock(const Eigen::MatrixXd &m, Triplets &ts, Idx row, Idx col);
	
	//Compute the static equilibrium constraint matrix for block bi and interface ifsi
	void blockInterfaceMatrix(Matrix6Xd &m, const B& b, const I& ifs);
	
	//Compute the static equilibrium matrix for the chain between block bi and anchor ai
	void blockChainMatrix(Vector6d &v, const B &b, const A &a);

	void fillEqRhs(VXd &d); //Fill right hand side of the equlibrium equality constraint
	void fillInequality(Triplets &ts, Cones *cones = NULL); //Fill right hand side of the equlibrium inequality constraint
	void fillLowerBounds(VXd &lb);
	void fillUpperBounds(VXd &ub);
	void fillObjective(Triplets &tsH, VXd &f, const EquilibriumForces *lastForces); //Fille objective matrix and vector
	void fillEquality(Triplets &tsC, VXd &d); //Fill equality matrix
	void fillInitialForces(const EquilibriumForces *_initF, VXd &x0); //Fill initial variable x0 from EquilibriumForces
	
	void trivialSolution( EquilibriumForces& f); //Compute a trivial solution by decomposing the negated gravity vector into vectors along the chains for each block
	
private:
	const Assembly* a_;
	QP& qp_;

	Idxs totalVertices_;

	//Maps for contigous memory layout
	std::vector<int> freeEnabledBlockMap_;
	unsigned int nFEB_;
	std::vector<int> enabledIfsMap_;
	unsigned int nEI_;
	std::vector<int> chainMap_;
	unsigned int nEC_;
	
	//objective weights, for documentation see getters/setters above
	Ws wBLsqTang_;
	Ws wBLsqNorm_;
	double wCLsq_;
	double wCL1_;
	double wCL1s_;
	double wCcL1_;
	Ws wSeg_;
	bool irls_; //Do iteratively reweighted least-squares, not iteratively reweighted l1. Experimental
	bool interfaceGroupSparsity_;	//Sparsify the interface forces by grouping all components of the force vector. If disabled, just the normal component.
	
	bool checkRounding_;
	bool lastRoundingCheckSuccessfull_;

	//constraints:
	double friction_; //Friction coefficient, corresponds to alpha in constraint (4)
	double anchorSafetyBound_;
	double chainSafetyBound_;
	bool segmentation_;
	bool fixSelectedInterfacesToZero_;
	bool debug_;
	int report_;

	//reweighting
	bool reweightChains_;
	bool reweightInterfaces_;
	int minReweightIterations_;
	int maxReweightIterations_;
	double lpReweight_;
	
};

}

#endif
