#ifndef ORDER_C
#define ORDER_C

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <limits.h>
#include <math.h>
#include <complex.h>
#include <string.h>

#include "../numeric/SU21-numeric.h"
#include "../numeric/SU21-numeric-base.c"

int Order(Mtx_t Mtx);
void FindFixedPt(FiniteOrder_t *FO_p,Mtx_t Mtx);
void Phases(FiniteOrder_t *FO_p, Mtx_t M);
void CalculateFiniteOrder(FiniteOrder_t *FO_p,Elt_t Elt);
 
// Given a matrix, assumed to be (projectively) unitary relative to
// $FF1=\diag(1,1,-1)$, check (a) whether it has finite order (no
// larger than MaxFixerSiz) and (b) whether it has a fixed point in
// the unit ball of~$\CC^2$.
//
// If the matrix has finite order, return the order.
// If the matrix fixes no points, return 0.
// If the matrix seems to fix a point, but nonetheless has infinite
// order, panic.

int Order(Mtx_t Mtx) {
  Mtx_t PowMtx,tmpMtx;
  double trace,sum;
  int n,ix;
  
  memcpy(PowMtx,Mtx,sizeof(Mtx_t));
  NormalizeMtx(PowMtx);
  for(n=1;n<=MaxFixerSiz;n++) {
    //printf("Order A: PowMtx ---\n");
    //PrintMtx(PowMtx);
    if(EqMtx(PowMtx,Id3,epsBigger))return n;

    // For an F-unitary matrix M, a necessary condition for having a
    // fixed point is that $|\tr M|<3$.  For a projectively unitary
    // matrix M such that $M^* F M = c^2 F$, the correct condition is
    // $|\tr M|/c < 3$, that is $|\tr M|^2 / c^2 < 9$.
    trace=sum=0;
    for(ix=0;ix<3;ix++) {
      trace+=PowMtx[ix][ix];
      sum+=CONJ(PowMtx[ix][2])*FF1[ix][ix]*PowMtx[ix][2];
    }
    //printf("Order B: trace=%f, sum=%f, FF1[2][2]=%f, PowMtx ---\n",
    // trace,sum,FF1[2][2]);
    //PrintMtx(PowMtx);
    if( CABS(sum)<epsSmall ||
	CABS(trace*trace / sum) > 9.0+epsBig) return 0;

    MultMtx(tmpMtx,Mtx,PowMtx);
    memcpy(PowMtx,tmpMtx,sizeof(Mtx_t));
    NormalizeMtx(PowMtx);
  }

  // At this point we assume that our matrix belongs to a discrete
  // group.  Therefore, it ought not to fix a point without being of
  // finite order.  We're also assuming the order is $\leq$
  // MaxFixerSiz (=600).  Given all that, it's unreasonable that we
  // don't have an answer by now.

  assert(FALSE);
  return -1;
}

// For a given matrix, identify the fixed point in $B(\CC^2)$.  If the
// matrix acts on the ball as a complex reflection, it will have an
// entire disc of fixed points.  This routine will find the fixed
// point nearest the origin.
void FindFixedPt(FiniteOrder_t *FO_p,Mtx_t Mtx){
  Pt_t V,M;

    memset(&(FO_p->FixedPt),0,sizeof(Pt_t)); // Trial point
    while(TRUE) {
      ApplyMtxToPt(V,Mtx,FO_p->FixedPt); // Shift our trial point by the
				     // matrix
      //printf("Order F: EqPt(FO_p->FixedPt,V)=%i FO_p->FixedPt, V, M\n",
      //	     EqPt(FO_p->FixedPt,V));
      //PrintPt(FO_p->FixedPt);
      //PrintPt(V);
      //PrintPt(M);

      // Have we found a fixed point?
      if(EqPt(FO_p->FixedPt,V,epsSmall)) return;

      Midpoint(M,FO_p->FixedPt,V); // Calculate the point midway
   			           // between the trial point and its
   			           // shift
      memcpy(&(FO_p->FixedPt),&M,sizeof(Pt_t)); // That's the new
						// trial point
    }
}


