/******************************************/
/* SOFEA.CPP                              */
/*                                        */
/* 31.5.2002 Alexandre Landreau           */
/* 14.6.2002 - 23.7.2002 Re-written by TC */
/*                                        */
/******************************************/

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream.h>
#include <math.h>

#include "tools.h"
#include "sofea.h"
#include "link1.h"
#include "beam2.h"
#include "plane3.h"
#include "plane4.h"


#define MAXELEM 5000	// can be increased as required


/* ELEMENT TYPES SUPPORTED
Linear Elastic Structural Finite Elements:
  1 - link1 2d 2 node
  2 - beam2 2d 2 node
  3 - plane3 2d 3 node constant strain triangle
  4 - plane4 2d 4 node
  5 -
  6 -
  7 -
  8 -
  9 -
 10 -
*/



Gauss22 *DGauss22;


void main (int argc, char **argv)
{
	FILE *file ;
	FILE *fptr_out;
	
	Node *DNodes;
	Material *DMat[MAXELEM];
	CrossSection *DCrossSection[MAXELEM];

	// Element types
	Link1 *DLink1[MAXELEM];
	Plane3 *DPlane3 [MAXELEM];
	Beam2 *DBeam2 [MAXELEM];
	Plane4 *DPlane4 [MAXELEM];
	
	long NbElems=0, NbNodes=0;
	int countlev1, *indx, MaxDOF=2;
	int nodedof;
	long count[256]={0},num; // to know how many objects are created per kind of element
	double *Forces, max, max_displacement;
	int interactive;
	char fn1[80], fn2[80];
	int i,j;

	char string[100];
	char tokensep[] = ",\n";
	char *token, *arg1, *arg2, *arg3;
	
	int type[100];			// element types (max 100)
	int current_type;		// element type selected
	long current_real;		// real constant set selected
	long current_material;	// material selected
	
	long ielem=0;
	long imat=0;
	long ics=0;

	DGauss22 = new Gauss22;

	if(argc < 3)    {
	    cout << "*** SOFEA PROGRAM V1 @ 14.6.2002***\n";
	    interactive = 1;
	    cout << "USAGE = " << argv[0] << "<model file> <results file>\n";
	    cout << "Model file name       :\n-> ";
	    cin >> fn1;
	    cout << "Results data file name:\n-> ";
	    cin >> fn2;
	    }
	else    {
	    interactive = 0;
	    strcpy(fn1,argv[1]); 
		strcpy(fn2,argv[2]);
	    }

	cout << "Model file name is   = " << fn1 << "\n";
	cout << "Results file name is = " << fn2 << "\n";

	cout << "\nReading input file ...\n";

	if ((file =fopen(fn1,"r")) == NULL)
		nerror("Data input file cannot be found.");
	cout << "File opened\n";

	while(!feof(file))	{
	    fgets(string,99,file);
	    token = strtok(string,tokensep);
		if (!strcmp ("",token))	{
			//blank line
			}
		if (!strcmp ("!",token))	{
			//comment line
			}
		if (!strcmp ("node",token))	{	//Number of nodes:
            token = strtok(NULL,tokensep);
			sscanf(token,"%ld", &NbNodes);
			//Create Node array
			DNodes =new Node[NbNodes+1];
			cout << "Number of nodes = " << NbNodes << "\n";
			}
		if (!strcmp ("elem",token))	{  // Number of elements:
            token = strtok(NULL,tokensep);
			sscanf(token,"%ld", &NbElems);
			cout << "Number of elems = " << NbElems << "\n";
			}
		if (!strcmp ("n",token))	{	// Node coordinates
			long inode;
			double x,y;
		    arg1 = strtok(NULL,tokensep); // node number
		    arg2 = strtok(NULL,tokensep); // x coordinate
		    arg3 = strtok(NULL,tokensep); // y coordinate
			sscanf(arg1,"%ld",&inode);
			DNodes[inode-1].Init();
			DNodes[inode-1].number=inode;
			sscanf(arg2,"%le", &x);
			sscanf(arg3,"%le", &y);
			DNodes[inode-1].nx[0] = x;
			DNodes[inode-1].nx[1] = y;
			}
		if (!strcmp("et",token))	{	// Define element type references
			int typenum;
			int etype;
			token = strtok(NULL,tokensep);
			sscanf(token,"%i",&typenum);
			if((typenum<1)||(typenum>99))
				nerror("Type number must be between 1-99");
			token = strtok(NULL,tokensep);
			sscanf(token,"%i",&etype);	  // element type
			//if((etype!=1)||(etype!=3))
			//	nerror("Only element types 1&3 are allowed as of 5.7.2002");
			type[typenum]=etype;
			}
		if (!strcmp("type",token))	{	// Select element type
			int typenum;
			token = strtok(NULL,tokensep);
			sscanf(token,"%i",&typenum);
			if((typenum<1)||(typenum>255))
				nerror("Type number must be between 1-255");
			current_type=type[typenum];
			}
		if (!strcmp("real",token))	{	// Select element type
			long realnum;
			token = strtok(NULL,tokensep);
			sscanf(token,"%ld",&realnum);
			if((realnum<1)||(realnum>MAXELEM))
				nerror("Real constant set number must be between 1-MAXELEM");
			current_real=realnum;
			}
		if (!strcmp("mat",token))	{	// Select material
			long matnum;
			token = strtok(NULL,tokensep);
			sscanf(token,"%ld",&matnum);
			if((matnum<1)||(matnum>MAXELEM))
				nerror("Material number must be between 1-MAXELEM");
			current_material=matnum;
			}
		if (!strcmp("e",token))	{	// Read elements
			// element number is ielem
			countlev1=current_type;	  // element type
			//
			// Link1 2d read
			//
			if (countlev1==1) {
				long connectivity[2];
				for(i=0;i<2;i++)	{
					token = strtok(NULL,tokensep);
					sscanf(token,"%ld", &connectivity[i]); 
					}
				DLink1[count[countlev1]] = new Link1(ielem,current_material,current_real,connectivity,DNodes);
				count[countlev1]++;
				ielem++;
				}
			//
			// Beam2 2d read
			//
			if (countlev1==2) {
				long connectivity[2];
				for(i=0;i<2;i++)	{
					token = strtok(NULL,tokensep);
					sscanf(token,"%ld", &connectivity[i]); 
					}
				DBeam2[count[countlev1]] = new Beam2(ielem,current_material,current_real,connectivity,DNodes);		
				count[countlev1]++;
				ielem++;
				}
			//
			// Plane3 2d 3 node read
			//
			if (countlev1==3) {
				long connectivity[3];
				for(i=0;i<3;i++)	{
					token = strtok(NULL,tokensep);
					sscanf(token,"%ld", &connectivity[i]); 
					}
				DPlane3[count[countlev1]] = new Plane3(ielem,current_material,current_real,connectivity,DNodes);
				count[countlev1]++;
				ielem++;
				}
			//
			// Plane4 2d 4 node read
			//
			if (countlev1==4) {
				long connectivity[4];
				for(i=0;i<4;i++)	{
					token = strtok(NULL,tokensep);
					sscanf(token,"%ld", &connectivity[i]); 
					}
				DPlane4[count[countlev1]] = new Plane4(ielem,current_material,current_real,connectivity,DNodes);
				count[countlev1]++;
				ielem++;
				}
			}
					
		// Material data read
		if (!strcmp ("mp",token))	{
			long number;
			double value;
			arg1 = strtok(NULL,tokensep);	// ex,nuxy,alpx,dens etc. (label)
			arg2 = strtok(NULL,tokensep);	// material number
			arg3 = strtok(NULL,tokensep);	// value
			sscanf(arg2,"%ld",&number);
			sscanf(arg3,"%le",&value);
			if(number>imat)	{
				DMat[number] = new Material(number);
				imat++;
				}
			if (!strcmp("ex",arg1))		DMat[number]->Ex = value;
			if (!strcmp("nuxy",arg1))	DMat[number]->Nuxy = value;
			if (!strcmp("alpx",arg1))	DMat[number]->Alphax = value;
			if (!strcmp("dens",arg1))	DMat[number]->Density = value;
			}

		// Cross Section Property data read
		if (!strcmp ("cs",token))	{
			long number;
			double value;
			arg1 = strtok(NULL,tokensep);	// area,Izz,height etc. (label)
			arg2 = strtok(NULL,tokensep);	// node number
			arg3 = strtok(NULL,tokensep);	// value
			sscanf(arg2,"%ld",&number);
			sscanf(arg3,"%le",&value);
			if(number>ics)	{
				DCrossSection[number] = new CrossSection(number);
				ics++;
				}
			if (!strcmp("area",arg1))	DCrossSection[number]->Area = value;
			if (!strcmp("izz",arg1))	DCrossSection[number]->Izz = value;
			if (!strcmp("height",arg1))	DCrossSection[number]->Height = value;
			}
		// Read Force Boundary Conditions
		if(!strcmp(token,"f"))  {
			long number;
			double f;
			arg1 = strtok(NULL,tokensep);	// node number
			arg2 = strtok(NULL,tokensep);	// fx,fy,fz,mx,my,mz etc. (label)
			arg3 = strtok(NULL,tokensep);	// value
			sscanf(arg1,"%ld",&number);
			sscanf(arg3,"%le",&f);
			if(!strcmp(arg2,"fx"))	DNodes[number-1].ForceBC[0] = f;
			if(!strcmp(arg2,"fy"))	DNodes[number-1].ForceBC[1] = f;
			if(!strcmp(arg2,"fz"))	DNodes[number-1].ForceBC[2] = f;
			if(!strcmp(arg2,"mx"))	DNodes[number-1].ForceBC[3] = f;
			if(!strcmp(arg2,"my"))	DNodes[number-1].ForceBC[4] = f;
			if(!strcmp(arg2,"mz"))	DNodes[number-1].ForceBC[5] = f;
			}
			
		// Read Displacement Boundary Conditions
		if (!strcmp("d",token))	{
			long number;
			double u;
			arg1 = strtok(NULL,tokensep);	// node number
			arg2 = strtok(NULL,tokensep);	// ux,uy,uz,rotx,roty,rotz etc. (label)
			arg3 = strtok(NULL,tokensep);	// value
			sscanf(arg1,"%ld",&number);
			sscanf(arg3,"%le",&u);
			if(!strcmp(arg2,"ux"))   {
				DNodes[number-1].FixedDOF[0] = 1;
				DNodes[number-1].DispBC[0] = u;
				}
			if(!strcmp(arg2,"uy"))   {
				DNodes[number-1].FixedDOF[1] = 1;
				DNodes[number-1].DispBC[1] = u;
				}
			if(!strcmp(arg2,"uz"))   {
				token = strtok(NULL,tokensep);
				DNodes[number-1].FixedDOF[2] = 1;
				DNodes[number-1].DispBC[2] = u;
				}
			if(!strcmp(arg2,"rotx"))   {
				DNodes[number-1].FixedDOF[3] = 1;
				DNodes[number-1].DispBC[3] = u;
				}
			if(!strcmp(arg2,"roty"))   {
				DNodes[number-1].FixedDOF[4] = 1;
				DNodes[number-1].DispBC[4] = u;
				}
			if(!strcmp(arg2,"rotz"))   {
				DNodes[number-1].FixedDOF[5] = 1;
				DNodes[number-1].DispBC[5] = u;
				}
			}
		}

	fclose (file);
	cout << "completed reading data!\n";

	cout << "Assemble matrices...";

	// count active DOF per node
	MaxDOF=0;
	for(i=0;i<NbNodes;i++)	{
		DNodes[i].CumSum=MaxDOF;
		for(j=0;j<6;j++) DNodes[i].DOF+=DNodes[i].ActiveDOF[j];
		MaxDOF+=DNodes[i].DOF;
		}


	indx = new int [MaxDOF];
	for(i=0;i<MaxDOF;i++)	indx[i]=1;

	for(i=0;i<NbNodes;i++)	{
		nodedof=0;
		for(j=0;j<6;j++)	{
			if(DNodes[i].ActiveDOF[j] == 1)	{
				if(DNodes[i].FixedDOF[j] == 1) indx[DNodes[i].CumSum+nodedof]=0;
				nodedof++;
				}
			}
		}

	Forces = new double [MaxDOF];
	for (i=0;i<MaxDOF;i++)		Forces[i]=0;

	for(i=0;i<NbNodes;i++)	{
		nodedof=0;
		for(j=0;j<6;j++)	{
			if(DNodes[i].ActiveDOF[j] == 1)	{
				Forces[DNodes[i].CumSum+nodedof]=DNodes[i].ForceBC[j];
				nodedof++;
				}
			}
		}
	
	// Global matrix creation ! square matrix for the moment
	double **Global = new double*[MaxDOF]; 
	for (i=0;i<MaxDOF;i++)	Global[i] = new double[MaxDOF];

	// Initialization of Global Matrix
	for (i=0;i<MaxDOF;i++)
		for (j=0;j<MaxDOF;j++)
			Global[i][j]=0;

	// Assemble Matrix
	for (i=0;i<count[1];i++)	{	
		DLink1[i]->CalculateMatrix(DNodes, DMat, DCrossSection);
		DLink1[i]->AssembleMatrix(Global,MaxDOF,DNodes);
		}
	for (i=0;i<count[2];i++)	{	
		DBeam2[i]->CalculateMatrix(DNodes, DMat, DCrossSection);
		DBeam2[i]->AssembleMatrix(Global,MaxDOF,DNodes);
		}
	for (i=0;i<count[3];i++)	{	
		DPlane3[i]->CalculateMatrix(DNodes, DMat);
		DPlane3[i]->AssembleMatrix(Global,MaxDOF,DNodes);
		}
	for (i=0;i<count[4];i++)	{	
		DPlane4[i]->CalculateMatrix(DNodes, DMat);
		DPlane4[i]->AssembleMatrix(Global,MaxDOF,DNodes);
		}

	cout << "completed!\n";

// Open results output file 
	if((fptr_out = fopen(fn2,"w")) == NULL)
	    nerror("SOFEA results output file cannot be opened.");
	fprintf(fptr_out,"************************************\n");
	fprintf(fptr_out,"*SOFEA V1.0 FINITE ELEMENT RESULTS *\n");
	fprintf(fptr_out,"************************************\n");

	fprintf(fptr_out,"Nodes:\n");
	for (i=0;i<NbNodes;i++)	{
		fprintf(fptr_out,"%7d, %16.7E, %16.7E\n",DNodes[i].number,DNodes[i].nx[0],DNodes[i].nx[1]);
		}

/* For debugging element matrices
	fprintf(fptr_out,"Element matrices:\n");
	for (i=0;i<count[1];i++)	DLink1[i]->WriteStiffMatrix(fptr_out);
	for (i=0;i<count[2];i++)	DBeam2[i]->WriteStiffMatrix(fptr_out);
	for (i=0;i<count[3];i++)	DPlane3[i]->WriteStiffMatrix(fptr_out);
	for (i=0;i<count[4];i++)	DPlane4[i]->WriteStiffMatrix(fptr_out);

	fprintf(fptr_out,"Global matrix after assembly\n");
	for (i=0;i<MaxDOF;i++) {
		for (j=0;j<MaxDOF;j++)	{
			fprintf(fptr_out,"%13.3E ",Global[i][j]);
			}
		fprintf(fptr_out,"\n");
		}
	fprintf(fptr_out,"\n");

*/

// Apply Displacement Boundary Conditions - penalty method

	for (i=0;i<MaxDOF;i++)	if (indx[i] == 0)	Global[i][i]+=1e30;

	for(i=0;i<NbNodes;i++)	{
		for(j=0;j<DNodes[i].DOF;j++)	{
			if(DNodes[i].FixedDOF[j] == 1) {
				Forces[DNodes[i].CumSum+j]=DNodes[i].DispBC[j]*1e30;
				}
			}
		}

	cout << "Solving by LU decomposition...";

//display the Result the backsubstition put all the result in the Forces vector.. 
//(Forces replaced by Displacements)

// First step use the tool header and the Lower Upper Decomposition
	ludcmp(Global,MaxDOF,indx,Forces);

// Then let's do a back substitution to get the result
	lubksb(Global,MaxDOF,indx,Forces);

	cout << "completed!\nSorting calculated displacements...";

	// copy results into node
	for(i=0;i<NbNodes;i++)	{
		nodedof=0;
		for(j=0;j<6;j++)	{
			if(DNodes[i].ActiveDOF[j] == 1)	{
				DNodes[i].u[j]=Forces[DNodes[i].CumSum+nodedof];
				nodedof++;
				}
			}
		}

	// calculate maximum displacement
	max=0;
	for (i=0;i<NbNodes;i++) {
		double ux2,uy2,uz2;
		ux2=pow(DNodes[i].u[0],2);
		uy2=pow(DNodes[i].u[1],2);
		uz2=pow(DNodes[i].u[2],2);
		max_displacement=sqrt(ux2+uy2+uz2);
		if (max_displacement > max)	{
			max=max_displacement;
			num=i+1;
			}
		}

	cout<<"completed!\n"
		<<"\nThe maximum displacement is: "<< max
		<<" at node: "<< num
		<<"\n   ux = "<< DNodes[num-1].u[0]
		<<" and uy = "<< DNodes[num-1].u[1]
		<<"\n\n";
	

	fprintf(fptr_out,"Nodal DOF Results\n");
	for (i=0;i<NbNodes;i++)	{
		fprintf(fptr_out,"%7d",DNodes[i].number);
		for (j=0;j<6;j++)	fprintf(fptr_out,", %16.7E",DNodes[i].u[j]);
		fprintf(fptr_out,"\n");
		}

	fprintf(fptr_out,"\nThe maximum displacement is: %16.7E \n",max);
	fprintf(fptr_out,"at node number: %7d\n",num);
	fprintf(fptr_out,"    ux = %16.7E \n", DNodes[num-1].u[0]);
	fprintf(fptr_out,"and uy = %16.7E \n", DNodes[num-1].u[1]);

/* for debugging global matrix assembly etc
	fprintf(fptr_out,"Global matrix\n");
	for (i=0;i<MaxDOF;i++) {
		for (j=0;j<MaxDOF;j++)	{
			fprintf(fptr_out,"%13.3E ",Global[i][j]);
			}
		fprintf(fptr_out,"\n");
		}
	fprintf(fptr_out,"\n");
*/
	fclose(fptr_out);
}