package geomExtension;


import util.Util;
import java.awt.*;
import java.awt.geom.*;
import java.text.*;
import java.io.*;

public class FergusonCurve2D{
    Point2D[] P=null;
    Vector2D[] Tin=null;
    Vector2D[] Tout=null;
    boolean closed=false;
    static double pai=Math.PI;
    static double eps=1e-12;
    public static final int TIN=0;
    public static final int TOUT=1;
    NumberFormat nf = NumberFormat.getNumberInstance();
    static int debug=0;
    
    public FergusonCurve2D(){}

    public FergusonCurve2D(Point2D[] P, Vector2D[] Tin, Vector2D[] Tout){
        if(P==null||Tin==null||Tout==null||P.length!=Tin.length||P.length!=Tout.length){
            System.err.println("*** Error in FergusonCurve2D.Constructor");
            return;
        }
        this.closed=false;
        double dist=Vector2D.dist(new Vector2D(P[0]), new Vector2D(P[P.length-1]));
        if(dist<Curve2D.closedTolerance) {
            this.closed=true;
            P[P.length-1]=(Point2D)P[0].clone();
        }
        this.P=P;
        this.Tin=Tin;
        this.Tout=Tout;
        for(int i=0;i<P.length;i++){
            this.P[i]=(Point2D)P[i].clone();
            this.Tin[i]=(Vector2D)Tin[i].clone();
            this.Tout[i]=(Vector2D)Tout[i].clone();
        }
    }

    public boolean isClosed(){
        return closed;
    }
    
    public int getNumOfSegments(){
        if(this.P!=null) return this.P.length-1;
        return 0;
    }
    
    public CubicCurve2DE getCubicCurve2DE(){
        Segment2D[] segments=new Segment2D[this.P.length-1];
        for(int i=0;i<this.P.length-1;i++){
            CubicCurve2D cubicSegment=this.getCubicCurve2D(i);
            segments[i]=new Segment2D(Segment2D.CUBIC,cubicSegment);
        }
        CubicCurve2DE newCurve=new CubicCurve2DE(segments);
        newCurve.closed=this.closed;
        return newCurve;
    }

    public CubicCurve2D getCubicCurve2D(int index){
        if(index<0||index>=this.P.length-1){
            System.err.println("*** Error FergusonCurve2D.getBezierSegment; index out og range; index="+index);
            return null;
        }
        Point2D[] Q=new Point2D[4];
        Q[0]=this.getP(index);
        Q[3]=this.getP(index+1);
        Vector2D Tout=this.getTout(index);
        Vector2D Tin=this.getTin(index+1);
        Q[1]=new Point2D.Double(Q[0].getX()+Tout.getX()/3d, Q[0].getY()+Tout.getY()/3d);
        Q[2]=new Point2D.Double(Q[3].getX()-Tin.getX()/3d, Q[3].getY()-Tin.getY()/3d);
        CubicCurve2D segment=new CubicCurve2D.Double(Q[0].getX(), Q[0].getY(), 
                Q[1].getX(), Q[1].getY(), Q[2].getX(), Q[2].getY(), Q[3].getX(), Q[3].getY());
        return segment;
    }

    public Point2D[] getCtrlPoints(){
        int PLen=this.P.length;
        int TinLen=this.Tin.length;
        int ToutLen=this.Tout.length;
        if(PLen!=TinLen||PLen!=ToutLen){
            System.err.println("*** Error FergusonCurve2D.getCtrlPoints");
            return null;
        }
        Point2D[] Q=new Point2D[3*(PLen-1)+1];
        Point2D[] Q0=new Point2D[4];
        for(int i=0;i<=PLen-2;i++){
            Q0[0]=(Point2D)this.P[i].clone();
            Q0[3]=(Point2D)this.P[i+1].clone();
            Vector2D T0out=(Vector2D)this.Tout[i].clone();
            Vector2D T0in=(Vector2D)this.Tin[i+1].clone();
            Q0[1]=new Point2D.Double(Q0[0].getX()+T0out.getX()/3d, Q0[0].getY()+T0out.getY()/3d);
            Q0[2]=new Point2D.Double(Q0[3].getX()-T0in.getX()/3d, Q0[3].getY()-T0in.getY()/3d);
            if(i==0) Q[3*i]=Q0[0];
            Q[3*i+1]=Q0[1];
            Q[3*i+2]=Q0[2];
            Q[3*i+3]=Q0[3];
        }
        return Q;
    }
    
