// beam2.h
//
//--------Beam2-------------------
//

class Beam2
{
private:
	long number;
	long NbLink[2];			// connectivity
	long Mat;				// Material
	long R;					// Real
	double A, B, K, M, F, G, P, H, Q, Z; // temporary variables	
	double C,S;				//cos and sin functions for Length etc...
	double dx,dy;			//x and y values for C and S ( SOH CAH TOA... )
	double EL;				// Young's modulus to length ratio
	double Volume;
	double Mass;
	double Angle;			// Angle between the x-axis and the beam
	double Length;
	double **StiffMat;	// Stiffness Matrix dim = 3 (3*2Nodes)

public:
	Beam2(long num, long mat, long real, long *connectivity, Node *DNodes, CrossSection **DCrossSection);
	void CalculateMatrix(Node *DNodes, Material **DMat, CrossSection **DCrossSection, long Loop);
	void AssembleMatrix(double **Global,int MaxDOF,Node *DNodes);
	void WriteStiffMatrix(FILE *file);
	void CalculateStrains(Node *DNodes, Material **DMat, long Loop, CrossSection **DCrossSection, double *Forces);
	//double const GetStrain() const {return Strain1 ; }
	//double GetStress() const { return Stress1; }
	double TopStress1, TopStress2, BotStress1, BotStress2;
	double TopStrain1, TopStrain2, BotStrain1, BotStrain2;
};

/////////////////////////////////////////////

Beam2::Beam2(long num,long mat, long real, long *connectivity, Node *DNodes, CrossSection **DCrossSection)
{
	number = num;
	Mat = mat;
	R = real;

	NbLink[0] = connectivity[0];
	NbLink[1] = connectivity[1];

	DNodes[(NbLink[0]-1)].ActiveDOF[0]=1;	// x DOF active node 1
	DNodes[(NbLink[0]-1)].ActiveDOF[1]=1;	// y DOF active
	DNodes[(NbLink[0]-1)].ActiveDOF[5]=1;	// rotz DOF active
	DNodes[(NbLink[1]-1)].ActiveDOF[0]=1;	// x DOF active node 2
	DNodes[(NbLink[1]-1)].ActiveDOF[1]=1;	// y DOF active
	DNodes[(NbLink[1]-1)].ActiveDOF[5]=1;	// rotz DOF active
	
	dx=DNodes[(NbLink[1] - 1)].nx[0]-DNodes[(NbLink[0] - 1)].nx[0];
	dy=DNodes[(NbLink[1] - 1)].nx[1]-DNodes[(NbLink[0] - 1)].nx[1];
	Length=sqrt(pow(dx,2)+pow(dy,2));
	Volume=Length*DCrossSection[R]->Area;
	if(Volume<=0.0)
		nerror("Element volume must be greater than 0.");
	
	C=dx/Length;
	S=dy/Length;
	Angle = acos(C)*180/PI;
	TopStrain1 = TopStrain2 = BotStrain1 = BotStrain2 = 0;
	TopStress1 = TopStress2 = BotStress1 = BotStress2 = 0;
	StiffMat = new double*[6];
	for (int i=0;i<6;i++){
		StiffMat[i] = new double [6];
	}

}

/////////////////////////////////////////////

void Beam2::CalculateMatrix(Node *DNodes, Material **DMat, CrossSection **DCrossSection, long Loop)
{
	int i,j;
	
	Mass=Volume*DMat[Mat]->Density[Loop];
	
	EL=DMat[Mat]->Ex[Loop]/Length;	// E/L ratio Young modulus over the length
	//Previous Calculations 
	A=4*EL*DCrossSection[R]->Izz;				//A=Area 
	Z=DCrossSection[R]->Area*EL;	// Verify this is the area use instead of A
	B=2*EL*DCrossSection[R]->Izz; 
	K=12*EL*DCrossSection[R]->Izz/Length/Length;
	M = 6*EL*DCrossSection[R]->Izz/Length; 
	F= Z*pow(C,2)+ K*pow(S,2);
	G= (Z-K)*C*S;
	P = Z*pow(S,2)+ K*pow(C,2);
	H = -M*S;
	Q= M*C;
	StiffMat[0][0]= F;
	StiffMat[0][1]= G;
	StiffMat[0][2]= H;
	StiffMat[0][3]= -F;
	StiffMat[0][4]= -G;
	StiffMat[0][5]= H;
	StiffMat[1][1]= P;
	StiffMat[1][2]= Q;
	StiffMat[1][3]= -G;
	StiffMat[1][4]= -P;
	StiffMat[1][5]= Q;
	StiffMat[2][2]= A;
	StiffMat[2][3]= -H;
	StiffMat[2][4]= -Q;
	StiffMat[2][5]= B;		
	StiffMat[3][3]= F;
	StiffMat[3][4]= G;
	StiffMat[3][5]= -H;
	StiffMat[4][4]= P;
	StiffMat[4][5]= -Q;
	StiffMat[5][5]= A;
	// Copy matrix symmetric half
	for (i=0;i<6;i++)
		for (j=i+1;j<6;j++)
			StiffMat[j][i]=StiffMat[i][j];
}

