// Copyright (c) 1997-1999 Matra Datavision
// Copyright (c) 1999-2023 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 "Standard_DimensionError.hxx"
#include "Standard_DivideByZero.hxx"
#include "Standard_RangeError.hxx"
#include "Standard_NullValue.hxx"

#include <stdio.h>

template <typename TheItemType>
math_VectorBase<TheItemType>::math_VectorBase(const int theLower, const int theUpper)
    : Array(theUpper - theLower + 1 <= math_VectorBase::THE_BUFFER_SIZE
              ? NCollection_Array1<TheItemType>(*myBuffer.data(), theLower, theUpper)
              : NCollection_Array1<TheItemType>(theLower, theUpper))
{
}

template <typename TheItemType>
math_VectorBase<TheItemType>::math_VectorBase(const int         theLower,
                                              const int         theUpper,
                                              const TheItemType theInitialValue)
    : Array(theUpper - theLower + 1 <= math_VectorBase::THE_BUFFER_SIZE
              ? NCollection_Array1<TheItemType>(*myBuffer.data(), theLower, theUpper)
              : NCollection_Array1<TheItemType>(theLower, theUpper))
{
  Array.Init(theInitialValue);
}

template <typename TheItemType>
math_VectorBase<TheItemType>::math_VectorBase(const TheItemType* theTab,
                                              const int          theLower,
                                              const int          theUpper)
    : Array(*theTab, theLower, theUpper)
{
}

template <typename TheItemType>
math_VectorBase<TheItemType>::math_VectorBase(const gp_XY& theOther)
    : Array(*myBuffer.data(), 1, 2)
{
  Array(1) = static_cast<TheItemType>(theOther.X());
  Array(2) = static_cast<TheItemType>(theOther.Y());
}