// Given a matrix, projectively unitary with respect to
// $\diag(1,1,-1)$, given its finite order $n$, and given its fixed
// point, $Z$ find the two numbers~$t_0$, and~$t_1$ so that the action
// at the fixed point is conjugate to the action of the $2\times 2$
// unitary matrix:
//
//    e^{2\pi i t_0/n}          0
//            0         e^{2\pi i t_1/n}

void Phases(FiniteOrder_t *FO_p,Mtx_t M) {
  const int n=(FO_p->EltOrder);
  Mtx_t C,CInv,Prod,MConj;
  Pt_t Pt;
  COMPLEX_t b,c,u0,u1,Root,Disc;
  FLOAT_t Arg0,Arg1,nFloat;

  ApplyMtxToPt(Pt,M,FO_p->FixedPt);
  assert(EqPt(Pt,FO_p->FixedPt,epsBig));

  OriginToZMtx(C,CInv,FO_p->FixedPt);
  MultMtx(Prod,CInv,M); MultMtx(MConj,Prod,C);
  NormalizeMtx(MConj);
  
  assert(CheckPU(MConj,FF1,epsBig));
  ApplyMtxToPt(Pt,MConj,ZeroPt);
  assert(EqPt(Pt,ZeroPt,epsBig));

  // Find the eigenvalues of the upper left-hand $2\times 2$ block
  // by solving a quadratic equation.
  b=0.5*(MConj[0][0]+MConj[1][1]);  // 1/2 the trace
  c=MConj[0][0]*MConj[1][1]-MConj[0][1]*MConj[1][0]; // determinant
  Disc=b*b-c; // discriminant of the quadratic
  // If the roots are equal, make them exactly equal.  This avoids the
  // numerical instability associated with square roots of small
  // numbers.
  if(CABS(Disc) < epsBig)
    u0=u1=b;
  else {
    Root=CSQRT(Disc);
    u0=b+Root; u1=(b-Root);
  }

  // Calculate the phases
  nFloat=(FLOAT_t) n;
  Arg0=CARG(u0); Arg1=CARG(u1);
  (FO_p->Phase)[0]=nFloat*(1.0+Arg0/(2.0*PI))+0.5; (FO_p->Phase)[0]-=n;
  (FO_p->Phase)[1]=nFloat*(1.0+Arg1/(2.0*PI))+0.5; (FO_p->Phase)[1]-=n;

  // Check them
  assert(FABS((((FLOAT_t) (FO_p->Phase)[0]/(FLOAT_t) n)*2.0*PI)-Arg0)
	 < epsBigger);
  assert(FABS((((FLOAT_t) (FO_p->Phase)[1]/(FLOAT_t) n)*2.0*PI)-Arg1)
	 < epsBigger);

  // Make sure that $-1$ has phase $\pi$, not $-\pi$.
  if(2*(FO_p->Phase)[0] == -n) (FO_p->Phase)[0]=n/2;
  if(2*(FO_p->Phase)[1] == -n) (FO_p->Phase)[1]=n/2;
  // Put the two phases in order:
  if(abs((FO_p->Phase)[0]) > abs((FO_p->Phase)[1]) || 
     (abs((FO_p->Phase)[0]) == abs((FO_p->Phase)[1]) &&
      (FO_p->Phase)[0]<0 )) {
    int tmp;
    tmp=(FO_p->Phase)[0];
    (FO_p->Phase)[0]=(FO_p->Phase)[1];
    (FO_p->Phase)[1]=tmp;
  }
}

// Given an element, calculate its order.  If that isn't infinite,
// calculate its fixed point and the phases of its eigenvalues.
void CalculateFiniteOrder(FiniteOrder_t *FO_p,Elt_t Elt) {
  FO_p->EltOrder=Order(Elt.Mtx);
  if(FO_p->EltOrder != 0) {
    FindFixedPt(FO_p,Elt.Mtx);
    Phases(FO_p,Elt.Mtx);
  }
  else {
    FO_p->FixedPt[0] = FO_p->FixedPt[1]
      = FO_p->Phase[0] = FO_p->Phase[1]=777;
  }
}

#endif