// TO DO
// resample polyloop !!!
// compute center of mass G and show
// rshift otate zoom wrt G
// CP map

int   _rec=3, _deg=3;                   // number of recursions and B-spline degree
//float _ga=-1.535224,  _gb= 0.410048,  _gs=1.5;    // subdivision parameters
float _ga=0.0,  _gb=0.0,  _gs=0.0;    // subdivision parameters
float [] _w = new float[5];          // weights for retrofitting
boolean symmetryEnds=true;

class Polyloop {             // class of polyloops (closed loop polygon)
  int vn = 5, cap=5000;      // number of control vertices and the cap on vn
  pt[] P = new pt [cap];     // control points
  vec[] L = new vec [cap];     // adjustment vectors
  pt G = new pt(0,0);        // center of mass
  float a = 0;               // angle to axis
  ring [] R = new ring[7];   // 7 rings
  int rc;                     // counter showing the next control point to load in the top rig
  Stepper stepper = new Stepper(_rec);    // stepper for knowing which ring to advance
  int bi=-1;
  boolean openLoop;           // says that curve is 
  int p = 0;                   // id of  point selected with mouse for dragging
  float area; pt lastPt = new pt(0,0), thisPt = new pt(0,0); int ctr;                //used to sum up the area
  
  int ni(int i) {return (i+1)%vn;}
  int pi(int i) {return (i+vn-1)%vn;}
  
  Polyloop () {vn=0; for (int i=0; i<cap; i++) {P[i]=new pt(0,0); L[i]=new vec(0,0);} for(int i=0; i<7; i++) R[i] = new ring(); } 
  void reset (int pvn) {vn=pvn; for (int i=0; i<vn; i++) {P[i].setTo(-sin((i+0.5)*TWO_PI/vn)*(height*0.40)+height/2.0,cos((i+0.5)*TWO_PI/vn)*(height*0.40)+height/2.0);}; }
  void from(int pvn, pt [] Q)  { vn=pvn; for (int i=0; i<vn; i++) P[i].setTo(Q[i]);  };
  void empty()  { vn=0; };
  void appendPt(pt Q)  { P[vn++].setTo(Q);  }; // add point at end of list
  
// DISPLAY 
  pt p(int i) {return(P[i].makeClone());}
  void drawVertices() {if(openLoop) for (int i=2; i<vn-2; i++) P[i].show(2); else for (int i=0; i<vn; i++) P[i].show(2);   };  // draws vertices
  void drawVertexIDs() {for (int i=0; i<vn; i++) {vec V=P[i].makeVecToCenter(P[this.ip(i)],P[this.in(i)]); V.normalize(); V.scaleBy(-10); V.add(-3,5); P[i].showLabel(str(i),V); };}; 
  void drawCurve() {              // draws polyloop curve
    if(openLoop) { beginShape();  for (int i=2; i<vn-2; i++) P[i].v(); endShape(); P[2].show(2); 
       stroke(grey,40); P[0].showSegmentTo(P[1]);  P[1].showSegmentTo(P[2]);  P[vn-1].showSegmentTo(P[vn-2]);  P[vn-2].showSegmentTo(P[vn-3]);  
     }
    else {  beginShape();  for (int i=0; i<vn; i++) P[i].v(); endShape(CLOSE);  };     } 
 
 
// AREA, PROJECTION, DISTANCE, INCLUSION
//  float area () {float A=0; for (int i=0; i<vn-1; i++) A+=(P[i+1].x-P[i].x)*(P[i+1].y+P[i].y)/2. ; A+=(P[0].x-P[vn-1].x)*(P[0].y+P[vn-1].y)/2.; return(A); }
  float length () {float L=0; 
    if (openLoop) for (int i=2; i<vn-3; i++) L+=P[i].disTo(P[this.in(i)]); 
    else for (int i=0; i<vn; i++) L+=P[i].disTo(P[this.in(i)]); 
    return(L); }
  float area () {float A=0; for (int i=0; i<vn; i++) A+=trapezeArea(P[i],P[this.in(i)]); return(A); }
  pt barycenter () {G.setTo(0,0); 
      for (int i=0; i<vn; i++) G.addScaledPt(trapezeArea(P[i],P[this.in(i)]),trapezeCenter(P[i],P[this.in(i)])); 
      G.scaleBy(1./this.area());
      return(G); }
  