template <typename TheItemType>
math_VectorBase<TheItemType>::math_VectorBase(const gp_XYZ& theOther)
    : Array(*myBuffer.data(), 1, 3)
{
  Array(1) = static_cast<TheItemType>(theOther.X());
  Array(2) = static_cast<TheItemType>(theOther.Y());
  Array(3) = static_cast<TheItemType>(theOther.Z());
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Init(const TheItemType theInitialValue)
{
  Array.Init(theInitialValue);
}

template <typename TheItemType>
math_VectorBase<TheItemType>::math_VectorBase(const math_VectorBase<TheItemType>& theOther)
    : Array(theOther.Array)
{
}

template <typename TheItemType>
math_VectorBase<TheItemType>::math_VectorBase(math_VectorBase<TheItemType>&& theOther) noexcept
    : myBuffer{},
      Array(theOther.Array.IsDeletable()
              ? std::move(theOther.Array)
              : (theOther.Length() <= math_VectorBase::THE_BUFFER_SIZE
                   ? NCollection_Array1<TheItemType>(*myBuffer.data(),
                                                     theOther.Lower(),
                                                     theOther.Upper())
                   : NCollection_Array1<TheItemType>(theOther.Lower(), theOther.Upper())))
{
  if (!theOther.Array.IsEmpty())
  {
    Array.Assign(theOther.Array);
  }
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::SetLower(const int theLower)
{
  Array.UpdateLowerBound(theLower);
}

template <typename TheItemType>
double math_VectorBase<TheItemType>::Norm() const
{
  const int          aLen  = Length();
  const TheItemType* aPtr  = &Array(Lower());
  double             aSum1 = 0.0, aSum2 = 0.0, aSum3 = 0.0, aSum4 = 0.0;
  int                i     = 0;
  int                aLen4 = aLen - 3;

  for (; i < aLen4; i += 4)
  {
    const double aVal0 = static_cast<double>(aPtr[i]);
    const double aVal1 = static_cast<double>(aPtr[i + 1]);
    const double aVal2 = static_cast<double>(aPtr[i + 2]);
    const double aVal3 = static_cast<double>(aPtr[i + 3]);
    aSum1 += aVal0 * aVal0;
    aSum2 += aVal1 * aVal1;
    aSum3 += aVal2 * aVal2;
    aSum4 += aVal3 * aVal3;
  }

  for (; i < aLen; ++i)
  {
    const double aVal = static_cast<double>(aPtr[i]);
    aSum1 += aVal * aVal;
  }

  return std::sqrt((aSum1 + aSum2) + (aSum3 + aSum4));
}

template <typename TheItemType>
double math_VectorBase<TheItemType>::Norm2() const
{
  const int          aLen  = Length();
  const TheItemType* aPtr  = &Array(Lower());
  double             aSum1 = 0.0, aSum2 = 0.0, aSum3 = 0.0, aSum4 = 0.0;
  int                i     = 0;
  int                aLen4 = aLen - 3;

  for (; i < aLen4; i += 4)
  {
    const double aVal0 = static_cast<double>(aPtr[i]);
    const double aVal1 = static_cast<double>(aPtr[i + 1]);
    const double aVal2 = static_cast<double>(aPtr[i + 2]);
    const double aVal3 = static_cast<double>(aPtr[i + 3]);
    aSum1 += aVal0 * aVal0;
    aSum2 += aVal1 * aVal1;
    aSum3 += aVal2 * aVal2;
    aSum4 += aVal3 * aVal3;
  }

  for (; i < aLen; ++i)
  {
    const double aVal = static_cast<double>(aPtr[i]);
    aSum1 += aVal * aVal;
  }

  return (aSum1 + aSum2) + (aSum3 + aSum4);
}

template <typename TheItemType>
int math_VectorBase<TheItemType>::Max() const
{
  int    I = 0;
  double X = RealFirst();

  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    if (Array(Index) > X)
    {
      X = Array(Index);
      I = Index;
    }
  }
  return I;
}

template <typename TheItemType>
int math_VectorBase<TheItemType>::Min() const
{
  int    I = 0;
  double X = RealLast();

  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    if (Array(Index) < X)
    {
      X = Array(Index);
      I = Index;
    }
  }
  return I;
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Set(const int                           theI1,
                                       const int                           theI2,
                                       const math_VectorBase<TheItemType>& theV)
{
  Standard_RangeError_Raise_if((theI1 < Lower()) || (theI2 > Upper()) || (theI1 > theI2)
                                 || (theI2 - theI1 + 1 != theV.Length()),
                               "math_VectorBase::Set() - invalid indices");
  int I = theV.Lower();
  for (int Index = theI1; Index <= theI2; Index++)
  {
    Array(Index) = theV.Array(I);
    I++;
  }
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Normalize()
{
  double Result = Norm();
  Standard_NullValue_Raise_if((Result <= RealEpsilon()),
                              "math_VectorBase::Normalize() - vector has zero norm");
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Array(Index) = Array(Index) / Result;
  }
}

template <typename TheItemType>
math_VectorBase<TheItemType> math_VectorBase<TheItemType>::Normalized() const
{
  math_VectorBase Result = *this;
  Result.Normalize();
  return Result;
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Invert()
{
  for (int Index = Lower(); Index <= (Lower() + Length()) >> 1; Index++)
  {
    int         J     = Upper() + Lower() - Index;
    TheItemType aTemp = Array(Index);
    Array(Index)      = Array(J);
    Array(J)          = aTemp;
  }
}

template <typename TheItemType>
math_VectorBase<TheItemType> math_VectorBase<TheItemType>::Inverse() const
{
  math_VectorBase Result = *this;
  Result.Invert();
  return Result;
}

template <typename TheItemType>
math_VectorBase<TheItemType> math_VectorBase<TheItemType>::Multiplied(
  const TheItemType theRight) const
{
  math_VectorBase Result(Lower(), Upper());
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Result.Array(Index) = Array(Index) * theRight;
  }
  return Result;
}

template <typename TheItemType>
math_VectorBase<TheItemType> math_VectorBase<TheItemType>::TMultiplied(
  const TheItemType theRight) const
{
  math_VectorBase Result(Lower(), Upper());
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Result.Array(Index) = Array(Index) * theRight;
  }
  return Result;
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Multiply(const TheItemType theRight)
{
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Array(Index) = Array(Index) * theRight;
  }
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Divide(const TheItemType theRight)
{
  Standard_DivideByZero_Raise_if(std::abs(theRight) <= RealEpsilon(),
                                 "math_VectorBase::Divide() - devisor is zero");

  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Array(Index) = Array(Index) / theRight;
  }
}

