% main1.m Multi-polynomial fit
% read data
xy;  N = max(size(XY));
% plot given points
ax = ppoints(XY)
% choose degrees of polynomials, assume all > 0
p=3; q=3; r=3;
% choose continuous derivatives (1=yes, 0=no)
derivatives = 1
% initialize break points:  xi < eta
%  xi0 = 7.34;  eta0 = 7.78;
xi0 =  6.4;  eta0 = 8;
% first partition of data
xi = xi0; eta = eta0;
[n1,n2,xf,yf,xg,yg,xh,yh] = partition(xi,eta,XY,N);
Jf = van(p,xf);   Jg = van(q,xg);   Jh = van(r,xh);
% initialize polynomial coefficients by least squares
a = polyfit(xf, yf,p); b = polyfit(xg,yg,q); c = polyfit(xh,yh,r);
% plot initial approximation functions
pfuncs(xi,eta,a,b,c, ax);
% initialize loop counter
k = 0;
% initialize corrections for the unknowns
dp = 1;
while (norm(dp) > 1e-5) & (k<40),
   k= k+1;
   JF = [dirsum(Jf,Jg,Jh), zeros(N,2)];
   gradfxi = van(p,xi);  gradgxi = van(q,xi);
   gradgeta = van(q,eta);  gradheta = van(r,eta);
   % coefficients of the first derivatives
   da = dpoly(p,a); db = dpoly(q,b); dc = dpoly(r,c);

   % linear constraints for continuity
   JG = [gradfxi -gradgxi zeros(size(gradheta)) ...
           polyval(da,xi)-polyval(db,xi) 0
         zeros(size(gradfxi)) gradgeta -gradheta ...
           0 polyval(db,eta)-polyval(dc,eta)]

   % continuity of first derivatives
   if derivatives,
      gradfsxi =[[p:-1:1].*van(p-1,xi),0];
      gradgsxi = [[q:-1:1].*van(q-1,xi),0];
      gradgseta = [[q:-1:1].*van(q-1,eta),0];
      gradhseta = [[r:-1:1].*van(r-1,eta),0];
      % coefficients of second derivatives
      dda = dpoly(p-1,da);
      ddb = dpoly(q-1,db);
      ddc = dpoly(r-1,dc);
      % add linear constraints for continuity of first derivatives
      JG = [JG
            gradfsxi -gradgsxi zeros(size(gradhseta))...
              polyval(dda,xi)-polyval(ddb,xi) 0
            zeros(size(gradfsxi)) gradgseta -gradhseta ...
              0 polyval(ddb,eta)-polyval(ddc,eta)]
   end % if derivatives

   % Right hand side for least squares problem
   z = [ yf-polyval(a,xf);yg-polyval(b,xg);yh-polyval(c,xh)]
   % Right hand side for constraints
   mG = -[polyval(a,xi)-polyval(b,xi)
          polyval(b,eta)-polyval(c,eta)]
   % for continuity of first derivatives update right hand side
   if derivatives,
      mG = -[-mG; polyval(da,xi)-polyval(db,xi)
             polyval(db,eta)-polyval(dc,eta)]
   end

   % solve the constrained least squares problems for corrections
   dp = lsqlce(JF,JG,z,mG);

   %  update unknowns
   a = a + dp(1:p+1)';    b = b + dp(p+2:p+q+2)';
   c = c + dp(p+q+3:p+q+r+3)';
   xi = xi + dp(p+q+r+4);   eta = eta + dp(p+q+r+5);

   if xi>eta,  error('xi > eta');   end
   % print norm of correction vector
   norm(dp)
   % plot current approximation functions
   pfuncs(xi,eta,a,b,c, ax);
   pause
   % new partition of the data
   [n1,n2,xf,yf,xg,yg,xh,yh] = partition(xi,eta,XY,N);
   Jf = van(p,xf);   Jg = van(q,xg);   Jh = van(r,xh);
end % while

% plot final result
ax = ppoints(XY); pfuncs(xi,eta,a,b,c, ax);
% compute residual and print results
rf = norm(polyval(a,xf)-XY(1:n1,2));
rg = norm(polyval(b,xg)-XY(n1+1:n2,2));
rh = norm(polyval(c,xh)-XY(n2+1:N,2));
% residual
rr = norm([rf, rg, rh])
% norm of last correction
dpnorm = norm(dp)
% degrees of polynomials, derivative (yes=1), # of iterations
[p q r derivatives k]
% breakpoints: initial, final
[xi0 eta0 xi eta]