  pt project(pt M) {
     int v=0; for (int i=1; i<vn; i++) if (M.disTo(P[i])<M.disTo(P[v])) v=i; 
                                                                               fill(blue); P[v].show(3); noFill();
     int e=-1;
     float d = M.disTo(P[v]);
     for (int i=0; i<vn; i++) if ( M.projectsBetween(P[i],P[this.in(i)])  && (M.disToEdge(P[i],P[this.in(i)])<d)) {e=i; d=M.disToEdge(P[e],P[this.in(e)]);};
     if (e!=-1) return(M.projectOnEdge(P[e],P[this.in(e)])); else return(P[v].makeClone());
     }
  float distance(pt M) {return(M.disTo(this.project(M)));}
  boolean contains (pt M) {boolean isIn=false; for (int i=1; i<vn-1; i++) if (M.isInTriangle(P[0],P[i],P[this.in(i)])) isIn=!isIn; return(isIn); }
  void showAxis() {pt G=this.barycenter(); stroke(black); G.show(10); 
    float xx=0, xy=0, yy=0, px=0, py=0, mx=0, my=0;
    for (int i=0; i<vn; i++) {xx+=(P[i].x-G.x)*(P[i].x-G.x); xy+=(P[i].x-G.x)*(P[i].y-G.y); yy+=(P[i].y-G.y)*(P[i].y-G.y);};
    float a = atan(2*xy/(xx-yy))/2.;
    vec V= new vec(50,0); V.rotateBy(a); vec U = V.makeTurnedLeft();
    for (int i=0; i<vn; i++) {vec W=G.makeVecTo(P[i]); float vd=dot(W,V); float ud=dot(W,U); if(vd>0) px+=vd; else mx-=vd; if(ud>0) py+=ud; else my-=ud; };
    if(mx>max(px,py,my)) V.rotateBy(PI);   if(py>max(px,mx,my)) V.rotateBy(PI/2.);  if(my>max(px,mx,py)) V.rotateBy(-PI/2.); 
    strokeWeight(1); stroke(black); V.showArrowAt(G);  
    stroke(black,60); V.turnLeft(); V.showAt(G); V.turnLeft(); V.showAt(G); V.turnLeft(); V.showAt(G);
    }
  void updateGa() {G=this.barycenter();
    float xx=0, xy=0, yy=0, px=0, py=0, mx=0, my=0;
    for (int i=0; i<vn; i++) {xx+=(P[i].x-G.x)*(P[i].x-G.x); xy+=(P[i].x-G.x)*(P[i].y-G.y); yy+=(P[i].y-G.y)*(P[i].y-G.y);};
    a = atan(2*xy/(xx-yy))/2.;
    vec V= new vec(50,0); V.rotateBy(a); vec U = V.makeTurnedLeft();
    for (int i=0; i<vn; i++) {vec W=G.makeVecTo(P[i]); float vd=dot(W,V); float ud=dot(W,U); if(vd>0) px+=vd; else mx-=vd; if(ud>0) py+=ud; else my-=ud; };
    if(mx>max(px,py,my)) a+=PI;   if(py>max(px,mx,my)) a+=PI/2.;  if(my>max(px,mx,py)) a-=PI/2.; 
    }
  void showGa() { G.show(10); vec V= new vec(50,0); V.rotateBy(a); V.showArrowAt(G);  
    V.turnLeft(); V.showAt(G); V.turnLeft(); V.showArrowAt(G); V.turnLeft(); V.showAt(G);
    }

// EDIT
  void pickClosestVertex(pt M) {p=0; for (int i=1; i<vn; i++) if (M.disTo(P[i])<M.disTo(P[p])) p=i; }
  void dragVertex(vec V) {P[p].translateBy(V); if(openLoop) this.adjustOpenEnds();}
  void delete() { for (int i=p; i<vn-1; i++) P[i].setTo(P[this.in(i)]); vn--; p=this.ip(p);}
  void grabInsert(pt M ) {                // grabs closeest vertex or adds vertex at closest edge. It will be dragged by te mouse
      p=0; for (int i=1; i<vn; i++) if (M.disTo(P[i])<M.disTo(P[p])) p=i; 
     int e=-1;
     float d = M.disTo(P[p]);
     for (int i=0; i<vn; i++) 
        if ( (0.25<M.ratioOfProjectionBetween(P[i],P[this.in(i)])) && (M.ratioOfProjectionBetween(P[i],P[this.in(i)])<0.75) && (M.disToEdge(P[i],P[this.in(i)])<d))
          {e=i; d=M.disToEdge(P[e],P[this.in(e)]);};
      if (e!=-1) { for (int i=vn-1; i>e; i--) P[i+1].setTo(P[i]); vn++; p=this.in(e); P[p].setTo(mouseX,mouseY);  };
      }
  void closeLoop() {openLoop=false; vn-=4;  for (int i=0; i<vn; i++) { P[i].setTo(P[i+2]);}; p=0; } 
  void openLoop() { openLoop=true; for (int i=vn-1; 0<=i; i--) P[i+2].setTo(P[i]); vn+=4; p=2;}   
  void adjustOpenEnds() {
    pt P0= End0(P[2],P[this.in(2)],P[this.in(this.in(2))]);
    pt P1= End1(P[2],P[this.in(2)],P[this.in(this.in(2))]);
    pt Pn= End1(P[vn-3],P[this.ip(vn-3)],P[this.ip(this.ip(vn-3))]);
    pt Pnn=End0(P[vn-3],P[this.ip(vn-3)],P[this.ip(this.ip(vn-3))]);
    P[0].setTo(P0); P[1].setTo(P1);   P[vn-2].setTo(Pn); P[vn-1].setTo(Pnn); 
     }
  void frame() {
     float sx=height; float sy=height; float bx=0.0; float by=0.0; 
     for (int i=0; i<vn; i++) {if (P[i].x>bx) {bx=P[i].x;}; if (P[i].x<sx) {sx=P[i].x;}; if (P[i].y>by) {by=P[i].y;}; if (P[i].y<sy) {sy=P[i].y;}; };
     float m=max(bx-sx,by-sy);  float dx=(m-(bx-sx))/2; float dy=(m-(by-sy))/2; 
     for (int i=0; i<vn; i++) {P[i].x=(P[i].x-sx+dx)*4*height/5/m+height/10;  P[i].y=(P[i].y-sy+dy)*4*height/5/m+height/10;};    }   
  void zoom(float s) {for (int i=0; i<vn; i++) P[i].translateTowards(s, new pt(mouseX,mouseY));};   
  void scaleBy(float s) {for (int i=0; i<vn; i++) P[i].translateTowards(s,P[p]);};   
  void scaleByG(float s) {for (int i=0; i<vn; i++) P[i].translateTowards(s,G);};   
  void translateBy(vec V) {for (int i=0; i<vn; i++) P[i].translateBy(V); G.translateBy(V); };   
  void rotateBy(float aa) {for (int i=0; i<vn; i++) P[i].rotateBy(aa,P[p]);}; 
  void rotateBy(float aa, pt Q) {for (int i=0; i<vn; i++) P[i].rotateBy(aa,Q);}; 
  void rotateByG(float aa) {for (int i=0; i<vn; i++) P[i].rotateBy(aa,G);}; 
  void move(pt A, vec V) {
     vec U = G.makeVecTo(A); float d = U.norm(); 
     if (d<5) this.translateBy(V);
     else {
        U.normalize(); 
        vec W = U.makeScaledBy(dot(U,V));  this.translateBy(W); 
        pt C = G.makeTranslatedBy(-sq(height/8)/d,U); stroke(cyan); C.show(3);
        float aa = angle(C.makeVecTo(A).makeOffsetVec(V),C.makeVecTo(A)); this.rotateBy(aa,C);
       };
     }