template <typename TheItemType>
math_VectorBase<TheItemType> math_VectorBase<TheItemType>::Divided(const TheItemType theRight) const
{
  Standard_DivideByZero_Raise_if(std::abs(theRight) <= RealEpsilon(),
                                 "math_VectorBase::Divided() - devisor is zero");
  math_VectorBase temp = Multiplied(1. / theRight);
  return temp;
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Add(const math_VectorBase<TheItemType>& theRight)
{
  Standard_DimensionError_Raise_if(Length() != theRight.Length(),
                                   "math_VectorBase::Add() - input vector has wrong dimensions");

  int I = theRight.Lower();
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Array(Index) = Array(Index) + theRight.Array(I);
    I++;
  }
}

template <typename TheItemType>
math_VectorBase<TheItemType> math_VectorBase<TheItemType>::Added(
  const math_VectorBase<TheItemType>& theRight) const
{
  Standard_DimensionError_Raise_if(Length() != theRight.Length(),
                                   "math_VectorBase::Added() - input vector has wrong dimensions");

  math_VectorBase Result(Lower(), Upper());

  int I = theRight.Lower();
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Result.Array(Index) = Array(Index) + theRight.Array(I);
    I++;
  }
  return Result;
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Subtract(const math_VectorBase<TheItemType>& theRight)
{
  Standard_DimensionError_Raise_if(
    Length() != theRight.Length(),
    "math_VectorBase::Subtract() - input vector has wrong dimensions");

  int I = theRight.Lower();
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Array(Index) = Array(Index) - theRight.Array(I);
    I++;
  }
}

template <typename TheItemType>
math_VectorBase<TheItemType> math_VectorBase<TheItemType>::Subtracted(
  const math_VectorBase<TheItemType>& theRight) const
{
  Standard_DimensionError_Raise_if(
    Length() != theRight.Length(),
    "math_VectorBase::Subtracted() - input vector has wrong dimensions");

  math_VectorBase Result(Lower(), Upper());

  int I = theRight.Lower();
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Result.Array(Index) = Array(Index) - theRight.Array(I);
    I++;
  }
  return Result;
}

