/*
 * Created on 25-Oct-2010
 *
 * TODO 
 */
package uk.ac.roe.wfau;

import flanagan.complex.Complex;
import flanagan.complex.ComplexPoly;
import flanagan.math.Matrix;

/**
 * Javavication of IDL WCS code to try and get a Java only version of usefyl WCS stuff.
 * IDL WCA is in turn based based on wcslib
 * 
 * @author mar
 *
 */
public class WcsIDL {
  static final int ZPN_TYPE=6;
  static final int TAN_TYPE=2;
  static final double [] CDELT={1.0,1.0};
  static double [] ZEROS={0.0,0.0};
  static double [] DEFAULTS={-9999.9999,-9999.9999};
  static final double [] NAN={Double.NaN,Double.NaN};
  static final double TOL = 1.0e-12;
  static final double D2R =Math.PI/180.0;
  static double NORTH_OFFSET=1.d-7;
  static double SOUTH_OFFSET=1.d-7;
  static double POLE_OFFSET=1.d-7;
  
  static double RADEG=180.0/Math.PI;
  static double PI2 = Math.PI/2.0;
  
      
 
  public static int getProjType(String cType) {
      if (cType.toLowerCase().indexOf("ZPN") >= 0) {
          return ZPN_TYPE;
      }
      else if (cType.toLowerCase().indexOf("TAN") >= 0) {
          return TAN_TYPE;
      }
      else {
          return -1;
      }
      }
  /*
   * pro wcs_rotate, longitude, latitude, phi, theta, crval, LONGPOLE = longpole, $
          LATPOLE = latpole,REVERSE=reverse, ORIGIN = origin, THETA0 = theta0
   */
  

  
  public static double getW0(double [] pv) throws ArithmeticException {
      int k;
      double w0=Double.NaN;
      for (k = pv.length-1; k >= 0 && pv[k] == 0.0; k--);
      System.out.println("get W0 k "+k);
      
      //Check for known parameters in archive
      
      if (k==3) {
          if (pv[1]==1.0 && pv[2]==0.0) {
              if (pv[3]==-60.0) return 0.07453559924999299;
              if (pv[3]==-50.0) return 0.08164965809277261;
              if (pv[3]==-46.0) return 0.08512565307587486;
              if (pv[3]==-45.0) return 0.08606629658238704;
              if (pv[3]==-44.0) return 0.08703882797784891;                            
          }
      }
      else if (k==5) {
          if (pv[1]==1.0 && pv[2]==0.0 && pv[3]==42. && pv[4]==0. && pv[5]==-10000.) return 0.07685210339567314;
      }
      
      
      if (k < 0) {
          throw new ArithmeticException("Invalid PV polynomial");
      }
      if (k <2) {
          w0=Math.PI;
      }
      else {
          double zd1=0.0;
          double d1=pv[1];
          if (d1 <= 0.0) {
              throw new ArithmeticException("PV, no solution");
            }
          double zd2=0;
          double d2=0;
          /* Find the point where the derivative first goes negative. */
          int j;
          for (j = 0; j < 180; j++) {
              zd2 = j*D2R;
              d2  = 0.0;
              for (int m = k; m > 0; m--) {
                d2 = d2*zd2 + m*pv[m];
              }
              
              if (d2 <= 0.0) break;
              zd1 = zd2;
              d1  = d2;
            }
          System.out.println(" j "+j);
          double zd=0;
          double d;
          if (j == 180) {
              /* No negative derivative -> no point of inflection. */
              zd = Math.PI;
              
            } else {
              /* Find where the derivative is zero. */
              for (j = 1; j <= 15; j++) {
                zd = zd1 - d1*(zd2-zd1)/(d2-d1);

                d = 0.0;
                for (int m = k; m > 0; m--) {
                  d = d*zd + m*pv[m];
                }
                System.out.println(j+" get W0 d "+d);
                if (Math.abs(d) < TOL) break;

                if (d < 0.0) {
                  zd2 = zd;
                  d2  = d;
                } else {
                  zd1 = zd;
                  d1  = d;
                }
              }
            }
          w0=zd;
            
          
      }
      System.out.println("get W0 w0 "+w0);
      return Double.NaN;
  }
  
