package edu.rice.linpack.Matrix.FMatrix;
import edu.rice.linpack.util.*;
import edu.rice.linpack.Vector.*;
import edu.rice.linpack.LNumber.*;

public class FSiPack extends FPacked {
  
  public FSiPack() {
    super();
    order = 0;
  }  
  public FSiPack(int o) {
    order = o;
    float r = (float).5*(order*(order + 1));
    rows = (int) r;
    cols = 1;
    Mat = new float[rows][1];
    pivot = new int[order];
  }
  public FSiPack(float[] f, int o) {
    rows = f.length;
    cols = 1;
    Mat = new float[rows][cols];
    for(int i=0;i<rows;i++) 
      Mat[i][0] = f[i];
    order = o;
    pivot = new int[order];
  }
  public FSiPack(float[][] f, int o) {
    rows = f.length;
    cols = 1;
    order = o;
    Mat = new float[rows][1];
    for(int i=0;i<rows;i++) 
      Mat[i][0] = f[i][0];
    pivot = new int[order];
  }
  public FSiPack(float[][] f) {
    cols = 1;
    order = f.length;
    float r = (float).5*(order*(order + 1));
    rows = (int) r;
    Mat = new float[rows][1];
    Mat = pack(f);
    pivot = new int[order];
  }
  public FSiPack(FSiPack F) {
    rows = F.rows;
    cols = 1;
    Mat = new float[rows][1];
    for(int i=0;i<rows;i++)
      Mat[i][0] = F.Mat[i][0];
    order = F.order;
    pivot = new int[order];
  }
  public FSiPack(FSiFull F) {
    cols = 1;
    order = F.numofRows();
    float r = (float).5*(order*(order + 1));
    rows = (int)r;
    Mat = new float[rows][1];
    Mat = pack(F.Mat);
    pivot = new int[order];
  }
  
  public FSiFull unpack() {
    int k = 0;
    int n = this.order;
    FSiFull R = new FSiFull(n,n);
    for(int j=0;j<n;j++) {
      for(int i=0;i<=j;i++) {
	R.Mat[i][j] = this.Mat[k][0];
	if(j != i)
	  R.Mat[j][i] = this.Mat[k][0];
	k++;
      }
    }	
    return R;
  }

  /*  Solve is completely checked out.
      Factor is done except for:  3,12,14
      determ is completely tested
      inverse is completely tested
      inertia works

      */