template <typename TheItemType>
math_VectorBase<TheItemType> math_VectorBase<TheItemType>::Slice(const int theI1,
                                                                 const int theI2) const
{
  Standard_RangeError_Raise_if((theI1 < Lower()) || (theI1 > Upper()) || (theI2 < Lower())
                                 || (theI2 > Upper()),
                               "math_VectorBase::Slice() - invalid indices");

  if (theI2 >= theI1)
  {
    math_VectorBase Result(theI1, theI2);
    for (int Index = theI1; Index <= theI2; Index++)
    {
      Result.Array(Index) = Array(Index);
    }
    return Result;
  }
  else
  {
    math_VectorBase Result(theI2, theI1);
    for (int Index = theI1; Index >= theI2; Index--)
    {
      Result.Array(Index) = Array(Index);
    }
    return Result;
  }
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Add(const math_VectorBase<TheItemType>& theLeft,
                                       const math_VectorBase<TheItemType>& theRight)
{
  Standard_DimensionError_Raise_if((Length() != theRight.Length())
                                     || (theRight.Length() != theLeft.Length()),
                                   "math_VectorBase::Add() - input vectors have wrong dimensions");

  int I = theLeft.Lower();
  int J = theRight.Lower();
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Array(Index) = theLeft.Array(I) + theRight.Array(J);
    I++;
    J++;
  }
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Subtract(const math_VectorBase<TheItemType>& theLeft,
                                            const math_VectorBase<TheItemType>& theRight)
{
  Standard_DimensionError_Raise_if(
    (Length() != theRight.Length()) || (theRight.Length() != theLeft.Length()),
    "math_VectorBase::Subtract() - input vectors have wrong dimensions");

  int I = theLeft.Lower();
  int J = theRight.Lower();
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Array(Index) = theLeft.Array(I) - theRight.Array(J);
    I++;
    J++;
  }
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Multiply(const math_Matrix&                  theLeft,
                                            const math_VectorBase<TheItemType>& theRight)
{
  Standard_DimensionError_Raise_if(
    (Length() != theLeft.RowNumber()) || (theLeft.ColNumber() != theRight.Length()),
    "math_VectorBase::Multiply() - input matrix and /or vector have wrong dimensions");

  // result[r] = sum_c theLeft(r, c) * theRight[c]
  const int          aNRows    = theLeft.RowNumber();
  const int          aNCols    = theLeft.ColNumber();
  const double*      aMatData  = &theLeft(theLeft.LowerRow(), theLeft.LowerCol());
  const TheItemType* aRightPtr = &theRight.Array(theRight.Lower());
  TheItemType*       aResPtr   = &Array(Lower());

  for (int r = 0; r < aNRows; r++)
  {
    TheItemType   aSum     = 0.0;
    const double* aRowData = aMatData + r * aNCols;
    for (int c = 0; c < aNCols; c++)
    {
      aSum += aRowData[c] * aRightPtr[c];
    }
    aResPtr[r] = aSum;
  }
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Multiply(const math_VectorBase<TheItemType>& theLeft,
                                            const math_Matrix&                  theRight)
{
  Standard_DimensionError_Raise_if(
    (Length() != theRight.ColNumber()) || (theLeft.Length() != theRight.RowNumber()),
    "math_VectorBase::Multiply() - input matrix and /or vector have wrong dimensions");

  // result[c] = sum_r theLeft[r] * theRight(r, c)
  const int          aNCols   = theRight.ColNumber();
  const int          aNRows   = theRight.RowNumber();
  const double*      aMatData = &theRight(theRight.LowerRow(), theRight.LowerCol());
  const TheItemType* aLeftPtr = &theLeft.Array(theLeft.Lower());
  TheItemType*       aResPtr  = &Array(Lower());

  for (int c = 0; c < aNCols; c++)
  {
    TheItemType aSum = 0.0;
    for (int r = 0; r < aNRows; r++)
    {
      aSum += aLeftPtr[r] * aMatData[r * aNCols + c];
    }
    aResPtr[c] = aSum;
  }
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::TMultiply(const math_Matrix&                  theTLeft,
                                             const math_VectorBase<TheItemType>& theRight)
{
  Standard_DimensionError_Raise_if(
    (Length() != theTLeft.ColNumber()) || (theTLeft.RowNumber() != theRight.Length()),
    "math_VectorBase::TMultiply() - input matrix and /or vector have wrong dimensions");

  // result[c] = sum_r theTLeft(r, c) * theRight[r]  (transpose-multiply)
  const int          aNCols    = theTLeft.ColNumber();
  const int          aNRows    = theTLeft.RowNumber();
  const double*      aMatData  = &theTLeft(theTLeft.LowerRow(), theTLeft.LowerCol());
  const TheItemType* aRightPtr = &theRight.Array(theRight.Lower());
  TheItemType*       aResPtr   = &Array(Lower());

  for (int c = 0; c < aNCols; c++)
  {
    TheItemType aSum = 0.0;
    for (int r = 0; r < aNRows; r++)
    {
      aSum += aMatData[r * aNCols + c] * aRightPtr[r];
    }
    aResPtr[c] = aSum;
  }
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::TMultiply(const math_VectorBase<TheItemType>& theLeft,
                                             const math_Matrix&                  theTRight)
{
  Standard_DimensionError_Raise_if(
    (Length() != theTRight.RowNumber()) || (theLeft.Length() != theTRight.ColNumber()),
    "math_VectorBase::TMultiply() - input matrix and /or vector have wrong dimensions");

  // result[r] = sum_c theLeft[c] * theTRight(r, c)
  const int          aNCols   = theTRight.ColNumber();
  const int          aNRows   = theTRight.RowNumber();
  const double*      aMatData = &theTRight(theTRight.LowerRow(), theTRight.LowerCol());
  const TheItemType* aLeftPtr = &theLeft.Array(theLeft.Lower());
  TheItemType*       aResPtr  = &Array(Lower());

  for (int r = 0; r < aNRows; r++)
  {
    TheItemType   aSum     = 0.0;
    const double* aRowData = aMatData + r * aNCols;
    for (int c = 0; c < aNCols; c++)
    {
      aSum += aLeftPtr[c] * aRowData[c];
    }
    aResPtr[r] = aSum;
  }
}

template <typename TheItemType>
TheItemType math_VectorBase<TheItemType>::Multiplied(
  const math_VectorBase<TheItemType>& theRight) const
{
  Standard_DimensionError_Raise_if(
    Length() != theRight.Length(),
    "math_VectorBase::Multiplied() - input vector has wrong dimensions");

  const int          aLen      = Length();
  const TheItemType* aLeftPtr  = &Array(Lower());
  const TheItemType* aRightPtr = &theRight.Array(theRight.Lower());
  TheItemType        aResult   = 0;
  for (int i = 0; i < aLen; i++)
  {
    aResult += aLeftPtr[i] * aRightPtr[i];
  }
  return aResult;
}

template <typename TheItemType>
math_VectorBase<TheItemType> math_VectorBase<TheItemType>::Opposite()
{
  math_VectorBase Result(Lower(), Upper());
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    Result.Array(Index) = -Array(Index);
  }
  return Result;
}

template <typename TheItemType>
math_VectorBase<TheItemType> math_VectorBase<TheItemType>::Multiplied(
  const math_Matrix& theRight) const
{
  Standard_DimensionError_Raise_if(
    Length() != theRight.RowNumber(),
    "math_VectorBase::Multiplied() - input matrix has wrong dimensions");

  math_VectorBase Result(theRight.LowerCol(), theRight.UpperCol());
  for (int J2 = theRight.LowerCol(); J2 <= theRight.UpperCol(); J2++)
  {
    Result.Array(J2) = 0.0;
    int theI2        = theRight.LowerRow();
    for (int I = Lower(); I <= Upper(); I++)
    {
      Result.Array(J2) = Result.Array(J2) + Array(I) * theRight(theI2, J2);
      theI2++;
    }
  }
  return Result;
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Multiply(const TheItemType                   theLeft,
                                            const math_VectorBase<TheItemType>& theRight)
{
  Standard_DimensionError_Raise_if(
    (Length() != theRight.Length()),
    "math_VectorBase::Multiply() - input vector has wrong dimensions");
  for (int I = Lower(); I <= Upper(); I++)
  {
    Array(I) = theLeft * theRight.Array(I);
  }
}

template <typename TheItemType>
math_VectorBase<TheItemType>& math_VectorBase<TheItemType>::Initialized(
  const math_VectorBase<TheItemType>& theOther)
{
  Array.CopyValues(theOther.Array);
  return *this;
}

template <typename TheItemType>
math_VectorBase<TheItemType>& math_VectorBase<TheItemType>::operator=(
  math_VectorBase<TheItemType>&& theOther)
{
  if (this == &theOther)
  {
    return *this;
  }

  if (Array.IsDeletable() && theOther.Array.IsDeletable() && Lower() == theOther.Lower()
      && Length() == theOther.Length())
  {
    Array.Move(theOther.Array);
  }
  else
  {
    Initialized(theOther);
  }
  return *this;
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Dump(Standard_OStream& theO) const
{
  theO << "math_Vector of Length = " << Length() << "\n";
  for (int Index = Lower(); Index <= Upper(); Index++)
  {
    theO << "math_Vector(" << Index << ") = " << Array(Index) << "\n";
  }
}

template <typename TheItemType>
void math_VectorBase<TheItemType>::Resize(const int theSize)
{
  const int  theLower      = Array.Lower();
  const int  theUpper      = theLower + theSize - 1;
  const bool aNewFitsStack = theSize <= THE_BUFFER_SIZE;
  const bool aWasOnStack   = !Array.IsDeletable();

  if (aWasOnStack && aNewFitsStack)
  {
    // Stack -> Stack: data is already in myBuffer, just update Array bounds
    Array = NCollection_Array1<TheItemType>(*myBuffer.data(), theLower, theUpper);
  }
  else if (aNewFitsStack)
  {
    // Heap -> Stack: copy data to stack buffer
    const int aCopyLen = std::min(Array.Length(), theSize);
    for (int i = 0; i < aCopyLen; ++i)
    {
      myBuffer[i] = Array.Value(theLower + i);
    }
    Array = NCollection_Array1<TheItemType>(*myBuffer.data(), theLower, theUpper);
  }
  else
  {
    // Stack -> Heap or Heap -> Heap: Array.Resize handles data copy
    Array.Resize(theLower, theUpper, true);
  }
}