  public static double [] wcsRotate(double longitude,double latitude,double [] crval,double theta0,boolean reverse) {
      return wcsRotate(longitude,latitude,crval,180.0,theta0,reverse);
  }
  
  public static double [] wcsRotate(double longitude,double latitude,double [] crval,double longpole,double theta0,boolean reverse) {
      System.out.println( "longpole "+longpole);
      double phi_p = longpole/RADEG;
      double sp = Math.sin(phi_p);
      double cp = Math.cos(phi_p);
      double alpha_p=0,delta_p=0;
      
      double [] retValues=new double [2];
      
      if (theta0 == 90.0) { 
          System.out.println( "theta0 = 90");
      alpha_p = crval[0]/RADEG;
      delta_p = crval[1]/RADEG;
      } else {
          // TODO
      }
      double sa = Math.sin(alpha_p);
      double ca = Math.cos(alpha_p);
      double sd = Math.sin(delta_p);
      double cd = Math.cos(delta_p);
      
      
      System.out.println( "sa "+ sa);
      System.out.println( "ca "+ ca);
      System.out.println( "sd "+ sd);
      System.out.println( "cd "+ cd);
      /*
      double [][] r = {
              {-sa*sp - ca*cp*sd,  ca*sp - sa*cp*sd, cp*cd},
              {sa*cp - ca*sp*sd, -ca*cp - sa*sp*sd, sp*cd},
              {ca*cd           ,  sa*cd           , sd}
      };
      */
      double [][] r = {
              {-sa*sp - ca*cp*sd,  sa*cp - ca*sp*sd, ca*cd  },
              { ca*sp - sa*cp*sd ,-ca*cp - sa*sp*sd, sa*cd},
              { cp*cd            , sp*cd           , sd}
      };
      
      double phi,phi1,theta1;
      if (reverse) {
          System.out.println( "reverse ");
          phi1=longitude/RADEG;
          theta1=latitude/RADEG;
          for (int row = 0; row < 3; row++)
          {
              for (int col = 0; col < row; col++)
              {
                  double temp = r[row][col];
                  r[row][col] = r[col][row];
                  r[col][row] = temp;
              }
          }
      }
      else {
          System.out.println( "forward ");
          phi = longitude;
          phi1 = longitude/RADEG;
          theta1 = latitude/RADEG;
      }
      System.out.println( "phi1 "+phi1 );
      System.out.println( "theta1 "+theta1 );
      
      double l = Math.cos(theta1)*Math.cos(phi1);
      double m = Math.cos(theta1)*Math.sin(phi1);
      double n = Math.sin(theta1);
      
      System.out.println( "r00 "+r[0][0] );
      System.out.println( "r01 "+r[0][1] );
      System.out.println( "r02 "+r[0][2] );
      System.out.println( "r10 "+r[1][0] );
      System.out.println( "r11 "+r[1][1] );
      System.out.println( "r12 "+r[1][2] );
      System.out.println( "r20 "+r[2][0] );
      System.out.println( "r21 "+r[2][1] );
      System.out.println( "r22 "+r[2][2] );
      
      System.out.println( "l "+ l);
      System.out.println( "m "+ m);
      System.out.println( "n "+ n );
     
      double b0 = r[0][0]*l + r[1][0]*m + r[2][0]*n;
      double b1 = r[0][1]*l + r[1][1]*m + r[2][1]*n;
      double b2 = (r[0][2]*l + r[1][2]*m + r[2][2]*n);
      
      if (b2 < -1.0) {
          b2=-1.0;
      }
      if (b2 > 1.0) {
          b2=1.0;
      }
      System.out.println( "b0 "+ b0);
      System.out.println( "b1 "+ b1);
      System.out.println( "b2 "+ b2);
      if (reverse) { 
      retValues[1] = Math.asin(b2)*RADEG;
      retValues[0] = Math.atan2( b1, b0)*RADEG;
      }
      else {
      retValues[1] = Math.asin(b2)*RADEG;
      retValues[0] = Math.atan2( b1, b0)*RADEG;
      }
      return retValues;
  }
  
  
  public static double [] xy2ad(double x, double y,int map_type, double [] crval,double [] crpix,double [][] cd,double [] pv2,boolean reverse) {     
      return xy2ad(x,y,map_type,crval,crpix,cd,pv2,CDELT,reverse);
  }
  
