#include <iostream>
#include <gp_Lin2d.hxx>
#include <gp_Pnt2d.hxx>
#include <TColgp_Array1OfPnt2d.hxx>
#include <math_Vector.hxx>
#include <math_SVD.hxx>
#include <math_Gauss.hxx>
#include <math_GaussLeastSquare.hxx>
#include <OSD_Chronometer.hxx>
void fitLine(const TColgp_Array1OfPnt2d& thePoints,
             const std::string& theFileName,
             gp_Lin2d& theLine)
{
    math_Vector aB(1, 2, 0.0);
    math_Vector aX(1, 2, 0.0);
    math_Matrix aM(1, 2, 1, 2);
    Standard_Real aSxi = 0.0;
    Standard_Real aSyi = 0.0;
    Standard_Real aSxx = 0.0;
    Standard_Real aSxy = 0.0;
    std::ofstream aDrawFile(theFileName);
    for (Standard_Integer i = thePoints.Lower(); i <= thePoints.Upper(); ++i)
    {
        const gp_Pnt2d& aPoint = thePoints.Value(i);
        aSxi += aPoint.X();
        aSyi += aPoint.Y();
        aSxx += aPoint.X() * aPoint.X();
        aSxy += aPoint.X() * aPoint.Y();
        aDrawFile << "vpoint p" << i << " " <<
                     aPoint.X() << " " << aPoint.Y() << " 0" << std::endl;
    }
    aM(1, 1) = thePoints.Size();
    aM(1, 2) = aSxi;
    aM(2, 1) = aSxi;
    aM(2, 2) = aSxx;
    aB(1) = aSyi;
    aB(2) = aSxy;
    OSD_Chronometer aChronometer;
    aChronometer.Start();
    math_Gauss aSolver(aM);
    //math_GaussLeastSquare aSolver(aM);
    //math_SVD aSolver(aM);
    aSolver.Solve(aB, aX);
    if (aSolver.IsDone())
    {
        Standard_Real aA = aX(1);
        Standard_Real aB = aX(2);
        gp_Pnt2d aP1(0.0, aA);
        gp_Pnt2d aP2(-aA/aB, 0.0);
        theLine.SetLocation(aP1);
        theLine.SetDirection(gp_Vec2d(aP1, aP2).XY());
        aDrawFile << "vaxis l "
                  << aP1.X() << " " << aP1.Y() << " 0 "
                  << aP2.X() << " " << aP2.Y() << " 0 " << std::endl;
        std::cout << "===================" << std::endl;
        aX.Dump(std::cout);
    }
    aChronometer.Stop();
    aChronometer.Show();
}
int main()
{
    gp_Lin2d aLine;
    // Test data 1
    TColgp_Array1OfPnt2d aPoints1(1, 6);
    aPoints1.SetValue(1, gp_Pnt2d(36.9, 181.0));
    aPoints1.SetValue(2, gp_Pnt2d(46.7, 197.0));
    aPoints1.SetValue(3, gp_Pnt2d(63.7, 235.0));
    aPoints1.SetValue(4, gp_Pnt2d(77.8, 270.0));
    aPoints1.SetValue(5, gp_Pnt2d(84.0, 283.0));
    aPoints1.SetValue(6, gp_Pnt2d(87.5, 292.0));
    fitLine(aPoints1, "fit1.tcl", aLine);
    // Test data 2
    TColgp_Array1OfPnt2d aPoints2(0, 7);
    aPoints2.SetValue(0, gp_Pnt2d(0.0, 27.0));
    aPoints2.SetValue(1, gp_Pnt2d(1.0, 26.8));
    aPoints2.SetValue(2, gp_Pnt2d(2.0, 26.5));
    aPoints2.SetValue(3, gp_Pnt2d(3.0, 26.3));
    aPoints2.SetValue(4, gp_Pnt2d(4.0, 26.1));
    aPoints2.SetValue(5, gp_Pnt2d(5.0, 25.7));
    aPoints2.SetValue(6, gp_Pnt2d(6.0, 25.3));
    aPoints2.SetValue(7, gp_Pnt2d(7.0, 24.8));
    fitLine(aPoints2, "fit2.tcl", aLine);
    return 0;
}