////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2001-2023 Alexey Kuryakin daqgroup@mail.ru under MIT license //
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// This file is part of the CRW-DAQ project by DaqGroup - component CRWLIB.   //
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Purpose:                                                                   //
// Solve linear equations, find least squares polinoms.                       //
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// History:                                                                   //
// 20011211 - Creation (uses CRW16) & test                                    //
// 20030328 - Struggle for safety (add some try/except checks)...             //
// 20230505 - Modified for FPC (A.K.)                                         //
////////////////////////////////////////////////////////////////////////////////

unit _crw_lsqpoly; // Least SQuare POLYnoms

{$I _crw_sysdef}

{$IFDEF FPC}{$mode Delphi}{$ENDIF}{$H+}

{$WARN 5023 off : Unit "$1" not used in $2}

interface

uses
 _crw_sharemem, // NB: THIS UNIT MUST BE FIRST !!!
 {$IFDEF UNIX} cthreads, dl, {$ENDIF}
 {$IFDEF WINDOWS} windows, {$ENDIF}
 sysutils, classes, math,
 _crw_alloc, _crw_fpu;

 {
 Default precision uses for least square equations.
 }
const
 LsqPolyEps = 1e-15;

 {
 *******************************************************************************
 Purpose:
  Solve square linear N*N set of equations A*X=Y by Gauss elimination with
  partial pivoting.
 Result:
  0 - Ok.
  1 - Invalid dimension N or M.
  2 - Matrix is singular, diagonal elements <= Eps.
 Note:
  1) Matrix are in one-dimension array A[i,j]<->A[i*M+j], i,j=0..N-1
  2) Matrix A and vector Y will be changed after call.
 *******************************************************************************
 }
function Partial_Pivoting(N,M:Integer; var A,X,Y:array of Double;
                          Eps:Double=LsqPolyEps):Integer;

 {
 *********************************************
 Least squares polynoms uses for calibrations.
 *********************************************
 }
type
 TPolynomPower = 0..10;
 TPolynom = class(TMasterObject)
 private
  myPower  : TPolynomPower;
  myCoeff  : packed array[TPolynomPower] of Double;
  myCenter : Double;
  myScale  : Double;
  function    GetPower:TPolynomPower;
  procedure   SetPower(aPower:TPolynomPower);
  function    GetCoeff(aIndex:TPolynomPower):Double;
  procedure   SetCoeff(aIndex:TPolynomPower; aValue:Double);
  function    GetCenter:Double;
  procedure   SetCenter(aCenter:Double);
  function    GetScale:Double;
  procedure   SetScale(aScale:Double);
 public
  constructor Create(aPower  : TPolynomPower = 1;
                     aCenter : Double        = 0;
                     aScale  : Double        = 1);
  destructor Destroy; override;
 public
  function   Get(at:Double; aDerivNum:Integer=0):Double;
  function   Find(const X,Y:array of Double; NumPoints:Integer; Eps:double=LsqPolyEps):boolean;
  function   FindWeight(const X,Y,W:array of Double; NumPoints:Integer; Eps:double=LsqPolyEps):boolean;
 public
  property   Power:TPolynomPower                read GetPower  write SetPower;
  property   Coeff[aIndex:TPolynomPower]:Double read GetCoeff  write SetCoeff; default;
  property   Center:Double                      read GetCenter write SetCenter;
  property   Scale:Double                       read GetScale  write SetScale;
 end;

function NewPolynom(aPower  : TPolynomPower = 1;
                    aCenter : Double        = 0;
                    aScale  : Double        = 1):TPolynom;
function NewPolynomCopy(aPolynom : TPolynom = nil):TPolynom;
procedure Kill(var TheObject:TPolynom); overload;

implementation

 {
 *******************************
 Partial_Pivoting implementation
 *******************************
 }