  public static double [] xy2ad(double x, double y,int map_type,double [] crval,double [] crpix,double [][] cd,double [] pv2,double [] cdelt,boolean reverse) {
      double [] ad= new double [2];
      if (cdelt[0] != 1.0) {
          cd[0][0] = cd[0][0]*cdelt[0];
          cd[0][1] = cd[0][1]*cdelt[0];
          cd[1][1] = cd[1][1]*cdelt[1];
          cd[1][0] = cd[1][0]*cdelt[1];
          }
      double xdif= x - (crpix[0]-1);
      double ydif= y - (crpix[1]-1);
      double xsi= cd[0][0]*xdif + cd[0][1]*ydif;
      double eta= cd[1][0]*xdif + cd[1][1]*ydif;
      
      if (reverse) {          
    	  double temp=crval[0];
    	  crval[0]=crval[1];
    	  crval[1]=temp;
          temp = xsi;
          xsi = eta;
          eta = temp;
      }
      System.out.println("xsi "+xsi);
      System.out.println("eta "+eta);
 
      
      ad=wcsXy2Sph(xsi,eta,map_type,crval,pv2);
            
	  return ad;
	  
  }
  public static double [] ad2xy(double longitude, double latitude,int map_type,double [] crval,double [] crpix,double [][] cd,double [] pv2,boolean reverse) {     
      return ad2xy(longitude,latitude,map_type,crval,crpix,cd,pv2,CDELT,reverse);
  }
  public static double [] ad2xy(double longitude, double latitude,int map_type,double [] crval,double [] crpix,double [][] cd,double [] pv2,double [] cdelt,boolean reverse) {
      double [] xyPix= new double [2];
      if (reverse) {
    	  double temp=crval[0];
    	  crval[0]=crval[1];
    	  crval[1]=temp;
      }
      System.out.println("crvals "+crval[0]+" : "+crval[1]);
      double [] xsiEta = wcsSph2Xy(longitude,latitude,map_type,crval,pv2);
      if (Double.isNaN(xsiEta[0]) || Double.isNaN(xsiEta[1])) {
          System.out.println("NOT a number");
          return NAN;
      }
      double xsi=xsiEta[0];
      double eta=xsiEta[1];
      
      if (cdelt[0] != 1.0) {
      cd[0][0] = cd[0][0]*cdelt[0];
      cd[0][1] = cd[0][1]*cdelt[0];
      cd[1][1] = cd[1][1]*cdelt[1];
      cd[1][0] = cd[1][0]*cdelt[1];
      }
      System.out.println("CD11 "+cd[0][0]);
      System.out.println("CD11 "+cd[0][1]);
      System.out.println("CD21 "+cd[1][0]);
      System.out.println("CD22 "+cd[1][1]);
      if (reverse) {
          double temp = xsi;
          xsi = eta;
          eta = temp;
      }
      Matrix mat=new Matrix(cd);
      Matrix invMat=mat.inverse();
      double [][] cdinv=invMat.getArrayCopy();
      System.out.println("cdinv11 "+cdinv[0][0]);
      System.out.println("cdinv11 "+cdinv[0][1]);
      System.out.println("cdinv21 "+cdinv[1][0]);
      System.out.println("cdinv22 "+cdinv[1][1]);
      double xdif = ( cdinv[0][0]*xsi + cdinv[0][1]*eta  );
      double ydif = ( cdinv[1][0]*xsi + cdinv[1][1]*eta  );
      System.out.println("xdif "+xdif);
      System.out.println("ydif "+ydif);
      xyPix[0]=xdif+crpix[0];
      xyPix[1]=ydif+crpix[1];
                          
      return xyPix;
  }
  
