#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#define tiny 1e-12
#include "real.h"
#define min(a,b) ((a)<(b)? (a):(b))
#define max(a,b) ((a)>(b)? (a):(b))

int rotate(int p, int q, real**A, int n, real** V);
int make_unity_matrix(real** V,int n);

void find_max_in_rows(real** A,int n,int* index){
	for(int row=0;row<n;row++){
		double big=-1;
		for(int col=0;col<n;col++)if(col!=row && fabs(A[col][row])>big)
			{index[row]=col;big=fabs(A[col][row]);}}}

void update_index(real** A,int n,int* index,int p,int q){
		double big; int row,col;
		row=p; col=index[row]; big=fabs(A[max(col,row)][min(col,row)]);
		for(int col=0;col<n;col++)
			if(col!=row && fabs(A[max(col,row)][min(col,row)])>big)
			{index[row]=col;big=fabs(A[max(col,row)][min(col,row)]);}
		row=q; col=index[row]; big=fabs(A[max(col,row)][min(col,row)]);
		for(int col=0;col<n;col++)
			if(col!=row && fabs(A[max(col,row)][min(col,row)])>big)
			{index[row]=col;big=fabs(A[max(col,row)][min(col,row)]);}}

int jacobi(real** A, int n, real** V){
// Jacobi diagonalization.
// Upper triangle of A is destroyed.
// V accumulates eigenvectors.

	make_unity_matrix(V,n);

	int* mindex=(int*)calloc(n,sizeof(int));
	find_max_in_rows(A,n,mindex);

	int max_rotations=5*n*n;
	int rotated, rotations=0;
	do{
		rotated=0;
		for(int row=0;row<n;row++){
			int p=min(row,mindex[row]), q=max(row,mindex[row]);
			if(
					tiny*fabs(A[p][p])<fabs(A[q][p]) ||
					tiny*fabs(A[q][q])<fabs(A[q][p])
				){
				rotated=1; 
				rotations++;
				if(rotations>max_rotations){
					fprintf(stderr,"max_rotations reached: %d\n",rotations);
					return -rotations;}
				rotate(p,q,A,n,V);
				update_index(A,n,mindex,p,q);
			}
		}
	}while(rotated==1);
	return rotations;
}