function Partial_Pivoting(N,M:Integer; var A,X,Y:array of Double; Eps:Double):Integer;
var i,j,k,p,pM,iM,jM,iMk,pMk,jMk:Integer; S:Double;
begin
 if (N<1) or (N>M) then begin  {Bad dimension?}
  Result:=1;
  exit;
 end;
 if N=1 then begin             {Special case:N=1?}
  if abs(A[0])<=Eps then       {Division by zero?}
   Result:=2
  else begin                   {Ok}
   X[0]:=Y[0]/A[0];
   Result:=0;
  end;
  exit;
 end;
 for j:=0 to N-2 do begin
  p:=j;
  for i:=j+1 to N-1 do if ABS(A[i*M+j])>ABS(A[p*M+j]) then p:=i;
  jM:=j*M;
  pM:=p*M;
  if p <> j then begin
   for k:=0 to N-1 do begin
    pMk:=pM+k;
    jMk:=jM+k;
    S:=A[pMk];
    A[pMk]:=A[jMk];
    A[jMk]:=S;
   end;
   S:=Y[p];
   Y[p]:=Y[j];
   Y[j]:=S;
  end;
  S:=A[jM+j];
  if abs(S)<=Eps then begin {Diagonal element=0?}
   Result:=2;
   exit;
  end;
  for i:=j+1 to N-1 do begin
   iM:=i*M;
   S:=-A[iM+j]/A[jM+j];
   for k:=0 to N-1 do begin
    iMk:=iM+k;
    A[iMk]:=A[iMk]+S*A[jM+k];
   end;
   Y[i]:=Y[i]+S*Y[j];
  end;
 end;
 S:=A[(N-1)*M+N-1];
 if abs(S)<Eps then begin {Diagonal element=0?}
  Result:=2;
  exit;
 end;
 for i:=N-1 downto 0 do begin
  S:=0;
  iM:=i*M;
  for j:=i+1 to N-1 do S:=S+A[iM+j]*X[j];
  X[i]:=(Y[i]-S)/A[iM+i];
 end;
 Result:=0;            {Ok}
end;

 {
 ************************************
 Internally uses by TPolynom utilites
 ************************************
 }
procedure CreateNormalLeastSquareEquations(const X         : array of Double;
                                           const Y         : array of Double;
                                                 NumPoints : Integer;
                                                 Center    : Double;
                                                 Scale     : Double;
                                                 N         : Integer;
                                                 M         : Integer;
                                             var A         : array of Double;
                                             var P         : array of Double;
                                             var Q         : array of Double);
var i,j,k,jM:Integer; Pw,t,Yi:Double;
begin
 jM:=0;
 for j:=0 to N-1 do begin
  Q[j]:=0;
  for k:=0 to N-1 do A[jM+k]:=0;
  jM:=jM+M;
 end;
 for i:=0 to NumPoints-1 do begin
  t:=(X[i]-Center)/Scale;
  Pw:=1;
  for j:=0 to N-1 do begin
   P[j]:=Pw;
   Pw:=Pw*t;
  end;
  jM:=0;
  for j:=0 to N-1 do begin
   for k:=0 to N-1 do A[jM+k]:=A[jM+k]+P[j]*P[k];
   jM:=jM+M;
  end;
  Yi:=Y[i];
  for j:=0 to N-1 do Q[j]:=Q[j]+Yi*P[j];
 end;
end;

procedure CreateWeightNormalLeastSquareEquations(const X         : array of Double;
                                                 const Y         : array of Double;
                                                 const W         : array of Double;
                                                       NumPoints : Integer;
                                                       Center    : Double;
                                                       Scale     : Double;
                                                       N         : Integer;
                                                       M         : Integer;
                                                   var A         : array of Double;
                                                   var P         : array of Double;
                                                   var Q         : array of Double);
