/************************************************************
*            Basic Unit for Complex Matrices                *
* --------------------------------------------------------- *
* See Demo Program tucmat.cpp (also uses Module Clu.cpp).   *
*                                                           *
*                          C++ Release 1.0 By J-P Moreau.   *
************************************************************/ 
#include "ucmat.h"

#define  ABS   fabs
#define  SQRT  sqrt
#define  PI    4.0*atan(1.0)

  double CABS(COMPLEX Z) {
  //  CABS COMPUTES THE ABSOLUTE VALUE OR MAGNITUDE OF A DOUBLE
  //  PRECISION COMPLEX VARIABLE Z = x + iy
      double U, V, Q, S;
      U = ABS(Z[0]);
      V = ABS(Z[1]);
      S = U + V;
      if (S == 0.0) goto e20;
      if (U > V) goto e10;
      Q = U/V;
      return V*SQRT(1.0+Q*Q);
e10:  Q = V/U;
      return U*SQRT(1.0+Q*Q);
e20:  return 0.0;
} // CABS()

// add two complex numbers
  void CSum(COMPLEX z1, COMPLEX z2, COMPLEX z) {
    z[0] = z1[0] + z2[0];
    z[1] = z1[1] + z2[1];
  }

// subtract two complex numbers
  void CMinus(COMPLEX z1, COMPLEX z2, COMPLEX z) {
    z[0] = z1[0] - z2[0];
    z[1] = z1[1] - z2[1];
  }

// multiply two complex numbers
  void CMul(COMPLEX z1, COMPLEX z2, COMPLEX z) {
    double r1,r2,r,t1,t2,t;
    r1=SQRT(z1[0]*z1[0] + z1[1]*z1[1]);
    if (z1[0] == 0.0)
      if (z1[1] > 0.0)
	    t1=PI/2.0;
      else if (z1[1] < 0.0)
	    t1=-PI/2.0; 
	  else 
	    t1=0.0;
	else {
      t1=atan(z1[1]/z1[0]);
      if (z1[0] < 0.0)
        if (z1[1] >= 0.0)
		  t1=t1+PI;
		else 
		  t1=t1-PI;
    }
    r2=SQRT(z2[0]*z2[0] + z2[1]*z2[1]);
    if (z2[0]==0.0)
      if (z2[1] > 0.0)
	    t2=PI/2.0;
      else if (z2[1] < 0.0) 
	    t2=-PI/2.0;
	  else 
	    t2=0.0;
	else {
      t2=atan(z2[1]/z2[0]);
      if (z2[0] < 0.0)
        if (z2[1] >= 0.0) 
		  t2=t2+PI; 
		else 
		  t2=t2-PI;
    };
    r=r1*r2;
    t=t1+t2;
    if (t>PI)  t=t-2*PI;
    if (t<-PI) t=t+2*PI;
    z[0]=r*cos(t); z[1]=r*sin(t);
  }

// divide two complex numbers
  void CDiv(COMPLEX z1, COMPLEX z2, COMPLEX z) {
    double r1,r2,r,t1,t2,t;
    r1=SQRT(z1[0]*z1[0] + z1[1]*z1[1]);
    if (z1[0] == 0.0)
      if (z1[1] > 0.0)
	    t1=PI/2.0;
      else if (z1[1] < 0.0)
	    t1=-PI/2.0; 
	  else 
	    t1=0.0;
	else {
      t1=atan(z1[1]/z1[0]);
      if (z1[0] < 0.0)
        if (z1[1] >= 0.0)
		  t1=t1+PI;
		else 
		  t1=t1-PI;
    }
    r2=SQRT(z2[0]*z2[0] + z2[1]*z2[1]);
    if (z2[0]==0.0)
      if (z2[1] > 0.0)
	    t2=PI/2.0;
      else if (z2[1] < 0.0) 
	    t2=-PI/2.0;
	  else 
	    t2=0.0;
	else {
      t2=atan(z2[1]/z2[0]);
      if (z2[0] < 0.0)
        if (z2[1] >= 0.0) 
		  t2=t2+PI; 
		else 
		  t2=t2-PI;
    };
    r=r1/r2;
    t=t1-t2;
    if (t>PI)  t=t-2*PI;
    if (t<-PI) t=t+2*PI;
    z[0]=r*cos(t); z[1]=r*sin(t);
  }


// Add two complex matrices
  void AddCMat(int M, int N, CMat A, CMat B, CMat C) {
    int I,J;
    for (I=1; I<=M; I++)
	  for (J=1; J<=N; J++) {
		C[I][J][0] = A[I][J][0] + B[I][J][0];
        C[I][J][1] = A[I][J][1] + B[I][J][1];
      }
  }