  public void factor() 
       throws SingularMatrixException
  {

    int info = 0;
    int kstep = 1;
    boolean swap;
    int im = 0;

    float alpha = (float)(1+Math.sqrt(17))/8;

    int ik = (order*(order-1))/2 - 1;
    
    for(int k=order-1;k>=0;k -= kstep) {
      if(k == 0) {
	pivot[0] = 1;
	if(this.Mat[0][0] == 0) 
	  throw new SingularMatrixException(1);
      }
      else if(k > 0) {
	int km = k-1;
	int kk = ik+k+1;
	float absakk = Math.abs(this.Mat[kk][0]);
	
	int imax = this.i_amax(k,1,ik+1,0);
	int imk = ik+imax+1;
	float colmax = Math.abs(this.Mat[imk][0]);
	
	if(absakk >= alpha*colmax) {
	  kstep = 1;
	  swap = false;
	}
	else {
	  float rowmax = 0;
	  im = imax*(imax+1)/2 - 1;
	  int imj = im + 2*(imax+1);
	  // can move one imax from above to inside loop & imj to top of loop
	  for(int j=imax+1;j<=k;j++){
	    rowmax = Math.max(rowmax,Math.abs(this.Mat[imj][0]));
	    imj += j + 1;
	  }
	  if(imax != 0) {
	    int jmax = this.i_amax(imax,1,im+1,0);
	    int jmim = jmax+im;
	    rowmax = Math.max(rowmax,Math.abs(this.Mat[jmim][0]));
	  }
	  int imim = imax+im+1;
	  if(Math.abs(this.Mat[imim][0]) >= alpha*rowmax) {
	    kstep = 1;
	    swap = true;
	  }
	  else if(absakk >= alpha*colmax*(colmax/rowmax)) {
	    kstep = 1;
	    swap = false;
	  }
	  else {
	    kstep = 2;
	    swap = imax != km;
	  }
	}
	if(Math.max(absakk,colmax) == 0) {
	  pivot[k] = k + 1;
	  info = k+1;
	}
	else {
	  if(kstep == 1) {
	    if(swap) {
	      this.swap(imax+1,1,im+1,0,this,1,ik+1,0);
	      int imj = ik + imax + 1;
	      for(int j=k;j>=imax;j--) {
		int jk = ik + j + 1;
		this.swapElem(jk,imj);
		imj -= j;
	      }
	    }
	    int ij = ik - k;
	    for(int j=k-1;j>=0;j--) {
	      int jk = ik + j + 1;
	      float mulk = -this.Mat[jk][0]/this.Mat[kk][0];
	      this.axpy(j+1,mulk,1,ik+1,0,this,1,ij+1,0);
	      this.Mat[jk][0] = mulk;
	      ij -= j;
	    }
	    pivot[k] = k + 1;
	    if(swap) 
	      pivot[k] = imax + 1;
	  }
	  else {
	    int kmk = ik + k;
	    int ikm = ik - k;
	    
	    if(swap) {
	      this.swap(imax+1,1,im+1,0,this,1,ikm+1,0);
	      int imj = ikm + imax + 1;
	      for(int j=km;j>=imax;j--) {
		int jkm = ikm + j + 1;
		this.swapElem(jkm,imj);
		imj -= j;
	      }
	      this.swapElem(kmk,imk);
	    }
	    int km2 = k - 2;
	    if(km2 >= 0) {
	      float ak = this.Mat[kk][0]/this.Mat[kmk][0];
	      int kmkm = ikm + k;
	      float akm = this.Mat[kmkm][0]/this.Mat[kmk][0];
	      float denom = 1 - ak*akm;
	      int ij = ik - k - (k-1);
	      for(int j=km2;j>=0;j--) {
		this.mulk(j,ik,ikm,kmk,ij,ak,akm,denom);
		ij -= j;
	      }
	    }
	    pivot[k] = -k;
	    if(swap) 
	      pivot[k] = -imax - 1;
	    pivot[k-1] = pivot[k];
	  }
	  ik -= k;
	  if(kstep == 2) 
	    ik -= k-2;
	}
      }
    }
    if(info != 0)
      throw new SingularMatrixException(info);
  }
  public void mulk(int j,int ik, int ikm, int kmk, int ij,
		   float ak, float akm, float denom) {
    int jk = ik + j + 1;
    int jkm = ikm + j + 1;
    float Bk = this.Mat[jk][0]/this.Mat[kmk][0];
    float Bkm = this.Mat[jkm][0]/this.Mat[kmk][0];
    float mulk = (akm*Bk - Bkm)/denom;
    float mulkm = (ak*Bkm - Bk)/denom;
    this.axpy(j+1,mulk,1,ik+1,0,this,1,ij+1,0);
    this.axpy(j+1,mulkm,1,ikm+1,0,this,1,ij+1,0);
    this.Mat[jk][0] = mulk;
    this.Mat[jkm][0] = mulkm;
  }