var i,j,k,jM:Integer; Pw,t,Yi,Weight:Double;
begin
 jM:=0;
 for j:=0 to N-1 do begin
  Q[j]:=0;
  for k:=0 to N-1 do A[jM+k]:=0;
  jM:=jM+M;
 end;
 for i:=0 to NumPoints-1 do begin
  Weight:=abs(W[i]);
  t:=(X[i]-Center)/Scale;
  Pw:=1;
  for j:=0 to N-1 do begin
   P[j]:=Pw;
   Pw:=Pw*t;
  end;
  jM:=0;
  for j:=0 to N-1 do begin
   for k:=0 to N-1 do A[jM+k]:=A[jM+k]+P[j]*P[k]*Weight;
   jM:=jM+M;
  end;
  Yi:=Y[i];
  for j:=0 to N-1 do Q[j]:=Q[j]+Yi*P[j]*Weight;
 end;
end;

 {
 ***********************
 TPolynom implementation
 ***********************
 }
constructor TPolynom.Create(aPower  : TPolynomPower = 1;
                            aCenter : Double        = 0;
                            aScale  : Double        = 1);
begin
 inherited Create;
 myPower:=max(0,min(High(myCoeff),aPower));
 SafeFillChar(myCoeff,sizeof(myCoeff),0);
 myCenter:=aCenter;
 myScale:=abs(aScale)+ord(aScale=0);
 Exceptions:=false;
end;

destructor TPolynom.Destroy;
begin
 inherited Destroy;
end;

function TPolynom.GetPower:TPolynomPower;
begin
 Result:=0;
 if Assigned(Self) then Result:=myPower;
end;

procedure TPolynom.SetPower(aPower:TPolynomPower);
begin
 if Assigned(Self) then myPower:=max(0,min(High(myCoeff),aPower));
end;

function  TPolynom.GetCoeff(aIndex:TPolynomPower):Double;
begin
 Result:=0;
 if Assigned(Self) and (Integer(aIndex)>=Low(myCoeff)) and (Integer(aIndex)<=High(myCoeff))
 then Result:=myCoeff[aIndex];
end;

procedure TPolynom.SetCoeff(aIndex:TPolynomPower; aValue:Double);
begin
 if Assigned(Self) and (Integer(aIndex)>=Low(myCoeff)) and (Integer(aIndex)<=High(myCoeff))
 then myCoeff[aIndex]:=aValue;
end;

function TPolynom.GetCenter:Double;
begin
 Result:=0;
 if Assigned(Self) then Result:=myCenter;
end;

procedure TPolynom.SetCenter(aCenter:Double);
begin
 if Assigned(Self) then myCenter:=aCenter;
end;

function TPolynom.GetScale:Double;
begin
 Result:=0;
 if Assigned(Self) then Result:=myScale;
end;

procedure TPolynom.SetScale(aScale:Double);
begin
 if Assigned(Self) then myScale:=abs(aScale)+ord(aScale=0)
end;

function TPolynom.Get(at:Double; aDerivNum:Integer=0):Double;
var t,fact:Double; i,j:Integer;
begin
 Result:=0;
 if Assigned(Self) then begin
  t:=(at-myCenter)/myScale;
  case aDerivNum of
   0:
    for i:=myPower downto 0 do Result:=Result*t+myCoeff[i];
   1:
    begin
     for i:=myPower-1 downto 0 do Result:=Result*t+myCoeff[i+1]*(i+1);
     Result:=Result/myScale;
    end;
   2..High(myCoeff):
    begin
     for i:=myPower-aDerivNum downto 0 do begin
      fact:=myCoeff[i+aDerivNum];
      for j:=1 to aDerivNum do fact:=fact*(i+j);
      Result:=Result*t+fact;
     end;
     fact:=1/myScale;
     for i:=1 to aDerivNum do Result:=Result*fact;
    end;
  end;
 end;
end;