// Substract two complex matrices
  void SubCMat(int M, int N, CMat A, CMat B, CMat C) {
    int I,J;
    for (I=1; I<=M; I++)
	  for (J=1; J<=N; J++) {
		C[I][J][0] = A[I][J][0] - B[I][J][0];
        C[I][J][1] = A[I][J][1] - B[I][J][1];
      }
  }

// Multiply two complex matrices - result in C
  void MulCMat(int M, int N, CMat A, CMat B, CMat C) {
    int I,J, K;
    COMPLEX Sum,tmp,tmp1;
    for (I=1; I<=M; I++)                                                                  
	  for (J=1; J<=M; J++) {
        Sum[0] = 0.0;
        Sum[1] = 0.0;
        for (K=1; K<=N; K++) {
          CMul(A[I][K],B[K][J],tmp);
          CSum(Sum,tmp,tmp1);
          Sum[0] = tmp1[0]; Sum[1] = tmp1[1];
        }
        C[I][J][0] = Sum[0];
        C[I][J][1] = Sum[1];
      }                                                                   
  }

// calculate B(M,M)=A(M,M)^N
  void PowCMat(int M,  int N, CMat A, CMat B) {
    CMat M1,M2,TMP; int i,j;
    for (i=1; i<=M; i++)      // copy A in M1 and set M2 to unity
	  for (j=1; j<=M; j++) {
        M1[i][j][0] = A[i][j][0];
        M1[i][j][1] = A[i][j][1];
        if (j==i) {
          M2[i][j][0] = 1.0;
          M2[i][j][1] = 0.0;
        }
        else {
          M2[i][j][0] = 0.0;
          M2[i][j][1] = 0.0;
        } 
      }
    while (N > 0)
	  if ((1.0*N)/2 != int((1.0*N)/2)) { //if N is odd 
        N--;
        MulCMat(M,M,M2,M1,TMP);
        for (i=1; i<=M; i++)      // copy TMP in M2
		  for (j=1; j<=M; j++) {
            M2[i][j][0] = TMP[i][j][0];
            M2[i][j][1] = TMP[i][j][1];
          }
      }
      else {
        N = N / 2;
        MulCMat(M,M,M1,M1,TMP);
        for (i=1; i<=M; i++)      // copy TMP in M1
		  for (j=1; j<=M; j++) {
            M1[i][j][0] = TMP[i][j][0];
            M1[i][j][1] = TMP[i][j][1];
          }
      }
    for (i=1; i<=M; i++)          // copy M2 in B
	  for (j=1; j<=M; j++) {
        B[i][j][0] = M2[i][j][0];
        B[i][j][1] = M2[i][j][1];
      }
  }

  void CDet(int N, CMat A, COMPLEX det) {
/*******************************************************
* Calculate the determinant of a complex matrix A(N,N) *
* Ref.: www.advancedphysics.org/forum/                 *
*******************************************************/
  COMPLEX CP, CQ, One, Tmp;
  double P, Q;
  int I, J, K, K1, L;
  One[0] = 1.0; One[1] = 0.0;
  det[0] = 1.0; det[1] = 0.0;
  for (K=1; K<=N; K++) {
    P=0.0;
    for (J=K; J<=N; J++) {
      Q = CABS(A[J][K]);
      if (Q > P) {
        P=Q;
	    I=J;
      }
    }
    CDiv(One,A[I][K],CP);
    if (I != K) {
      det[0] = -det[0];
      det[1] = -det[1];
      for (L=K; L<=N; L++) {
        CQ[0] = A[I][L][0];
        CQ[1] = A[I][L][1];
	    A[I][L][0]=A[K][L][0];
        A[I][L][1]=A[K][L][1];
        A[K][L][0]=CQ[0];
        A[K][L][1]=CQ[1];
      }
    }
    CMul(det,A[K][K],Tmp);
    det[0] = Tmp[0]; det[1] = Tmp[1];
    if (K < N) {
      K1=K+1;
      for (I=K1; I<=N; I++) {
        CMul(A[I][K],CP,CQ);
        for (L=K1; L<=N; L++) {
		  // A(I,L)=A(I,L)-CQ*A(K,L)
          CMul(CQ,A[K][L],Tmp);
          A[I][L][0] = A[I][L][0] - Tmp[0];
          A[I][L][1] = A[I][L][1] - Tmp[1];
        }
      }
    }
  }
}

// end of file ucmat.cpp