  void align() {for (int i=0; i<vn; i++) {
    if ( abs(P[in(i)].x-P[i].x)>abs(P[in(i)].y-P[i].y)) {float dy=(P[in(i)].y-P[i].y)/2; P[in(i)].y-=dy; P[i].y+=dy;}
    else {float dx=(P[in(i)].x-P[i].x)/2; P[in(i)].x-=dx; P[i].x+=dx;};   }; } 

// INDICES
  int in(int j) {  if (j==vn-1) {return (0);}  else {return(j+1);}  };  // next vertex in loop
  int ip(int j) {  if (j==0) {return (vn-1);}  else {return(j-1);}  };  // previous vertex in loop                                                     

// ARCHIVAL ON FILE
 void savePts() {this.savePts(0);}
 void loadPts() {this.loadPts("C0.pts");}
 void savePts(int n) { String [] inppts = new String [vn+1];
    int s=0; inppts[s++]=str(vn); for (int i=0; i<vn; i++) {inppts[s++]=str(P[i].x)+","+str(P[i].y);};
    saveStrings("C"+str(n)+".pts",inppts);  };
  void loadPts(String fn) { String [] ss = loadStrings(fn);
    String subpts;
    int s=0; int comma; vn = int(ss[s]);
    for(int i=0;i<vn; i++) { comma=ss[++s].indexOf(',');
      P[i]=new pt (float(ss[s].substring(0, comma)), float(ss[s].substring(comma+1, ss[s].length()))); }; };

// WITH OTHER POLYLOOPS 
  void setTo(Polyloop Q)  {vn=Q.vn; for (int i=0; i<vn; i++) P[i].setTo(Q.P[i]);  openLoop=Q.openLoop;};
  void blendTowards(Polyloop Q, float s)  {        // blend towards Q by ratio s
     if (Q.vn>=vn) for (int i=0; i<vn; i++) P[i].translateTowards(s,Q.P[i]);  };
  