  public LNumber condition() 
       throws SingularMatrixException, WrongDataTypeException
  {
    Vector Z = new FVector(order);
    return this.condition(Z);
  }
  public LNumber condition(Vector Ze) 
       throws SingularMatrixException, WrongDataTypeException
  {
    float[] Z = Ze.getFloatArray();
    float anorm = this.oneNorm();
    float ynorm;

    try {
      this.factor();
    } finally {

      //  Solve norms  //
    
      for(int j=0;j<order;j++) 
	Z[j] = 0;

      this.solveUDW(Z);
    
      this.solveTransUY(Z);
    
      float S = 1/FUtil.asum(order,Z,1);
      FUtil.scal(order,S,Z,1);
    
      ynorm = this.solveUDV(Z);

      this.solveTransUY(Z);
    
      S = 1/FUtil.asum(order,Z,1);
      FUtil.scal(order,S,Z,1);
      ynorm *= S;
    }
    LFloat R;

    if(anorm != 0)
      R = new LFloat(ynorm/anorm);
    else
      R = new LFloat(anorm);

    return R;
  }
  private void solveUDW(float[] Z) {
    float ek = 1;
    int topk = (order*(order-1))/2;
    int ks;
    for(int k=order-1;k >= 0;k-=ks) {
      int kk = topk+k;
      int topkm = topk-k;
      ks = 1;
      if(pivot[k] < 0)
	ks = 2;
      int kp = Math.abs(pivot[k]) - 1;
      int kps = k+1-ks;
      if(kp != kps) 
	FUtil.swapElems(Z,kp,kps);
      if(Z[k] != 0) 
	ek = FUtil.signOf(ek,Z[k]);      
      Z[k] += ek;
      this.axpy(k-ks+1,Z[k],1,topk,0,Z,1,0);
      if(ks == 1) {
	if(Math.abs(Z[k]) >= Math.abs(this.Mat[kk][0])) {
	  float S = Math.abs(this.Mat[kk][0])/Math.abs(Z[k]);
	  FUtil.scal(order,S,Z,1);
	  ek *= S;
	}
	if(this.Mat[kk][0] != 0) 
	  Z[k] = Z[k]/this.Mat[kk][0];
	else
	  Z[k] = 1;
      }
      else {
	if(Z[k-1] != 0) 
	  ek = FUtil.signOf(ek,Z[k-1]);
	Z[k-1] += ek;
	this.axpy(k-ks+1,Z[k-1],1,topkm,0,Z,1,0);
	this.Zfixer(k,kk,topk-1,Z);
      }
      topk = topkm;
      if(ks == 2)
	topk -= k-1;
    }
    float S = 1/FUtil.asum(order,Z,1);
    FUtil.scal(order,S,Z,1);
  }
  private void solveTransUY(float[] Z) {
    int topk = 0;
    int ks = 1;
    for(int k=0;k<order;k+=ks) {
      ks = 1;
      if(pivot[k] < 0)
	ks = 2;
      if(k != 0) {
	Z[k] += this.dot(k,1,topk,0,Z,1,0);
	int topkp = topk + k + 1;
	if(ks == 2)
	  Z[k+1] += this.dot(k,1,topkp,0,Z,1,0);
	int kp = Math.abs(pivot[k]) - 1;
	if(kp != k) 
	  FUtil.swapElems(Z,k,kp);
      }
      topk += k+1;
      if(ks == 2)
	topk += k+2;
    }
  }
  private float solveUDV(float[] Z) {
    
    float ynorm = 1;
    
    int topk = order*(order-1)/2;
    int ks;
    for(int k=order-1;k >= 0;k-=ks) {
      int kk = topk+k;
      int topkm = topk-k;
      ks = 1;
      if(pivot[k] < 0) 
	ks = 2;
      if(k != ks-1) {
	int kp = Math.abs(pivot[k]) - 1;
	int kps = k + 1 - ks;
	if(kp != kps) 
	  FUtil.swapElems(Z,kp,kps);
	this.axpy(k-ks+1,Z[k],1,topk,0,Z,1,0);
	if(ks == 2)
	  this.axpy(k-ks+1,Z[k-1],1,topkm,0,Z,1,0);
      }
      if(ks == 1) {
	if(Math.abs(Z[k]) > Math.abs(this.Mat[kk][0])) {
	  float S = Math.abs(this.Mat[kk][0])/Math.abs(Z[k]);
	  FUtil.scal(order,S,Z,1);
	  ynorm *= S;
	}
	if(this.Mat[kk][0] == 0) 
	  Z[k] = 1;
	else
	  Z[k] = Z[k]/this.Mat[kk][0];
      }
      else 
	Zfixer(k,kk,topk-1,Z);
      topk = topkm;
      if(ks == 2)
	topk -= k-1;
    }
    float S = 1/FUtil.asum(order,Z,1);
    FUtil.scal(order,S,Z,1);
    ynorm *= S;
    
    return ynorm;
  }
  private void Zfixer(int k, int kk, int topk, float[] Z) {
    int kmk = topk+k;
    int kmkm = topk;
    float ak = this.Mat[kk][0]/this.Mat[kmk][0];
    float akm = this.Mat[kmkm][0]/this.Mat[kmk][0];
    float bk = Z[k]/this.Mat[kmk][0];
    float bkm = Z[k-1]/this.Mat[kmk][0];
    float denom = ak*akm - 1;
    Z[k] = (akm*bk - bkm)/denom;
    Z[k-1] = (ak*bkm - bk)/denom;
  }