    public Point2D getP(int index){
        if(index<0||index>this.P.length-1){
            System.err.println("*** Error FergusonCurve2D.getP; index out of range; index="+index);
            return null;
        }
        return this.P[index];
    }
    
    public void setP(Point2D P, int index){
        if(index<0||index>this.P.length-1){
            System.err.println("*** Error FergusonCurve2D.setP; index out of range; index="+index);
            return;
        }
        this.P[index]=P;
        if(closed){
            if(index==0) this.P[this.P.length-1]=P;
            if(index==this.P.length-1) this.P[0]=P;
        }
    }
    
    public Vector2D getTin(int index){
        if(index<0||index>this.P.length-1){
            System.err.println("*** Error FergusonCurve2D.getTin ; index out of range; index="+index);
            return null;
        }
        return this.Tin[index];
    }
    
    public void setTin(Vector2D Tin, int index){
        if(index<0||index>this.P.length-1){
            System.err.println("*** Error FergusonCurve2D.setTin; index out of range; index="+index);
            return;
        }
        this.Tin[index]=Tin;
        if(closed&&index==0) this.Tout[this.P.length-1]=Tin;
    }
    
    public Vector2D getTout(int index){
        if(index<0||index>this.P.length-1){
            System.err.println("*** Error FergusonCurve2D.getTout; index out of range; index="+index);
            return null;
        }
        return this.Tout[index];
    }
    
    public void setTout(Vector2D Tout, int index){
        if(index<0||index>this.P.length-1){
            System.err.println("*** Error FergusonCurve2D.setTout; index out of range; index="+index);
            return;
        }
        this.Tout[index]=Tout;
        if(closed&&index==(this.P.length-1)) this.Tin[0]=Tout;
    }

    public Point2D getP(double t){
        int it=(int)t;
        if(it<0) it=0;
        if(it>=this.P.length-1) it=this.P.length-2;
        double t0=t-it;
        double f0=(1-t0)*(1-t0)*(1+2*t0);
        double f1=(3-2*t0)*t0*t0;
        double g0=(1-t0)*(1-t0)*t0;
        double g1=(1-t0)*t0*t0;
        double x=f0*this.P[it].getX()+f1*this.P[it+1].getX()+g0*this.Tout[it].getX()-g1*this.Tin[it+1].getX();
        double y=f0*this.P[it].getY()+f1*this.P[it+1].getY()+g0*this.Tout[it].getY()-g1*this.Tin[it+1].getY();
        
        return new Point2D.Double(x,y);
    }
    
    public Vector2D getTangent(double t){
        int it=(int)t;
        if(it<0) it=0;
        if(it>=this.P.length-1) it=this.P.length-2;
        double t0=t-it;
        double df0=-6*(1-t0)*t0;
        double df1=6*(1-t0)*t0;
        double dg0=(1-t0)*(1-3*t0);
        double dg1=(2-3*t0)*t0;
        double Tx=df0*this.P[it].getX()+df1*this.P[it+1].getX()+dg0*this.Tout[it].getX()-dg1*this.Tin[it+1].getX();
        double Ty=df0*this.P[it].getY()+df1*this.P[it+1].getY()+dg0*this.Tout[it].getY()-dg1*this.Tin[it+1].getY();
        return new Vector2D(Tx,Ty);
    }
    