  void setAsRefined(Polyloop C) {C.loadRing(); C.deriveRings(); 
   vn=0;
   if(C.openLoop) {C.next();  for (int j=0; j<=(C.vn-5)*int(pow(2,_rec)); j++) P[vn++].setTo(C.next()); this.openLoop();  }
   else  for (int j=0; j<C.vn*int(pow(2,_rec)); j++) P[vn++].setTo(C.next());
   this.shiftIDs(int(pow(2,_rec+1))-1); 
    }  
 
  void setAsResampled(Polyloop C, int nrv) {
    float L = C.length();                              
    float d = L / nrv;                          
    float rd=d;  // remaining distance to next sample
    float cl=0;  // length of remaining portion of current edge
    int k=0,nk;
    pt Q = new pt(0,0);
     vn=0;
     if (C.openLoop) k=2;  
     Q.setTo(C.P[0]);             //   stroke(green,50);    Q.show(d/2);
     P[vn++].setTo(Q);        
     while (vn<nrv) {
       nk=C.in(k);
       cl=Q.disTo(C.P[nk]);                            
       if (rd<cl) {Q.translateTowardsBy(rd,C.P[nk]); //  stroke(green,50); Q.show(d/2); 
     P[vn++].setTo(Q); cl-=rd; rd=d;         }
        else {rd-=cl; Q.setTo(C.P[nk]); k++;       };
     };
   }
   
  void setToAverage() {
    vn=CC[0].vn; for (int i=0; i<vn; i++) P[i].setTo(0,0); 
    for (int j=0; j<10; j++) for (int i=0; i<vn; i++) P[i].addPt(CC[j].P[i]);
    for (int i=0; i<vn; i++) P[i].scaleBy(0.10); 
    }

// BSPLINE SUBDIVISOIN
  void bspline(int deg, int rec) { 
    for(int r=0; r<rec; r++) {this.refine(); for(int d=0; d<deg-1; d++) this.dual(); for(int d=0; d<int(deg/2); d++) this.shiftIDs();
  if(openLoop) { vn-=2;}; };
     }
  void shiftIDs() { pt P0=P[vn-1].makeClone(); for (int i=vn-1; i>0; i--) P[i].setTo(P[i-1]);  P[0].setTo(P0); };  // shits vetex table
  void shiftIDs(int n) {for (int j=0; j<n; j++) { pt P0=P[vn-1].makeClone(); for (int i=vn-1; i>0; i--) P[i].setTo(P[i-1]);  P[0].setTo(P0); };};  // shifts vetex table
  void dual() {this.refine(); this.coarsen();}
  void coarsen() {vn/=2; for (int i=0; i<vn; i++) P[i].setTo(P[2*i+1]); } 
  void refine() {
    for (int i=vn-1; 0<=i; i--) P[2*i].setTo(P[i]);  vn*=2;     
    for (int i=1; i<vn-1; i+=2) P[i].setTo(average(P[i-1],P[i+1]));  
    P[vn-1].setTo(average(P[vn-2],P[0])); } 
    
