/*
 * \file Constraints.c
 * \brief: implementation of the Constraints class, derived from DataSet class
 */

/*Headers: {{{1*/
#ifdef HAVE_CONFIG_H
	#include "config.h"
#else
#error "Cannot compile with HAVE_CONFIG_H symbol! run configure first!"
#endif

#include <vector>
#include <functional>
#include <algorithm>
#include <iostream>

#include "./DataSet.h"
#include "../shared/shared.h"
#include "../include/include.h"
#include "../EnumDefinitions/EnumDefinitions.h"

using namespace std;
/*}}}*/

/*Object constructors and destructor*/
/*FUNCTION Constraints::Constraints(){{{1*/
Constraints::Constraints(){
	return;
}
/*}}}*/
/*FUNCTION Constraints::Constraints(int in_enum){{{1*/
Constraints::Constraints(int in_enum): DataSet(in_enum){
	//do nothing;
	return;
}
/*}}}*/
/*FUNCTION Constraints::~Constraints(){{{1*/
Constraints::~Constraints(){
	return;
}
/*}}}*/

/*Numerics: */
/*FUNCTION Constraints::NumberOfLocalRgbs{{{1*/
int   Constraints::NumberOfLocalRgbs(int analysis_type){

	int i;
	int  count=0;

	for(i=0;i<this->Size();i++){

		Object* object=(Object*)this->GetObjectByOffset(i);

		/*Check this is a single point constraint (spc): */
		if (object->Enum()==RgbEnum){

			Rgb* rgb=(Rgb*)object;
			if(rgb->InAnalysis(analysis_type))count++;
		}
	}

	return count;
}
/*}}}*/
/*FUNCTION Constraints::NumberOfConstraints{{{1*/
int Constraints::NumberOfConstraints(void){

	int localconstraints;
	int numberofconstraints;

	/*Get number of local constraints*/
	localconstraints=this->Size();

	/*figure out total number of constraints combining all the cpus (no clones here)*/
	#ifdef _PARALLEL_
	MPI_Reduce(&localconstraints,&numberofconstraints,1,MPI_INT,MPI_SUM,0,MPI_COMM_WORLD );
	MPI_Bcast(&numberofconstraints,1,MPI_INT,0,MPI_COMM_WORLD);
	#else
	numberofconstraints=localconstraints;
	#endif

	return numberofconstraints;
}
/*}}}*/
/*FUNCTION Constraints::SetupSpcs{{{1*/
void   Constraints::SetupSpcs(Nodes* nodes,Vec yg,int analysis_type){

	int i;

	Node* node=NULL;
	int nodeid;
	int dof;
	double value;

	for(i=0;i<this->Size();i++){

		Object* object=(Object*)this->GetObjectByOffset(i);

		/*Check this is a single point constraint (spc): */
		if(object->Enum()==SpcEnum){

			Spc* spc=(Spc*)object;

			if(spc->InAnalysis(analysis_type)){

				/*Ok, this object is a constraint. Get the nodeid from the node it applies to: */
				nodeid=spc->GetNodeId();
				dof=spc->GetDof();
				value=spc->GetValue();

				/*Now, chase through nodes and find the corect node: */
				node=(Node*)nodes->GetObjectById(NULL,nodeid);

				/*Apply constraint: */
				if(node){ //in case the spc is dealing with a node on another cpu
					node->ApplyConstraint(yg,dof,value);
				}
			}

		}
	}

	/*Assemble yg: */
	VecAssemblyBegin(yg);
	VecAssemblyEnd(yg);
}
/*}}}*/
/*FUNCTION Constraints::SetupMpcs{{{1*/
void Constraints::SetupMpcs(Mat Rmg,Nodes* nodes,int analysis_type){

	int i;

	int  nodeid1;
	int  nodeid2;
	int  dof;

	int  dof1;
	int  dof2;


	Node* node1=NULL;
	Node* node2=NULL;

	int count=-1;

	for(i=0;i<this->Size();i++){

		Object* object=(Object*)this->GetObjectByOffset(i);

		/*Check this is a mutiple point constraint (spc): */
		if(object->Enum()==RgbEnum){

			Rgb* rgb=(Rgb*)object;

			if(rgb->InAnalysis(analysis_type)){

				/*we found an rgb, increment counter, so that row index for Rmg is up to date: */
				count++;


				nodeid1=rgb->GetNodeId1();
				nodeid2=rgb->GetNodeId2();
				dof=rgb->GetDof();

				/*For this rgb, find the nodes that go with it: */
				node1=(Node*)nodes->GetObjectById(NULL,nodeid1);
				node2=(Node*)nodes->GetObjectById(NULL,nodeid2);

				if ((node1 && !node2) || (!node1 && node2)){
					/*we are missing one node, not good!*/
					ISSMERROR("%s%p%s%p"," in Rgb, missing one node. node1: ",node1," node2: ",node2);
				}

				if(!node1 && !node2){
					/*That's ok, this Rgb can't find those nodes, so leave them alone. They are probably not on this 
					 * cpu!*/
				}
				else{
					/*Ok, this cpu owns both nodes. Put dof for node1 into m set, unless it is already there, 
					 * in which case node2 gets into the m set: */
					if(node1->DofIsInMSet(dof-1)){
						node2->DofInMSet(dof-1);
					}
					else{
						node1->DofInMSet(dof-1);
					}

					/*Plug values into Rmg. We essentially want dofs from node1 and node2 to be the 
					 *same: */
					dof1=node1->GetDof(dof-1); //matlab indexing
					dof2=node2->GetDof(dof-1); //matlab indexing

					MatSetValue(Rmg,count,dof1,1.0,INSERT_VALUES);
					MatSetValue(Rmg,count,dof2,-1.0,INSERT_VALUES);

				}
			}
		}
	}
}
/*}}}*/