    public void movePoint(int index, Point2D newPoint){
        this.setP(newPoint, index);
        int nseg=this.getNumOfSegments();
        if(this.isClosed()&&(index==0||index==nseg)) {
            this.setP(newPoint, 0);
            this.setP(newPoint, nseg);
        }
        //int it=index;
        int[] its=new int[2];
        int[] ite=new int[2];
        its[0]=-1; 
        ite[0]=index;
        its[1]=index; 
        ite[1]=-1;
        if(index>=1) its[0]=index-1;
        if(index<=nseg-1) ite[1]=index+1;
        if(this.isClosed()&&(index==0||index==nseg)){
            its[0]=nseg-1; ite[0]=nseg;
            its[1]=0; ite[1]=1;
        }
        for(int i=0;i<2;i++){
            if(its[i]<0||ite[i]<0) continue;
            Vector2D[] newTangents=FergusonCurve2D.getOptimizedTangents(this.getP(its[i]), 
                    this.getP(ite[i]), this.getTout(its[i]), this.getTin(ite[i]));
            this.setTout(newTangents[0], its[i]);
            this.setTin(newTangents[1], ite[i]);
        }
    }
    
    public void moveTangent(int index, int inout, Vector2D newTangent){
        if(inout==TIN) this.setTin(newTangent, index);
        else this.setTout(newTangent, index);
        int nseg=this.getNumOfSegments();
        if(this.isClosed()&&(index==0||index==nseg)) {
            if(inout==TIN){
                this.setTin(newTangent, 0);
                this.setTin(newTangent, nseg);
            } else{
                this.setTout(newTangent, 0);
                this.setTout(newTangent, nseg);
            }
        }
        int its, ite;
        if(inout==TIN){
            its=-1; 
            ite=index;
        } else{
            its=index; 
            ite=-1;
        }
        if(inout==TIN&&index>=1) its=index-1;
        if(inout==TOUT&&index<=nseg-1) ite=index+1;
        if(this.isClosed()&&(index==0||index==nseg)){
            if(inout==TIN){
                its=nseg-1; 
                ite=nseg;
            } else {
                its=0; 
                ite=1;
            }
        }
        if(its>=0&&ite>=0) {
            Vector2D[] newTangents=FergusonCurve2D.getOptimizedTangents(this.getP(its), 
                    this.getP(ite), this.getTout(its), this.getTin(ite));
            this.setTout(newTangents[0], its);
            this.setTin(newTangents[1], ite);
            
        }
    }

    public void addPoint(double t){
        int it=(int)t;
        if(it<0||it>=this.P.length-1) {
            System.err.println("*** Error FergusonCurve2D.addPoint ; t out of range; t="+t);
            return;
        }
        Point2D[] newP=new Point2D[this.P.length+1];
        Vector2D[] newTin=new Vector2D[this.P.length+1];
        Vector2D[] newTout=new Vector2D[this.P.length+1];
        for(int i=0;i<this.P.length;i++){
            if(i<=it){
                newP[i]=(Point2D)this.P[i].clone();
                newTin[i]=(Vector2D)this.Tin[i].clone();
                newTout[i]=(Vector2D)this.Tout[i].clone();
            }else{
                newP[i+1]=(Point2D)this.P[i].clone();
                newTin[i+1]=(Vector2D)this.Tin[i].clone();
                newTout[i+1]=(Vector2D)this.Tout[i].clone();
            }
        }
        newP[it+1]=this.getP(t);
        double dt1=t-it;
        double dt2=it+1-t;
        newTin[it+1]=Vector2D.multiply(dt1, this.getTangent(t));
        newTout[it+1]=Vector2D.multiply(dt2, this.getTangent(t));
        newTout[it]=Vector2D.multiply(dt1, newTout[it]);
        newTin[it+2]=Vector2D.multiply(dt2, newTin[it+2]);
        this.P=newP;
        this.Tin=newTin;
        this.Tout=newTout;
    }
    
