// Created on: 2005-12-15
// Created by: Julia GERASIMOVA
// Copyright (c) 2005-2014 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.

#include <math_EigenValuesSearcher.hxx>
#include <StdFail_NotDone.hxx>

//==========================================================================
// function : pythag
//           Computation of sqrt(x*x + y*y).
//==========================================================================
static inline Standard_Real pythag(const Standard_Real x, const Standard_Real y)
{
  return Sqrt(x * x + y * y);
}

math_EigenValuesSearcher::math_EigenValuesSearcher(const TColStd_Array1OfReal& Diagonal,
                                                   const TColStd_Array1OfReal& Subdiagonal)
{
  myIsDone = Standard_False;

  Standard_Integer n = Diagonal.Length();
  if (Subdiagonal.Length() != n)
    throw Standard_Failure("math_EigenValuesSearcher : dimension mismatch");

  myDiagonal                    = new TColStd_HArray1OfReal(1, n);
  myDiagonal->ChangeArray1()    = Diagonal;
  mySubdiagonal                 = new TColStd_HArray1OfReal(1, n);
  mySubdiagonal->ChangeArray1() = Subdiagonal;
  myN                           = n;
  myEigenValues                 = new TColStd_HArray1OfReal(1, n);
  myEigenVectors                = new TColStd_HArray2OfReal(1, n, 1, n);

  Standard_Real*   d = new Standard_Real[n + 1];
  Standard_Real*   e = new Standard_Real[n + 1];
  Standard_Real**  z = new Standard_Real*[n + 1];
  Standard_Integer i, j;
  for (i = 1; i <= n; i++)
    z[i] = new Standard_Real[n + 1];

  for (i = 1; i <= n; i++)
    d[i] = myDiagonal->Value(i);
  for (i = 2; i <= n; i++)
    e[i] = mySubdiagonal->Value(i);
  for (i = 1; i <= n; i++)
    for (j = 1; j <= n; j++)
      z[i][j] = (i == j) ? 1. : 0.;

  Standard_Boolean result;
  Standard_Integer m;
  Standard_Integer l;
  Standard_Integer iter;
  // Standard_Integer i;
  Standard_Integer k;
  Standard_Real    s;
  Standard_Real    r;
  Standard_Real    p;
  Standard_Real    g;
  Standard_Real    f;
  Standard_Real    dd;
  Standard_Real    c;
  Standard_Real    b;

  result = Standard_True;

  if (n != 1)
  {
    // Shift e.
    for (i = 2; i <= n; i++)
      e[i - 1] = e[i];

    e[n] = 0.0;

    for (l = 1; l <= n; l++)
    {
      iter = 0;

      do
      {
        for (m = l; m <= n - 1; m++)
        {
          dd = Abs(d[m]) + Abs(d[m + 1]);

          if (Abs(e[m]) + dd == dd)
            break;
        }

        if (m != l)
        {
          if (iter++ == 30)
          {
            result = Standard_False;
            break; // return result;
          }

          g = (d[l + 1] - d[l]) / (2. * e[l]);
          r = pythag(1., g);

          if (g < 0)
            g = d[m] - d[l] + e[l] / (g - r);
          else
            g = d[m] - d[l] + e[l] / (g + r);

          s = 1.;
          c = 1.;
          p = 0.;

          for (i = m - 1; i >= l; i--)
          {
            f        = s * e[i];
            b        = c * e[i];
            r        = pythag(f, g);
            e[i + 1] = r;

            if (r == 0.)
            {
              d[i + 1] -= p;
              e[m] = 0.;
              break;
            }

            s        = f / r;
            c        = g / r;
            g        = d[i + 1] - p;
            r        = (d[i] - g) * s + 2.0 * c * b;
            p        = s * r;
            d[i + 1] = g + p;
            g        = c * r - b;

            for (k = 1; k <= n; k++)
            {
              f           = z[k][i + 1];
              z[k][i + 1] = s * z[k][i] + c * f;
              z[k][i]     = c * z[k][i] - s * f;
            }
          }

          if (r == 0 && i >= 1)
            continue;

          d[l] -= p;
          e[l] = g;
          e[m] = 0.;
        }
      } while (m != l);
      if (result == Standard_False)
        break;
    } // end of for (l = 1; l <= n; l++)
  } // end of if (n != 1)

  if (result)
  {
    for (i = 1; i <= n; i++)
      myEigenValues->ChangeValue(i) = d[i];
    for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
        myEigenVectors->ChangeValue(i, j) = z[i][j];
  }

  myIsDone = result;

  delete[] d;
  delete[] e;
  for (i = 1; i <= n; i++)
    delete[] z[i];
  delete[] z;
}

Standard_Boolean math_EigenValuesSearcher::IsDone() const
{
  return myIsDone;
}

Standard_Integer math_EigenValuesSearcher::Dimension() const
{
  return myN;
}

Standard_Real math_EigenValuesSearcher::EigenValue(const Standard_Integer Index) const
{
  return myEigenValues->Value(Index);
}

math_Vector math_EigenValuesSearcher::EigenVector(const Standard_Integer Index) const
{
  math_Vector theVector(1, myN);

  Standard_Integer i;
  for (i = 1; i <= myN; i++)
    theVector(i) = myEigenVectors->Value(i, Index);

  return theVector;
}
