Unit TweeMaal;
{
  Least Squares Minimalization
  for two Point Clouds in 2-D

  sum_k [ (x_k - a)^2 + (y_k - b)^2 ]
        [ (x_k - p)^2 + (y_k - q)^2 ] = min(a,b,p,q)
}
INTERFACE

type
  vector = array[0..3] of double;
var
  x,y : array of double;

procedure dubbelen(N : integer);
procedure allemaal(N : integer; delta : double);
function een_stap(var u : vector; delta : double) : boolean;

IMPLEMENTATION

const
  a = 0; b = 1; p = 2; q = 3;
type
  matrix = array[a..q,a..q] of double;
var
  S : matrix; r : vector;

procedure dubbelen(N : integer);
{
  Random point clouds
}
var
  k,h : integer;
begin
  SetLength(x,N);
  SetLength(y,N);
  h := N div 2;
  for k := 0 to h-1 do
  begin
    x[k] := 1 + Random;
    y[k] := 1 + Random;
  end;
  for k := h to N-1 do
  begin
    x[k] := 2 + Random;
    y[k] := 2 + Random;
  end;
end;

procedure Vullen(u : vector);
{
  Fill matrix and RHS
}
var
  N,k,i,j : integer;
begin
  N := Length(x);
  for i := a to q do
  begin
    for j := a to q do
      S[i,j] := 0;
    r[i] := 0;
  end;
  for k := 0 to N-1 do
    r[a] := r[a] - 2*(x[k]-u[a])*(sqr(x[k]-u[p]) + sqr(y[k]-u[q]));
  for k := 0 to N-1 do
    r[b] := r[b] - 2*(y[k]-u[b])*(sqr(x[k]-u[p]) + sqr(y[k]-u[q]));
  for k := 0 to N-1 do
    r[p] := r[p] - 2*(x[k]-u[p])*(sqr(x[k]-u[a]) + sqr(y[k]-u[b]));
  for k := 0 to N-1 do
    r[q] := r[q] - 2*(y[k]-u[q])*(sqr(x[k]-u[a]) + sqr(y[k]-u[b]));
  for k := 0 to N-1 do
    S[a,a] := S[a,a] + 2*(sqr(x[k]-u[p]) + sqr(y[k]-u[q]));
  S[b,b] := S[a,a];
  for k := 0 to N-1 do
    S[p,p] := S[p,p] + 2*(sqr(x[k]-u[a]) + sqr(y[k]-u[b]));
  S[q,q] := S[p,p];
  S[a,b] := 0; S[b,a] := 0;
  S[p,q] := 0; S[q,p] := 0;
  for k := 0 to N-1 do
    S[a,p] := S[a,p] + 4*(x[k]-u[a])*(x[k]-u[p]);
  S[p,a] := S[a,p];
  for k := 0 to N-1 do
    S[b,p] := S[b,p] + 4*(y[k]-u[b])*(x[k]-u[p]);
  S[p,b] := S[b,p];
  for k := 0 to N-1 do
    S[a,q] := S[a,q] + 4*(x[k]-u[a])*(y[k]-u[q]);
  S[q,a] := S[a,q];
  for k := 0 to N-1 do
    S[b,q] := S[b,q] + 4*(y[k]-u[b])*(y[k]-u[q]);
  S[q,b] := S[b,q];
end;

procedure Druk(u : vector);
begin
  Writeln('(',u[0]:12:9,',',u[1]:12:9,')'
   + '  ;  (',u[2]:12:9,',',u[3]:12:9,')');
end;

procedure Inverteren(var b : matrix);
{
  Direct matrix inversion
  (inefficient but exact)
}
var
  pe : double;
  NDM,i,j,k : integer;
begin
  NDM := Length(b);
  for k := 0 to NDM-1 do
  begin
    pe := b[k,k];
    b[k,k] := 1;
    for i := 0 to NDM-1 do
    begin
      b[i,k] := b[i,k]/pe;
      if (i = k) then Continue;
      for j := 0 to NDM-1 do
      begin
        if (j = k) then Continue;
        b[i,j] := b[i,j] - b[i,k]*b[k,j];
      end;
    end;
    for j := 0 to NDM-1 do
      b[k,j] := - b[k,j]/pe;
    b[k,k] := 1/pe;
  end;
end;

procedure allemaal(N : integer; delta : double);
{
  Altogether
}
var
  h,e : double;
  i,j : integer;
  u : vector;
begin
  dubbelen(N);
  u[a] := 0 ; u[b] := 0;
  u[p] := 4 ; u[q] := 4;
  Druk(u);
{ Newton Raphson iterations }
  while true do
  begin
    Vullen(u);
    Inverteren(S);
    e := 0;
    for i := a to q do
    begin
      h := 0;
      for j := a to q do
        h := h - S[i,j]*r[j];
      u[i] := u[i] + h;
      e := e + sqr(h);
    end;
    Druk(u);
    if sqrt(e) < delta then Break;
  end;
end;

function een_stap(var u : vector; delta : double) : boolean;
{
  One iteration
}
var
  h,e : double;
  i,j : integer;
begin
  Vullen(u);
  Inverteren(S);
  e := 0;
  for i := a to q do
  begin
    h := 0;
    for j := a to q do
      h := h - S[i,j]*r[j];
    u[i] := u[i] + h;
    e := e + sqr(h);
  end;
  Druk(u);
  een_stap := (sqrt(e) < delta);
end;

END.