    public void deletePoint(int index){
        System.out.println(" ** deletePoint "+this.toString());
        int it=index;
        if(it<0||it>=this.P.length) {
            System.err.println("*** Error FergusonCurve2D.deletePoint; it out of range; it="+it);
            return;
        }
        int size=this.P.length;
        Point2D[] newP=new Point2D[size-1];
        Vector2D[] newTin=new Vector2D[size-1];
        Vector2D[] newTout=new Vector2D[size-1];
        if(this.isClosed()&&it==size-1) it=0;
        for(int i=0;i<size;i++){
            if(i<it){
                newP[i]=(Point2D)this.P[i].clone();
                newTin[i]=(Vector2D)this.Tin[i].clone();
                newTout[i]=(Vector2D)this.Tout[i];
            }
            if(i>it){
                newP[i-1]=(Point2D)this.P[i].clone();
                newTin[i-1]=(Vector2D)this.Tin[i].clone();
                newTout[i-1]=(Vector2D)this.Tout[i];
            }
        }
        int np=newP.length;
        if(it!=0&&it!=np) {
            Vector2D[] newTangents=
                    FergusonCurve2D.getOptimizedTangents(newP[it-1], newP[it], newTout[it-1], newTin[it]);
            if(newTangents!=null){
                newTout[it-1]=newTangents[0];
                newTin[it]=newTangents[1];
            }
        }
        if(this.isClosed()&&it==0) {
            newP[np-1]=newP[0];
            newTin[np-1]=(Vector2D)newTout[0].clone();
            Vector2D[] newTangents=
                    FergusonCurve2D.getOptimizedTangents(newP[np-2], newP[np-1], newTout[np-2], newTin[np-1]);
            if(newTangents!=null){
                newTout[np-2]=newTangents[0];
                newTin[np-1]=newTangents[1];
            }
        }
        this.P=newP;
        this.Tin=newTin;
        this.Tout=newTout;
        
    }

    public String toString(){
        String str="";
        int nseg=this.P.length-1;
        str+="Fergusoncurve2D, segments="+nseg+", close="+closed;
        for(int i=0;i<nseg;i++){
            double t=i;
            Point2D p0=this.getP(i);
            Point2D p1=this.getP(i+1);
            Vector2D T0=this.getTout(i);
            Vector2D T1=this.getTin(i+1);
            Vector2D unitChord=Vector2D.unitVector(Vector2D.sub(p1, p0));
            double sprodTout=Vector2D.sproduct(unitChord, Vector2D.unitVector(T0));
            double sprodTin=Vector2D.sproduct(unitChord, Vector2D.unitVector(T1));
            double angleTout=Math.acos(sprodTout)*180/Math.PI;
            double angleTin=Math.acos(sprodTin)*180/Math.PI;
            str+="\n   segment["+i+"]" +
                    "  startP="+Util.Pt(p0)+ ", endP="+Util.Pt(p1)+
                    "  Tout="+Util.Pt(T0)+", angle="+Util.Num(angleTout)+" deg."+
                    ", Tin="+Util.Pt(T1)+", angle="+Util.Num(angleTin)+" deg.";
        }
        return str;
    }