 // Js REFINEMENT 
 void refine(float s) { this.refine(s,s);}
 void refine(float a, float b) { 
      pt[] Q = new pt [2*vn];     
      for (int i=0; i<vn; i++) { Q[2*i]= B(P[ip(i)],P[i],P[in(i)],a); Q[2*i+1]=F(P[ip(i)],P[i],P[in(i)],P[in(in(i))],b); };
      vn*=2; for (int i=0; i<vn; i++) P[i].setTo(Q[i]);
     }

// RINGING SUBDIVISION
  void loadRing() {stepper.reset(); R[0].reset(); rc=0; for(int i=0; i<5; i++) {R[0].push(P[rc]); rc=this.in(rc); }; }     // pushes first 5 points to top ring
  void deriveRings() {R[0].reset(); for (int r=1; r<=_rec; r++) {float a=_gs, b=_gs;  if (r==1) {a=_ga; b=_gb;}; R[r].derive(R[r-1],a,b);}; }  // derive all other rings
  void showRefined() {this.loadRing(); this.deriveRings(); 
     if(openLoop) { beginShape(); this.next(); for (int j=0; j<=(vn-5)*int(pow(2,_rec)); j++) this.next().v(); endShape(); }
     else { beginShape(); for (int j=0; j<vn*int(pow(2,_rec)); j++) this.next().v(); endShape(CLOSE);  this.next().v(); } 
  }  // showRefined 
  void pushRing() {R[0].push(P[rc]); rc=this.in(rc);}                                                                  // pushes the next control point to the top ring
  void showRing(int r) {R[r].show();}                                                          // shows 5 points of ring (for demonstration only)
  void f(int r) {float a=_gs, b=_gs;  if (r==1) {a=_ga; b=_gb;}; R[r].push(R[r-1].f23(b));}        // pushes ring r with the f23 of parent ring
  void b(int r) {float a=_gs, b=_gs;  if (r==1) {a=_ga; b=_gb;}; R[r].push(R[r-1].b3(a));}          // pushes ring r with the b3 of parent ring
  pt next() {int level=_rec-stepper.next(); if(level==0) this.pushRing();      // advances last ring by one point along curve 
                                            else this.b(level); for (int r=level+1; r<=_rec; r++) this.f(r); return(R[_rec].pt());}    
                                            
 // RETROFITTING
 void  setAsLimitOf (Polyloop R) {vn=R.vn; for (int i=0; i<vn; i++) P[i].setTo(R.limitOfVertex(i)); }
 pt limitOfVertex (int k) {
   stepper.reset(); R[0].reset(); rc=this.ip(this.ip(k)); for(int i=0; i<5; i++) {R[0].push(P[rc]); rc=this.in(rc); };  
   this.deriveRings();
   return(R[_rec].c().makeClone());
   }
 void retrofit(Polyloop C, Polyloop S) { for (int i=0; i<vn; i++) P[i].translateBy(S.p(i).makeVecTo(C.p(i))); };

// SMOOTHING
 void computeL2x() {for (int i=0; i<vn; i++) L[i]=P[i].makeVecTo(average(P[pi(i)],P[ni(i)]).makeTranslatedBy( biL(P[pi(pi(i))],P[pi(i)],P[ni(i)],P[ni(ni(i))]) ) );};
 void computeL2() {for (int i=0; i<vn; i++) L[i]=cubic(P[pi(pi(i))],P[pi(i)],P[i],P[ni(i)],P[ni(ni(i))]);};
 void computeL() {for (int i=0; i<vn; i++) L[i]=P[i].makeVecTo(average(P[pi(i)],P[ni(i)]) ).makeScaledBy(0.5);};
 void applyL() {for (int i=0; i<vn; i++) P[i].translateBy(0.5,L[i]);};
 void applyLhere(float roi) {for (int i=0; i<vn; i++) if (P[i].disToMouse()<roi) P[i].translateBy(0.5,L[i]);};
 void applyLaround(float roi) {for (int i=0; i<vn; i++) if ((i!=p)&&(P[i].disToMouse()<roi)) P[i].translateBy(0.5,L[i]);};
 void showL() {for (int i=0; i<vn; i++) L[i].showAt(P[i]); }; 
   

 } // end polyloop class

class Stepper {                          // stepper for the rings
  boolean [] B = new boolean [10];       // Boolean flags and number of recursions  
  int d=0;
  Stepper (int pd) {d=pd; this.reset();};
  void reset() {for(int i=0; i<d; i++) B[i]=true; d=_rec;}
  int next() {int c=0; while(B[c]&&(c<d)) {B[c]=false; c++;}; B[c]=true; return(c); } // returns ID of ring that should do a b3 step
  }
