#define SIZE 100

void Smult(float a[][SIZE], float target[][SIZE], float s, int n);
void Mmult(float a[][SIZE], float b[][SIZE], float target[][SIZE], int n);
void Mtranspose(float a[][SIZE], float target[][SIZE], int n);
void Madd(float a[][SIZE], float b[][SIZE], float target[][SIZE], int n);
void Msub(float a[][SIZE], float b[][SIZE], float target[][SIZE], int n);
void Minvert(float a[][SIZE], float target[][SIZE], int n, int iterate);
float Mmag(float a[][SIZE], int n);
/* routines added 03/31/06 */
void Mortho(float target[][SIZE], int row, int col, float angle, int n);
void Morthotrans(float SymM[][SIZE], int row, int col, float angle, int n);
void Morthosweep(float SymM[][SIZE], int n);
void Mprint(float a[][SIZE],int n);
int Mread(float a[][SIZE],int n, char *filename);





void Mortho(float target[][SIZE], int row, int col,float angle, int n){
    /* creates an orthogonal matrix */
    int i,j;
    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	    target[i][j]=0.0;
	}
    }
	
    for(i=0;i<n;i++){
	target[i][i]=1.0;
    }
	
    target[row][row]=cos(angle);
    target[col][col]=cos(angle);
    target[row][col]=sin(angle);
    target[col][row]=-sin(angle);
}

void Morthotrans(float SymM[][SIZE], int row, int col, float angle, int n){
    /* performs orthogonal transformation on a symmmetric matrix */
    int i,j,p;
    float Ortho[SIZE][SIZE],Aux2[SIZE][SIZE], Aux3[SIZE][SIZE],C,S;
    C=cos(angle);
    S=sin(angle);
    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	    Ortho[i][j]=0.0;
	    Aux3[i][j]=0.0;
	    
	     }
	Ortho[i][i]=1.0;
    }

    Ortho[row][row]=C;
    Ortho[col][col]=C;
    Ortho[row][col]=S;
    Ortho[col][row]=-S;
    /* Ortho is the transformatio (orthogonal) matrix */

    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	    Aux3[i][j]=0.0;
	    for(p=0;p<n;p++){
		Aux3[i][j]=Aux3[i][j]+SymM[i][p]*Ortho[p][j];
	    }
	}
    }
    /* Aux3=SymM * Ortho */
		
    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	    Aux2[i][j]=0.0;
	    for(p=0;p<n;p++){
		Aux2[i][j]=Aux2[i][j]+Ortho[p][i]*Aux3[p][j];
	    }
	}
    }
    /* Aux2 is transformed matrix,  Aux2=Ortho^T * SymM * Ortho */
    /* Now replace SymM with Aux2 */
    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	    SymM[i][j]=Aux2[i][j];
	}
    }
}

void Morthosweep(float SymM[][SIZE], int n){
    /* apply orthogonal transformations to reduce off-diagonal elements */
    int i,j;
    float theta;
    for(i=0;i<n-1;i++){
	for(j=i+1;j<n;j++){
	    	theta=0.5*atan2(2.0*SymM[i][j],SymM[j][j]-SymM[i][i]);
		if(theta !=0.0)
		    Morthotrans(SymM,i,j,theta,n);
	}
    }
}


void Mprint(float a[][SIZE],int n){
    int i,j;

    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	    printf("%f\t",a[i][j]);
	}
	printf("\n");
    }
}


int Mread(float a[][SIZE],int n, char *filename){
    /* reads in N^2 numbers from a file into matrix, filling by rows */
    int i,j;
    FILE *fptr;
    fptr=fopen(filename, "r");
    if(fptr==NULL){
	return(0);
    }
    else{
	for(i=0;i<n;i++){
	    for(j=0;j<n;j++){
		fscanf(fptr,"%f", &a[i][j]);
	}
    }
	fclose(fptr);
	return(1);
    }

}  


void Smult(float a[][SIZE], float target[][SIZE], float s, int n){
    int i,j;
    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	    target[i][j]=s*a[i][j];
	}
    }
}


void Mmult(float a[][SIZE], float b[][SIZE], float target[][SIZE], int n){

    int i,j,k;
    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	    target[i][j]=0.0;
	    for(k=0;k<n;k++){
		target[i][j]=target[i][j]+a[i][k]*b[k][j];
	    }
	}
    }
}
	
    


void Mtranspose(float a[][SIZE], float target[][SIZE], int n){
    int i,j;
    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	    target[i][j]=a[j][i];
	}
    }
}


void Madd(float a[][SIZE], float b[][SIZE], float target[][SIZE], int n){

    int i,j,k;
    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	  target[i][j]=a[i][j]+b[i][j];	    
	}
    }
}

void Msub(float a[][SIZE], float b[][SIZE], float target[][SIZE], int n){

    int i,j,k;
    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	  target[i][j]=a[i][j]-b[i][j];	    
	}
    }
}



float  Mmag(float a[][SIZE], int n){

    int i,j,k;
    float sum;
    sum=0.0;
    for(i=0;i<n;i++){
	for(j=0;j<n;j++){
	    sum=sum+a[i][j]*a[i][j];	    
	}
    }
    return(sum);
}


void Minvert(float a[][SIZE], float target[][SIZE], int n, int iterate){
    int i;
    float mag;
    float c[SIZE][SIZE], d[SIZE][SIZE], e[SIZE][SIZE];
    Mtranspose(a,target,n);
    mag=Mmag(a,n);
    Smult(target,target,1.0/mag,n);
    for(i=0;i<iterate;i++){
	Smult(target,c, 2.0, n);
	Mmult(a,target,d,n);
	Mmult(target,d,e,n);
	Msub(c,e,target,n);
    }	
    
}