  public void solve(Vector B, int J)
       throws WrongDataTypeException
  {
    this.solve(B);
  }
  public void solve(Vector Be) 
       throws WrongDataTypeException
  {
    float[] B = Be.getFloatArray();

    int k = order-1;
    int ik = (order*(order-1))/2 - 1;
    
    while(k >= 0) {
      int kk = ik+k+1;
      if(pivot[k] >= 0) {
	if(k != 0) {
	  int kp = pivot[k] - 1;
	  if(kp != k) 
	    FUtil.swapElems(B,k,kp);
	  this.axpy(k,B[k],1,ik+1,0,B,1,0);  
	}

	B[k] = B[k]/this.Mat[kk][0];
	k--;
	ik = ik - k - 1;
      }
      else {
	int ikm = ik - k;
	if(k != 1) {
	  int kp = Math.abs(pivot[k]) - 1;
	  if(kp != k-1) 
	    FUtil.swapElems(B,k-1,kp);
	  this.axpy(k-1,B[k],1,ik+1,0,B,1,0);
	  this.axpy(k-1,B[k-1],1,ikm+1,0,B,1,0);
	}
	this.Zfixer(k,kk,ik,B);
	k -= 2;
	ik = ik - k - k - 3;
      }
    }
    k = 0;
    ik = 0;
    while(k < order) {
      if(pivot[k] >= 0) {
	if(k != 0) {
	  B[k] += this.dot(k,1,ik,0,B,1,0);
	  int kp = pivot[k] - 1;
	  if(kp != k) 
	    FUtil.swapElems(B,k,kp);
	}
	ik += k+1;
	k++;
      }
      else {
	if(k != 0) {
	  B[k] += this.dot(k,1,ik,0,B,1,0);
	  int ikp = ik+k+1;
	  B[k+1] += this.dot(k,1,ikp,0,B,1,0);
	  int kp = Math.abs(pivot[k]) - 1;
	  if(kp != k) 
	    FUtil.swapElems(B,k,kp);
	}
	ik += k+k+3;
	k += 2;
      }
    }
  }    

  public Vector determ() {
    float[] Det = new float[2];
    Det[0] = 1;
    Det[1] = 0;
    
    TD A = new TD();
    
    int kk;
    int ik = -1;
    for(int k=0;k<order;k++) {
      ik += k;
      kk = ik+k+1;
      this.TDer(A,k,ik,kk);
      
      Det[0] *= A.D;
      if(Det[0] != 0) 
	FUtil.detNorm(Det);
    }
    FVector V = new FVector(Det);
    return V;
  }
  