  public static double [] wcsXy2Sph(double xsi, double eta,int map_type,double [] crval) {
      return wcsXy2Sph(xsi,eta,map_type,crval,ZEROS);
  }
  public static double [] wcsXy2Sph(double xsi, double eta,int map_type,double [] crval, double [] pv2) {
      double [] ad= new double [] {Double.NaN,Double.NaN};
      double x=xsi;
      double y=eta;
      double xx=x;
      double yy=y;
      switch (map_type) {
      case ZPN_TYPE:                
          double rtheta = Math.sqrt(xx*xx + yy*yy)/RADEG; 
          System.out.println("rtheta "+rtheta);
          double phi = Math.atan2(xx, -yy);
          System.out.println("phi "+phi);
          int np=0;
          int nZ=0;
          for (int i=0;i<pv2.length;i++) {              
              if (pv2[i] != 0) {
                  np=i+1;
                  nZ++;
              }
          }
          System.out.println("np "+np);
          System.out.println("nz "+nZ);
          if (nZ==0) {
              return NAN;
          }
          double [] pv2NZ  = new double[np];
          double [] pv  = new double[np];
          for (int i=0; i<np;i++) {
              pv2NZ[i]=pv2[i];
              pv[i]=pv2[i];
              System.out.println("PV2NZ i "+pv2NZ[i]+" : "+i);
          }
          
          pv[0]=pv[0]-rtheta;
          //pv[4]=0.0;
          System.out.println("PV2NZ 0 and Pv 0 "+pv2NZ[0]+" : "+pv[0]);
          
          ComplexPoly cp = new ComplexPoly(pv);
          
          Complex [] c =cp.roots();
          

          for (int i=0;i<c.length;i++) {
              System.out.println("pv real imag "+c[i].getReal()+" : "+c[i].getImag());
              System.out.println(c[i].isRealperCent(0.0000000000001));
                      }
          
          
          break;
      case TAN_TYPE:
  
          
          break;
      default:
          ad[0]=Double.NaN;
          ad[1]=Double.NaN;
          break;
  }
      return ad;
  }
  public static double [] wcsSph2Xy(double longitude, double latitude,int map_type,double [] crval) {
      return wcsSph2Xy(longitude,latitude,map_type,crval,ZEROS);
  }
  
    public static double [] wcsSph2Xy(double longitude, double latitude,int map_type,double [] crval, double [] pv2) {
        //; Convert all longitude values into the range -180 to 180 so that equations work properly
        double [] xy= new double [] {Double.NaN,Double.NaN};
       
        
        if (longitude >= 180.0) {
            longitude=longitude-360.0;
        }
        if (Math.abs(latitude-90.0) < NORTH_OFFSET*RADEG ) {
            latitude= 90.0 - NORTH_OFFSET*RADEG;
        }
        if (Math.abs(latitude+90.0) < NORTH_OFFSET*RADEG ) {
            latitude=SOUTH_OFFSET*RADEG*90.0;
        }
        double pv2_1=pv2[0];
        double pv2_2=pv2[1];
        
        // Rotate from standard celestial coordinates into the native system.
        
        double theta0=0;
        if (map_type >= 1 && map_type <=8) {
            theta0=90;
        }
        else if (map_type >= 13 && map_type <=16 ){
            theta0 = pv2_1;
        }
        
        double [] phiTheta=wcsRotate(longitude,latitude,crval,theta0,false);
        
        double phi = phiTheta[0]/RADEG;
        double theta = phiTheta[1]/RADEG;
        System.out.println(" THETA "+theta);
        switch (map_type) {
            case ZPN_TYPE:                
                double z = PI2 - theta;
                System.out.println(" Z "+z);
                int np=0;
                int nZ=0;
                for (int i=0;i<pv2.length;i++) {
                    
                    if (pv2[i] != 0) {
                        np=i+1;
                        nZ++;
                    }
                }
                System.out.println("np "+np+" nZ"+nZ);
                double [] par = new double [np];
                for (int i=0;i<np;i++) {
                    par[i]=pv2[i];
                    System.out.println("par "+i+" "+par[i]);
                }
                boolean bad=false;
                double [] dpar = new double [np-1];
                if (np > 3) {
                    for (int i=0;i<dpar.length;i++) {
                        dpar[i]=(i+1)*par[i+1];
                        System.out.println("dpar "+i+" "+dpar[i]);
                    }
                        ComplexPoly cp = new ComplexPoly(dpar);
                        Complex [] c =cp.roots();
                        boolean realPresent=false;
                        boolean positiveRoots=false;
                        double rlim=-9999.9999;
                       
                        for (int j=0;j<c.length;j++) {
                            System.out.println(c[j].getImag());
                            if (c[j].getImag()==0){
                                realPresent=true;
                                System.out.println(j+" real "+c[j].getReal());
                                if (c[j].getReal() >0.0 ) {
                                    if (positiveRoots){
                                        rlim=Math.min(rlim,c[j].getReal());
                                    }
                                    else {
                                    positiveRoots=true;
                                    rlim=c[j].getReal();
                                    }
                                }
                            }
                        }
                        System.out.println("rlim "+rlim);
                        if (z > rlim || !positiveRoots) {
                            bad=true;
                        }
                 }
                if (!bad) {
                    double r_theta = RADEG*poly(z, par);
                    System.out.println("poly "+poly(z, par));
                    System.out.println("r_theta "+r_theta);
                    xy[0] = r_theta*Math.sin(phi);
                    xy[1] = -r_theta*Math.cos(phi);
                    System.out.println("xy "+xy[0]+" "+xy[1]);
                }
                break;
            case TAN_TYPE:
                if (theta >0 ) {
                    double r_theta = RADEG/Math.tan(theta);
                    xy[0] = r_theta*Math.sin(phi);
                    xy[1] = -r_theta*Math.cos(phi);
                }
                break;
            default:
                xy[0]=Double.NaN;
                xy[1]=Double.NaN;
                break;
        }
                
        
    

        
     return xy;   
    }
    