    public static FergusonCurve2D createNaturalSpline(Point2D[] nodePTs){
        if(nodePTs==null||nodePTs.length==0) {
            System.err.println("*** Error in FergusonCurve2D.createNaturalSpline PTs=null");
            return null;
        }
        FergusonCurve2D curve=null;
        Vector2D[] Tin =new Vector2D[nodePTs.length];
        Vector2D[] Tout=new Vector2D[nodePTs.length];
        
        boolean closed=false;
        double dist=Vector2D.dist(nodePTs[0], nodePTs[nodePTs.length-1]);
        if(dist<Curve2D.closedTolerance) {
            closed=true;
            nodePTs[nodePTs.length-1]=nodePTs[0];
        }
        if(nodePTs.length==2){
            Vector2D T=Vector2D.sub(nodePTs[1], nodePTs[0]);
            Tin[0]=new Vector2D(0,0);
            Tout[0]=T;
            Tin[1]=T;
            Tout[1]=new Vector2D(0,0);
        } else{
            Vector2D[] unitTangents=null;
          //-----------------------------------------------------------------------------//  
            if(!closed) unitTangents=FergusonCurve2D.getUnitTangentOfOpenCurve(nodePTs);
            if(closed) unitTangents=FergusonCurve2D.getUnitTangentOfClosedCurve(nodePTs);
          //-----------------------------------------------------------------------------//     
            Tin[0]=new Vector2D(0,0);
            Tout[nodePTs.length-1]=new Vector2D(0,0);
            for(int i=0;i<nodePTs.length-1;i++){
                Vector2D[] Tvecs=FergusonCurve2D.getOptimizedTangents(nodePTs[i], 
                        nodePTs[i+1], unitTangents[i], unitTangents[i+1]);
                Tout[i]=Tvecs[0];
                Tin[i+1]=Tvecs[1];
            }
            if(closed){
                int ie=nodePTs.length-1;
                Tin[0]=Tin[ie];
                Tout[ie]=Tout[0];
            }
        }
        curve=new FergusonCurve2D(nodePTs, Tin, Tout);
        if(debug>0) System.out.println("** createNaturalSpline curve="+curve.toString());
        return curve;
    }
    public static Vector2D[] getUnitTangentOfOpenCurve(Point2D[] nodePTs){
        Vector2D[] unitTangents=new Vector2D[nodePTs.length];
        int length=nodePTs.length;
        double[][] A=new double[length][length];
        double[] bx=new double[length];
        double[] by=new double[length];
        Arc2D[] arcs=Curve2DUtil.createCircularArcs(nodePTs[0], nodePTs[1], nodePTs[2]);
        Vector2D tangent=Vector2D.unitVector(Vector2D.sub(nodePTs[1], nodePTs[0]));
        if(arcs.length>0){
            Segment2D segment=new Segment2D(Segment2D.ARC, arcs[0]);
            tangent=segment.getTangent(0);
            tangent=Vector2D.unitVector(tangent);
        }
        A[0][0]=1.0d;
        A[0][1]=0.0d;
        bx[0]=tangent.getX();
        by[0]=tangent.getY();
        for(int i=1;i<=length-2;i++){
            double c0=Vector2D.dist(nodePTs[i-1], nodePTs[i]);
            double c1=Vector2D.dist(nodePTs[i], nodePTs[i+1]);
            A[i][i-1]=c1;
            A[i][i]=2*(c0+c1);
            A[i][i+1]=c0;
            bx[i]=3/(c0*c1)*(c1*c1*(nodePTs[i].getX()-nodePTs[i-1].getX())+
                    c0*c0*(nodePTs[i+1].getX()-nodePTs[i].getX()));
            by[i]=3/(c0*c1)*(c1*c1*(nodePTs[i].getY()-nodePTs[i-1].getY())+
                    c0*c0*(nodePTs[i+1].getY()-nodePTs[i].getY()));
        }
        int ie=length-1;
        arcs=Curve2DUtil.createCircularArcs(nodePTs[ie-2], nodePTs[ie-1], nodePTs[ie]);
        tangent=Vector2D.unitVector(Vector2D.sub(nodePTs[ie], nodePTs[ie-1]));
        if(arcs.length>0){
            Segment2D segment=new Segment2D(Segment2D.ARC, arcs[1]);
            tangent=segment.getTangent(1);
            tangent=Vector2D.unitVector(tangent);
        }
        A[ie][ie-1]=0.0d;
        A[ie][ie]=1.0d;
        bx[ie]=tangent.getX();
        by[ie]=tangent.getY();
        double[] Tx=Matrix.GaussianElimination(A, bx);
        double[] Ty=Matrix.GaussianElimination(A, by);
        if(debug>0){
            Matrix.print(A,bx,Tx); 
            Matrix.print(A,by,Ty); 
        }
        for(int i=0;i<length;i++) unitTangents[i]=Vector2D.unitVector(new Vector2D(Tx[i],Ty[i]));
        return unitTangents;
    }