  public int[] inertia() {
    int[] in = new int[3];
    
    for(int j=0;j<3;j++)
      in[j] = 0;
        
    TD A = new TD();

    int kk;
    int ik = -1;
    for(int k=0;k<order;k++) {
      ik += k;
      kk = ik+k+1;
      this.TDer(A,k,ik,kk);
      
      if(A.D > 0) 
	in[0]++;
      else if(A.D < 0)
	in[1]++;
      else
	in[2]++;      
    }
    return in;
  }
  private void TDer(TD A, int k, int ik, int kk) {
    A.D = this.Mat[kk][0];
    
    if(pivot[k] < 0) {
      if(A.T == 0) {
	int ikp = ik+k+1;
	int kkp = ikp+k+1;
	A.T = Math.abs(this.Mat[kkp][0]);
	A.D = (A.D/A.T)*this.Mat[kkp+1][0] - A.T;
      }
      else {
	A.D = A.T;
	A.T = 0;
      }
    }
  }
  
  public void inverse() {
    
    float[] Work = new float[order];

    int ik = -1;
    int kstep;
    for(int k=0;k<order;k+=kstep) {
      int km = k - 1;
      int kk = ik + k + 1;
      int ikp = ik + k + 1;
      int kkp = ikp + k + 1;
      
      if(pivot[k] >= 0) {
	this.Mat[kk][0] = 1/this.Mat[kk][0];
	if(k > 0) {
	  this.copy(k,1,ik+1,0,Work,1);
	  int ij = -1;
	  for(int j=0;j<k;j++) {
	    int jk = ik + j + 1;
	    this.Mat[jk][0] = this.dot(j+1,1,ij+1,0,Work,1,0);
	    this.axpy(j,Work[j],1,ij+1,0,this,1,ik+1,0);
	    ij += j + 1;
	  }
	  this.Mat[kk][0] += this.dot(k,1,ik+1,0,Work,1,0);
	}
	kstep = 1;
      }
      else {
	this.invAdj(kk,kkp);
	if(k > 0) {
	  this.copy(k,1,ikp+1,0,Work,1);
	  int ij = -1;
	  for(int j=0;j<k;j++) {
	    int jkp = ikp + j +1;
	    this.Mat[jkp][0] = this.dot(j+1,1,ij+1,0,Work,1,0);
	    this.axpy(j,Work[j],1,ij+1,0,this,1,ikp+1,0);
	    ij += j + 1;
	  }
	  this.Mat[kkp+1][0] += this.dot(k,1,ikp+1,0,Work,1,0);
	  this.Mat[kkp][0] += this.dot(k,1,ik+1,0,this,1,ikp+1,0);
	  this.copy(k,1,ik+1,0,Work,1);
	  
	  ij = -1;
	  for(int j=0;j<k;j++) {
	    int jk = ik+j+1;
	    this.Mat[jk][0] = this.dot(j+1,1,ij+1,0,Work,1,0);
	    this.axpy(j,Work[j],1,ij+1,0,this,1,ik+1,0);
	    ij += j+1;
	  }
	  this.Mat[kk][0] += this.dot(k,1,ik+1,0,Work,1,0);
	}
	kstep = 2;
      }
      
      //  Swap  //

      int ks = Math.abs(pivot[k]);
      if(ks-1 != k) {
	int iks = (ks*(ks-1))/2 - 1;
	this.swap(ks,1,iks+1,0,this,1,ik+1,0);
	int ksj = ik + ks;
	for(int j=k;j>=ks-1;j--) {
	  int jk = ik + j + 1;
	  this.swapElem(jk,ksj);
	  ksj -= j;
	}
	if(kstep == 2) {
	  int kskp = ikp + ks;
	  this.swapElem(kskp,kkp);
	}
      }
      ik += k + 1;
      if(kstep == 2) {
	ik += k+2;
      }
    }
  }  
  private void invAdj(int kk, int kkp) {
    
    float T = Math.abs(this.Mat[kkp][0]);
    float ak = this.Mat[kk][0]/T;
    float akp = this.Mat[kkp+1][0]/T;
    float akkp = this.Mat[kkp][0]/T;
    float D = T*(ak*akp - 1);
    this.Mat[kk][0] = akp/D;
    this.Mat[kkp+1][0] = ak/D;
    this.Mat[kkp][0] = -akkp/D;
  }
}