function TPolynom.Find(const X,Y:array of Double; NumPoints:Integer; Eps:double=LsqPolyEps):boolean;
var  A,P,Q:PDoubleArray; t:Double; N,M,i:Integer;
begin
 Result:=false;
 if Assigned(Self) and (@X<>nil) and (@Y<>nil) and (NumPoints>myPower) then
 try
  if myPower=0 then begin
   t:=0;
   for i:=0 to NumPoints-1 do t:=t+Y[i];
   myCoeff[0]:=t/NumPoints;
   Result:=true;
  end else begin
   N:=myPower+1;
   M:=N;
   A:=Allocate(N*M*sizeof(Double));
   P:=Allocate(N*sizeof(Double));
   Q:=Allocate(N*sizeof(Double));
   try
    if Assigned(A) and Assigned(P) and Assigned(Q) then begin
     CreateNormalLeastSquareEquations(X,Y,NumPoints,myCenter,myScale,N,M,A[0],P[0],Q[0]);
     if Partial_Pivoting(N,M,A[0],P[0],Q[0],Eps)=0 then begin
      for i:=0 to N-1 do myCoeff[i]:=P[i];
      Result:=true;
     end;
    end;
   finally
    Deallocate(Pointer(A));
    Deallocate(Pointer(P));
    Deallocate(Pointer(Q));
   end;
  end;
 except
  on E:Exception do ErrorFound(E,'Find');
 end;
end;

function TPolynom.FindWeight(const X,Y,W:array of Double; NumPoints:Integer; Eps:double=LsqPolyEps):boolean;
var  A,P,Q:PDoubleArray; t,v:Double; N,M,i:Integer;
begin
 Result:=false;
 if Assigned(Self) and (@X<>nil) and (@Y<>nil) and (@W<>nil) and (NumPoints>myPower) then
 try
  if myPower=0 then begin
   t:=0;
   v:=0;
   for i:=0 to NumPoints-1 do t:=t+Y[i]*W[i];
   for i:=0 to NumPoints-1 do v:=v+W[i];
   myCoeff[0]:=t/v;
   Result:=true;
  end else begin
   N:=myPower+1;
   M:=N;
   A:=Allocate(N*M*sizeof(Double));
   P:=Allocate(N*sizeof(Double));
   Q:=Allocate(N*sizeof(Double));
   try
    if Assigned(A) and Assigned(P) and Assigned(Q) then begin
     CreateWeightNormalLeastSquareEquations(X,Y,W,NumPoints,myCenter,myScale,N,M,A[0],P[0],Q[0]);
     if Partial_Pivoting(N,M,A[0],P[0],Q[0],Eps)=0 then begin
      for i:=0 to N-1 do myCoeff[i]:=P[i];
      Result:=true;
     end;
    end;
   finally
    Deallocate(Pointer(A));
    Deallocate(Pointer(P));
    Deallocate(Pointer(Q));
   end; 
  end;
 except
  on E:Exception do ErrorFound(E,'FindWeight');
 end;
end;

function NewPolynom(aPower  : TPolynomPower = 1;
                    aCenter : Double        = 0;
                    aScale  : Double        = 1):TPolynom;
begin
 Result:=nil;
 try
  Result:=TPolynom.Create(aPower,aCenter,aScale);
 except
  on E:Exception do BugReport(E,nil,'NewPolynom');
 end;
end;

function NewPolynomCopy(aPolynom : TPolynom = nil):TPolynom;
var i:TPolynomPower;
begin
 Result:=nil;
 if Assigned(aPolynom) then begin
  Result:=NewPolynom(aPolynom.Power,aPolynom.Center,aPolynom.Scale);
  for i:=Low(TPolynomPower) to High(TPolynomPower) do Result[i]:=aPolynom[i];
 end;
end;

procedure Kill(var TheObject:TPolynom); overload;
begin
 try
  FreeAndNil(TheObject);
 except
  on E:Exception do BugReport(E,nil,'Kill');
 end; 
end;

///////////////////////////////////////
// Unit initialization and finalization
///////////////////////////////////////

procedure Init_crw_lsqpoly;
begin
end;

procedure Free_crw_lsqpoly;
begin
end;

initialization

 Init_crw_lsqpoly;

finalization

 Free_crw_lsqpoly;

end.

//////////////
// END OF FILE
//////////////