    public static Vector2D[] getUnitTangentOfClosedCurve(Point2D[] nodePTs){
        Vector2D[] unitTangents=new Vector2D[nodePTs.length];
        int length=nodePTs.length-1;
        double[][] A=new double[length][length];
        double[] bx=new double[length];
        double[] by=new double[length];
        for(int i=0;i<=length-1;i++){
            int iMinus=i-1;
            if(iMinus<0) iMinus=iMinus+length;
            int iPlus=i+1;
            if(iPlus>=length) iPlus-=length;
            double c0=Vector2D.dist(nodePTs[iMinus], nodePTs[i]);
            double c1=Vector2D.dist(nodePTs[i], nodePTs[i+1]);
            A[i][iMinus]=c1;
            A[i][i]=2*(c0+c1);
            A[i][iPlus]=c0;
            bx[i]=3/(c0*c1)*(c1*c1*(nodePTs[i].getX()-nodePTs[iMinus].getX())+
                    c0*c0*(nodePTs[iPlus].getX()-nodePTs[i].getX()));
            by[i]=3/(c0*c1)*(c1*c1*(nodePTs[i].getY()-nodePTs[iMinus].getY())+
                    c0*c0*(nodePTs[iPlus].getY()-nodePTs[i].getY()));
        }
        double[] Tx=Matrix.GaussianElimination(A, bx);
        double[] Ty=Matrix.GaussianElimination(A, by);
        if(debug>0){
            Matrix.print(A,bx,Tx); 
            Matrix.print(A,by,Ty); 
        }
        for(int i=0;i<length;i++) unitTangents[i]=Vector2D.unitVector(new Vector2D(Tx[i],Ty[i]));
        unitTangents[length]=unitTangents[0];
        return unitTangents;
    }

    public static Vector2D[] getOptimizedTangents(Point2D P0, Point2D P1, Vector2D Tout, 
                                Vector2D Tin){
        double smallAngle=Math.sin(0.1*Math.PI);
        Vector2D[] newTangents=new Vector2D[2];
        newTangents[0]=Tout;
        newTangents[1]=Tin;
        Vector2D[] unitTangents=new Vector2D[2];
        unitTangents[0]=Vector2D.unitVector(Tout);
        unitTangents[1]=Vector2D.unitVector(Tin);
        double chordLen=Vector2D.dist(P0, P1);
        newTangents[0]=Vector2D.multiply(chordLen, unitTangents[0]);
        newTangents[1]=Vector2D.multiply(chordLen, unitTangents[1]);
        double vecterProduct=Vector2D.vproduct(unitTangents[0], unitTangents[1]);
        if(Math.abs(vecterProduct)>smallAngle) {
            Vector2D chord=Vector2D.sub(P1,P0);
            Vector2D unitChord=Vector2D.unitVector(chord);
            for(int i=0;i<2;i++){
                double sprod=Vector2D.sproduct(unitChord, unitTangents[i]);
                double teta=Math.acos(sprod);
                double r=0.5*chordLen/Math.sin(teta);
                double Tlen=4.0d*r*(1-Math.cos(teta))/Math.sin(teta);
                newTangents[i]=Vector2D.multiply(Tlen, unitTangents[i]);
            }
        }
        //System.out.println("-- Out getOptimizedTangents newT[0]="+newTangents[0].toString()+
        //        ", newT[1]="+newTangents[1].toString());
        return newTangents;
    }

}// end of class