/////////////////////////////////////////////

void Beam2::AssembleMatrix(double **Global, int MaxDOF,Node *DNodes)
{
	int i,j,k,l;

	for(i=0;i<2;i++)				// step for column (4 matrices 3x3)
		for(j=0;j<2;j++)			// step for row (4 matrices 3x3)
			for(k=0;k<3;k++)		// step for line (row)
				for(l=0;l<3;l++)	// step for line (column)
					Global[(DNodes[(NbLink[i]-1)].CumSum+k)][(DNodes[NbLink[j]-1].CumSum+l)]+=StiffMat[(i*3)+k][(j*3)+l];
}

/////////////////////////////////////////////

void Beam2::WriteStiffMatrix(FILE *file)
{
	int i,j;
	fprintf(file,"Element matrix %d, vol=%13.3E\n",number,Volume);
	for (i=0;i<6;i++)	{
		for (j=0;j<6;j++)	{
			fprintf(file,"%13.3E ",StiffMat[i][j]);
			}
		fprintf(file,"\n");
		}
	fprintf(file,"\n");
}

/**************************************************/
void Beam2::CalculateStrains(Node *DNodes, Material **DMat, long Loop, CrossSection **DCrossSection, double *Forces)
{
	
	double Force[6];
	//double Fxa1, Fya1, Fxa2, Fya2;		//Axial forces from x and y nodal Forces
	double Mom1, Mom2;		//Moments calculated from [k]{d}
	double y;
	double AxialStress, AxialForce1, AxialForce2;
	double BendStress1, BendStress2;
	y = DCrossSection[Mat]->Height/2;
	//Fxa1 = Fya1 = Fxa2 = Fya2 = Mom1 = Mom2 = 0.0;	//Set to zero...
	
	matrix_vector_multiply(StiffMat, Forces, 6,6,Force);	//Calc Forces at nodes

	AxialForce1 = Force[3*NbLink[0]-3]*C + Force[3*NbLink[1]-2]*S;
	AxialForce2 = Force[3*NbLink[1]-3]*C + Force[3*NbLink[1]-2]*S;
	//Fxa1 = Force[0]*C;		//Get axial force values from all components
	//Fya1 = Force[1]*S;		//for axial stress,
	Mom1 = Force[2];		//And Moment values for Bending Stress
	//Fxa2 = Force[3]*C;
	//Fya2 = Force[4]*S;
	Mom2 = Force[5];
	
	AxialStress = (AxialForce1-AxialForce2)/DCrossSection[Mat]->Area;	//Axial stress for element
	BendStress1 = Mom1*y/DCrossSection[Mat]->Izz;			//Bending stress - node 1
	BendStress2 = Mom2*y/DCrossSection[Mat]->Izz;			//Bending stress - node 2
	
	TopStress1 = AxialStress + BendStress1;		//Stress varies through element
	BotStress1 = AxialStress - BendStress1;		//BotStress1 is stress at bottom-outer fibre of 1st node of element
	TopStress2 = AxialStress + BendStress2;		//BotStress2 is stress at bottom-outer fibre of 2nd node of element
	BotStress2 = AxialStress - BendStress2;		
	
	TopStrain1 = TopStress1/DMat[Mat]->Ex[Loop];		//Strain = Stress/Ex
	BotStrain1 = BotStress1/DMat[Mat]->Ex[Loop];
	TopStrain2 = TopStress2/DMat[Mat]->Ex[Loop];
	BotStrain2 = BotStress2/DMat[Mat]->Ex[Loop];
	
}