    private static double poly(double x, double [] c) {
        double rslt=0;
        for (int i=0;i<c.length;i++) {
            rslt=rslt+c[i]*Math.pow(x,i);
            
        }       

        return rslt;
    }
    public static void main(String[] args) {
        /*
        double  [] pv2={0.0,1.0,0.0,-60.0,0.0,0.0};
        double  [] crpix={3.0156013E+03,-9.4614355E+02};
        double [][] cd={
                {5.8226448E-07,-1.1136074E-04},
                {1.1135777E-04, 5.5748001E-07}
        };

        
        double [] crval={2.031720373980E+02,2.617800229649E+01};
        double longitude=202.795072 , latitude=25.841906;
         0.0768521036137934
        */
//        double  [] pv2={0.0,1.0,0.0,42.0,0.0,-10000.0};
        double  [] pv2={0.0,1.0,0.0,42,0.0,-10000.0};
        double  [] crpix={5388.6,6847.8};
        double [][] cd={
                {0,-9.47983333333333E-05},
                {9.47983333333333E-05, 0}
        };

        
        double [] crval={330.00055417128,-60.0058333318789};
       
       
        
        double longitude=330.00055417128 , latitude=-61.0058333318789;
        
        
        
        for (int j=1;j<2;j++) {
            System.out.println("jjj "+j);
            double [] xypix=ad2xy(longitude,latitude,ZPN_TYPE,crval,crpix,cd,pv2,false);
            System.out.println("xyPix "+xypix[0]+" "+xypix[1]);
        //wcsSph2Xy(longitude,latitude,ZPN_TYPE,crval,pv2);
        }
        double tt = Double.NaN;
        System.out.println(tt < 0);
        System.out.println("\n\n\n\n OTHER");
        double [] xypix=xy2ad(100,100,ZPN_TYPE,crval,crpix,cd,pv2,false);
        
        double w0=getW0(pv2);
        System.out.println("\n\n\n\n w0000 " + w0);
        /*
        double [] out=wcsRotate(longitude,latitude,crval,90,false);
        System.out.println("retV "+out[0]+" "+out[1]);
        
        out=wcsRotate(-179.99999999999977,89.8999999999998,crval,90,true);
        System.out.println("retV "+out[0]+" "+out[1]);
        */
    }

}