int n(int c) {return((c+1)%5);}  int p(int c) {return((c+4)%5);}                                      // next and previous in ring

class ring {                                 // ring for traversing refined curves
    pt[] P = new pt[5];                      // a FIFO of 5 points {A,B,C,D,E}
    int c=2;                                 // index of middle point C  (it is rotated at each step to avoid copying points)
    ring () {for (int i=0; i<5; i++) P[i]=new pt(0,0);};
  void push (pt F) {c=n(c); P[n(n(c))]=F.makeClone();}            // loads new point and advances index
  void reset() {c=2;};
  pt c() {return(P[c].makeClone());}
  pt pt() {return(P[n(n(n(c)))].makeClone());}
  pt b1(float s)  {pt bb = b( P[p(p(c))],P[p(c)],P[c],                 s); return(bb); }      // b for second vertex
  pt f12(float s) {pt bb = f(P[p(p(c))], P[p(c)],P[c],      P[n(c)],   s); return(bb); }      // f for second mid-edge
  pt b2(float s)  {pt bb = b(P[p(c)],    P[c],   P[n(c)],              s); return(bb); }      // b for third vertex
  pt f23(float s) {pt bb = f(P[p(c)],    P[c],   P[n(c)],   P[n(n(c))],s); return(bb); }      // f for fourth mid-edge
  pt b3(float s)  {pt bb = b(P[c],       P[n(c)],P[n(n(c))],           s); return(bb); }      // b for fifth vertex
  void derive(ring Q, float a, float b) {c=2; P[0]=Q.b1(a); P[1]=Q.f12(b); P[2]=Q.b2(a); P[3]=Q.f23(b); P[4]=Q.b3(a);} // makes ring from parent ring
  void show() {beginShape(); int b=p(p(c)); for(int i=0; i<5; i++) {P[b].v(); b=n(b);}; endShape(); for(int i=0; i<5; i++) P[i].show(6);} // show ring
  }
pt   End1(pt P0, pt P1, pt P2) {
   pt P = new pt(); 
   float s=_gs;
   if (symmetryEnds) {P.setTo(P0); P.translateBy(P1.makeVecTo(P0));}
   else if ((_gs==_ga)&&(_gs==_gb)) {P=weightedSum( (9.-s)/4.,P0, (s-3.)/2.,P1, (1.-s)/4.,P2);}
   else {
     float a = _ga, a2=sq(a),  s2=sq(s), as=a*s, a2s=a2*s, as2=a*s2, s3=s2*s;
     float D = 144*a-108*a2   -16*s -16*as  +3*a2s   -4*s2  - 2*as2  - s3 ;  
     P= weightedSum(
         (288*a -216*a2  - 84*as  + 27*a2s   - 20*s2  + 6*as2   - s3)/D , P0,   
           (-144*a+ 108 *a2 - 48*s+ 120*as  - 45*a2s + 28*s2 - 18*as2 - s3) /D , P1,   
          (32*s-52*as + 21*a2s   - 12*s2  + 10*as2   +s3) /D , P2);    
     } 
   return(P);
   }
   
pt   End0(pt P0, pt P1, pt P2) {
   pt P = new pt(); 
   float s=_gs;
  if (symmetryEnds) {P.setTo(P0); P.translateBy(P1.makeVecTo(P0)); P.translateBy(P2.makeVecTo(P1));}
   else if ((_gs==_ga)&&(_gs==_gb)) {P= weightedSum( (12.-s)/2.,P0, (s-8.),P1, (6.-s)/2.,P2);}
   else {
      float a = _ga, a2=sq(a), s2=sq(s), as=a*s, a2s=a2*s, as2=a*s2, s3=s2*s;
     float D = 144*a-108*a2   -16*s -16*as  +3*a2s   -4*s2  - 2*as2  - s3 ; 
     P= weightedSum(
         (576*a -432*a2  +192*s  -336*as  + 54*a2s   - 64*s2  + 12*as2   - 2*s3)/D , P0,
           (-576*a + 432 *a2 - 448*s + 608*as  - 96*a2s   + 112*s2  - 32*as2 ) /D , P1,
          (144*a - 108*a2  + 240*s - 288*as  + 45*a2s   - 52*s2  + 18*as2   +s3) /D , P2);
   }
 return(P);
 }