// Copyright (C) 2004, 2009 International Business Machines and others.
// All Rights Reserved.
// This code is published under the Eclipse Public License.
//
// Authors:  Carl Laird, Andreas Waechter     IBM    2004-08-13

#include "IpDenseVector.hpp"
#include "IpBlas.hpp"
#include "IpUtils.hpp"
#include "IpDebug.hpp"

#include <cmath>
#include <limits>

namespace Ipopt
{

#if IPOPT_VERBOSITY > 0
static const Index dbg_verbosity = 0;
#endif

DenseVector::DenseVector(
   const DenseVectorSpace* owner_space
)
   : Vector(owner_space),
     owner_space_(owner_space),
     values_(NULL),
     expanded_values_(NULL),
     initialized_(false),
     homogeneous_(false)
{
   DBG_START_METH("DenseVector::DenseVector(Index dim)", dbg_verbosity);
   if( Dim() == 0 )
   {
      initialized_ = true;
      homogeneous_ = true;
      scalar_ = 0.;
   }
}

DenseVector::~DenseVector()
{
   DBG_START_METH("DenseVector::~DenseVector()", dbg_verbosity);
   if( values_ )
   {
      owner_space_->FreeInternalStorage(values_);
   }
   if( expanded_values_ )
   {
      owner_space_->FreeInternalStorage(expanded_values_);
   }
}

void DenseVector::SetValues(
   const Number* x
)
{
   initialized_ = true;
   IpBlasCopy(Dim(), x, 1, values_allocated(), 1);
   homogeneous_ = false;
   // This is not an overloaded method from
   // Vector. Here, we must call ObjectChanged()
   // manually.
   ObjectChanged();
}

const Number* DenseVector::ExpandedValues() const
{
   if( IsHomogeneous() )
   {
      if( !expanded_values_ )
      {
         expanded_values_ = owner_space_->AllocateInternalStorage();
      }
      IpBlasCopy(Dim(), &scalar_, 0, expanded_values_, 1);
      return expanded_values_;
   }
   else
   {
      return values_;
   }
}

void DenseVector::set_values_from_scalar()
{
   DBG_ASSERT(homogeneous_);
   initialized_ = true;
   homogeneous_ = false;
   Number* vals = values_allocated();
   IpBlasCopy(Dim(), &scalar_, 0, vals, 1);
}

void DenseVector::CopyImpl(
   const Vector& x
)
{
   DBG_START_METH("DenseVector::CopyImpl(const Vector& x)", dbg_verbosity);
   const DenseVector* dense_x = static_cast<const DenseVector*>(&x);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&x));

   DBG_ASSERT(dense_x->initialized_);
   DBG_ASSERT(Dim() == dense_x->Dim());
   homogeneous_ = dense_x->homogeneous_;
   if( homogeneous_ )
   {
      scalar_ = dense_x->scalar_;
   }
   else
   {
      IpBlasCopy(Dim(), dense_x->values_, 1, values_allocated(), 1);
   }
   initialized_ = true;
}

void DenseVector::ScalImpl(
   Number alpha
)
{
   DBG_ASSERT(initialized_);
   if( homogeneous_ )
   {
      scalar_ *= alpha;
   }
   else
   {
      IpBlasScal(Dim(), alpha, values_, 1);
   }
}

void DenseVector::AxpyImpl(
   Number        alpha,
   const Vector& x
)
{
   DBG_ASSERT(initialized_);
   const DenseVector* dense_x = static_cast<const DenseVector*>(&x);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&x));

   DBG_ASSERT(dense_x->initialized_);
   DBG_ASSERT(Dim() == dense_x->Dim());

   if( Dim() == 0 )
   {
      return;
   }

   if( homogeneous_ )
   {
      if( dense_x->homogeneous_ )
      {
         scalar_ += alpha * dense_x->scalar_;
      }
      else
      {
         homogeneous_ = false;
         Number* vals = values_allocated();
         for( Index i = 0; i < Dim(); i++ )
         {
            vals[i] = scalar_ + alpha * dense_x->values_[i];
         }
      }
   }
   else
   {
      if( dense_x->homogeneous_ )
      {
         if( dense_x->scalar_ != 0. )
         {
            IpBlasAxpy(Dim(), alpha, &dense_x->scalar_, 0, values_, 1);
         }
      }
      else
      {
         IpBlasAxpy(Dim(), alpha, dense_x->values_, 1, values_, 1);
      }
   }
}

Number DenseVector::DotImpl(
   const Vector& x
) const
{
   DBG_ASSERT(initialized_);
   Number retValue;
   const DenseVector* dense_x = static_cast<const DenseVector*>(&x);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&x));

   DBG_ASSERT(dense_x->initialized_);
   DBG_ASSERT(Dim() == dense_x->Dim());

   if( Dim() == 0 )
   {
      return 0.0;
   }

   if( homogeneous_ )
   {
      if( dense_x->homogeneous_ )
      {
         retValue = Dim() * scalar_ * dense_x->scalar_;
      }
      else
      {
         retValue = IpBlasDot(Dim(), dense_x->values_, 1, &scalar_, 0);
      }
   }
   else
   {
      if( dense_x->homogeneous_ )
      {
         retValue = IpBlasDot(Dim(), &dense_x->scalar_, 0, values_, 1);
      }
      else
      {
         retValue = IpBlasDot(Dim(), dense_x->values_, 1, values_, 1);
      }
   }
   return retValue;
}

Number DenseVector::Nrm2Impl() const
{
   DBG_ASSERT(initialized_);
   if( homogeneous_ )
   {
      return std::sqrt((Number) Dim()) * std::abs(scalar_);
   }
   else
   {
      return IpBlasNrm2(Dim(), values_, 1);
   }
}

Number DenseVector::AsumImpl() const
{
   DBG_ASSERT(initialized_);
   if( homogeneous_ )
   {
      return Dim() * std::abs(scalar_);
   }
   else
   {
      return IpBlasAsum(Dim(), values_, 1);
   }
}

Number DenseVector::AmaxImpl() const
{
   DBG_ASSERT(initialized_);
   if( Dim() == 0 )
   {
      return 0.;
   }

   if( homogeneous_ )
   {
      return std::abs(scalar_);
   }

   return std::abs(values_[IpBlasIamax(Dim(), values_, 1) - 1]);
}

void DenseVector::SetImpl(
   Number value
)
{
   initialized_ = true;
   homogeneous_ = true;
   scalar_ = value;
   // ToDo decide if we want this here:
   if( values_ )
   {
      owner_space_->FreeInternalStorage(values_);
      values_ = NULL;
   }
}

void DenseVector::ElementWiseDivideImpl(
   const Vector& x
)
{
   DBG_ASSERT(initialized_);
   const DenseVector* dense_x = static_cast<const DenseVector*>(&x);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&x));

   DBG_ASSERT(dense_x->initialized_);
   const Number* values_x = dense_x->values_;
   DBG_ASSERT(Dim() == dense_x->Dim());

   if( Dim() == 0 )
   {
      return;
   }

   if( homogeneous_ )
   {
      if( dense_x->homogeneous_ )
      {
         scalar_ /= dense_x->scalar_;
      }
      else
      {
         homogeneous_ = false;
         Number* vals = values_allocated();
         for( Index i = 0; i < Dim(); i++ )
         {
            vals[i] = scalar_ / values_x[i];
         }
      }
   }
   else
   {
      if( dense_x->homogeneous_ )
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] /= dense_x->scalar_;
         }
      }
      else
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] /= values_x[i];
         }
      }
   }
}

void DenseVector::ElementWiseMultiplyImpl(
   const Vector& x
)
{
   DBG_ASSERT(initialized_);
   const DenseVector* dense_x = static_cast<const DenseVector*>(&x);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&x));

   DBG_ASSERT(dense_x->initialized_);
   const Number* values_x = dense_x->values_;
   DBG_ASSERT(Dim() == dense_x->Dim());

   if( Dim() == 0 )
   {
      return;
   }

   if( homogeneous_ )
   {
      if( dense_x->homogeneous_ )
      {
         scalar_ *= dense_x->scalar_;
      }
      else
      {
         homogeneous_ = false;
         Number* vals = values_allocated();
         for( Index i = 0; i < Dim(); i++ )
         {
            vals[i] = scalar_ * values_x[i];
         }
      }
   }
   else
   {
      if( dense_x->homogeneous_ )
      {
         if( dense_x->scalar_ != 1.0 )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] *= dense_x->scalar_;
            }
         }
      }
      else
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] *= values_x[i];
         }
      }
   }
}

void DenseVector::ElementWiseSelectImpl(
   const Vector& x
)
{
   DBG_ASSERT(initialized_);
   const DenseVector* dense_x = static_cast<const DenseVector*>(&x);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&x));

   DBG_ASSERT(dense_x->initialized_);
   const Number* values_x = dense_x->values_;
   DBG_ASSERT(Dim() == dense_x->Dim());

   if( Dim() == 0 )
   {
      return;
   }

   if( homogeneous_ )
   {
      if( scalar_ == 0.0 )
      {
         return;
      }
      if( dense_x->homogeneous_ )
      {
         scalar_ *= dense_x->scalar_;
      }
      else
      {
         homogeneous_ = false;
         Number* vals = values_allocated();
         for( Index i = 0; i < Dim(); i++ )
         {
            vals[i] = scalar_ * values_x[i];
         }
      }
   }
   else
   {
      if( dense_x->homogeneous_ )
      {
         if( dense_x->scalar_ != 1.0 )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               if( values_[i] > 0.0 )
               {
                  values_[i] = dense_x->scalar_;
               }
               else if( values_[i] < 0.0 )
               {
                  values_[i] = -dense_x->scalar_;
               }
               // else values_[i] remains at 0.0
            }
         }
      }
      else
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            if( values_[i] > 0.0 )
            {
               values_[i] = values_x[i];
            }
            else if( values_[i] < 0.0 )
            {
               values_[i] = -values_x[i];
            }
            // else values_[i] remains at 0.0
         }
      }
   }
}

void DenseVector::ElementWiseMaxImpl(
   const Vector& x
)
{
   DBG_ASSERT(initialized_);
   const DenseVector* dense_x = static_cast<const DenseVector*>(&x);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&x));

   DBG_ASSERT(dense_x->initialized_);
   const Number* values_x = dense_x->values_;
   DBG_ASSERT(Dim() == dense_x->Dim());

   if( Dim() == 0 )
   {
      return;
   }

   if( homogeneous_ )
   {
      if( dense_x->homogeneous_ )
      {
         scalar_ = Ipopt::Max(scalar_, dense_x->scalar_);
      }
      else
      {
         homogeneous_ = false;
         Number* vals = values_allocated();
         for( Index i = 0; i < Dim(); i++ )
         {
            vals[i] = Ipopt::Max(scalar_, values_x[i]);
         }
      }
   }
   else
   {
      if( dense_x->homogeneous_ )
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] = Ipopt::Max(values_[i], dense_x->scalar_);
         }
      }
      else
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] = Ipopt::Max(values_[i], values_x[i]);
         }
      }
   }
}

void DenseVector::ElementWiseMinImpl(
   const Vector& x
)
{
   DBG_ASSERT(initialized_);
   const DenseVector* dense_x = static_cast<const DenseVector*>(&x);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&x));

   DBG_ASSERT(dense_x->initialized_);
   const Number* values_x = dense_x->values_;
   DBG_ASSERT(Dim() == dense_x->Dim());

   if( Dim() == 0 )
   {
      return;
   }

   if( homogeneous_ )
   {
      if( dense_x->homogeneous_ )
      {
         scalar_ = Ipopt::Min(scalar_, dense_x->scalar_);
      }
      else
      {
         homogeneous_ = false;
         Number* vals = values_allocated();
         for( Index i = 0; i < Dim(); i++ )
         {
            vals[i] = Ipopt::Min(scalar_, values_x[i]);
         }
      }
   }
   else
   {
      if( dense_x->homogeneous_ )
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] = Ipopt::Min(values_[i], dense_x->scalar_);
         }
      }
      else
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] = Ipopt::Min(values_[i], values_x[i]);
         }
      }
   }
}

void DenseVector::ElementWiseReciprocalImpl()
{
   DBG_ASSERT(initialized_);

   if( Dim() == 0 )
   {
      return;
   }

   if( homogeneous_ )
   {
      scalar_ = 1.0 / scalar_;
   }
   else
   {
      for( Index i = 0; i < Dim(); i++ )
      {
         values_[i] = 1.0 / values_[i];
      }
   }
}

void DenseVector::ElementWiseAbsImpl()
{
   DBG_ASSERT(initialized_);
   if( homogeneous_ )
   {
      scalar_ = std::abs(scalar_);
   }
   else
   {
      for( Index i = 0; i < Dim(); i++ )
      {
         values_[i] = std::abs(values_[i]);
      }
   }
}

void DenseVector::ElementWiseSqrtImpl()
{
   DBG_ASSERT(initialized_);
   if( homogeneous_ )
   {
      scalar_ = std::sqrt(scalar_);
   }
   else
   {
      for( Index i = 0; i < Dim(); i++ )
      {
         values_[i] = std::sqrt(values_[i]);
      }
   }
}

void DenseVector::AddScalarImpl(
   Number scalar
)
{
   DBG_ASSERT(initialized_);
   if( homogeneous_ )
   {
      scalar_ += scalar;
   }
   else
   {
      IpBlasAxpy(Dim(), 1., &scalar, 0, values_, 1);
   }
}

Number DenseVector::MaxImpl() const
{
   DBG_ASSERT(initialized_);
   if( Dim() == 0 )
   {
      return -std::numeric_limits<Number>::max();
   }

   Number max;
   if( homogeneous_ )
   {
      max = scalar_;
   }
   else
   {
      max = values_[0];
      for( Index i = 1; i < Dim(); i++ )
      {
         max = Ipopt::Max(values_[i], max);
      }
   }
   return max;
}

Number DenseVector::MinImpl() const
{
   DBG_ASSERT(initialized_);
   if( Dim() == 0 )
   {
      return std::numeric_limits<Number>::max();
   }

   Number min;
   if( homogeneous_ )
   {
      min = scalar_;
   }
   else
   {
      min = values_[0];
      for( Index i = 1; i < Dim(); i++ )
      {
         min = Ipopt::Min(values_[i], min);
      }
   }
   return min;
}

Number DenseVector::SumImpl() const
{
   DBG_ASSERT(initialized_);
   Number sum;
   if( homogeneous_ )
   {
      sum = Dim() * scalar_;
   }
   else
   {
      sum = 0.;
      for( Index i = 0; i < Dim(); i++ )
      {
         sum += values_[i];
      }
   }
   return sum;
}

Number DenseVector::SumLogsImpl() const
{
   DBG_ASSERT(initialized_);
   Number sum;
   if( Dim() == 0 )
   {
      sum = 0.0;
   }
   else if( homogeneous_ )
   {
      sum = Dim() * std::log(scalar_);
   }
   else
   {
      sum = 0.0;
      for( Index i = 0; i < Dim(); i++ )
      {
         sum += std::log(values_[i]);
      }
   }
   return sum;
}

void DenseVector::ElementWiseSgnImpl()
{
   DBG_ASSERT(initialized_);
   if( homogeneous_ )
   {
      if( scalar_ > 0. )
      {
         scalar_ = 1.;
      }
      else if( scalar_ < 0. )
      {
         scalar_ = -1.;
      }
      else
      {
         scalar_ = 0.;
      }
   }
   else
   {
      for( Index i = 0; i < Dim(); i++ )
      {
         if( values_[i] > 0. )
         {
            values_[i] = 1.;
         }
         else if( values_[i] < 0. )
         {
            values_[i] = -1.;
         }
         else
         {
            values_[i] = 0;
         }
      }
   }
}

// Specialized Functions
void DenseVector::AddTwoVectorsImpl(
   Number        a,
   const Vector& v1,
   Number        b,
   const Vector& v2,
   Number        c
)
{
   if( Dim() == 0 )
   {
      DBG_ASSERT(initialized_);
      return;
   }

   const Number* values_v1 = NULL;
   bool homogeneous_v1 = false;
   Number scalar_v1 = 0;
   if( a != 0. )
   {
      const DenseVector* dense_v1 = static_cast<const DenseVector*>(&v1);
      DBG_ASSERT(dynamic_cast<const DenseVector*>(&v1));

      DBG_ASSERT(dense_v1->initialized_);
      DBG_ASSERT(Dim() == dense_v1->Dim());
      values_v1 = dense_v1->values_;
      homogeneous_v1 = dense_v1->homogeneous_;
      if( homogeneous_v1 )
      {
         scalar_v1 = dense_v1->scalar_;
      }
   }
   const Number* values_v2 = NULL;
   bool homogeneous_v2 = false;
   Number scalar_v2 = 0;
   if( b != 0. )
   {
      const DenseVector* dense_v2 = static_cast<const DenseVector*>(&v2);
      DBG_ASSERT(dynamic_cast<const DenseVector*>(&v2));

      DBG_ASSERT(dense_v2->initialized_);
      DBG_ASSERT(Dim() == dense_v2->Dim());
      values_v2 = dense_v2->values_;
      homogeneous_v2 = dense_v2->homogeneous_;
      if( homogeneous_v2 )
      {
         scalar_v2 = dense_v2->scalar_;
      }
   }
   DBG_ASSERT(c == 0. || initialized_);
   if( (c == 0. || homogeneous_) && homogeneous_v1 && homogeneous_v2 )
   {
      homogeneous_ = true;
      Number val = 0;
      if( c != 0. )
      {
         val = c * scalar_;
      }
      scalar_ = val + a * scalar_v1 + b * scalar_v2;
      initialized_ = true;
      return;
   }
   if( c == 0. )
   {
      // make sure we have memory allocated for this vector
      values_allocated();
      homogeneous_ = false;
   }

   // If any of the vectors is homogeneous, call the default implementation
   if( homogeneous_ || homogeneous_v1 || homogeneous_v2 )
   {
      // ToDo:Should we implement specialized methods here too?
      Vector::AddTwoVectorsImpl(a, v1, b, v2, c);
      return;
   }
   DBG_ASSERT(values_v1 != NULL || a == 0.);
   DBG_ASSERT(values_v2 != NULL || b == 0.);
   DBG_ASSERT(values_ != NULL);

   // I guess I'm going over board here, but it might be best to
   // capture all cases for a, b, and c separately...
   if( c == 0. )
   {
      if( a == 1. )
      {
         if( b == 0. )
         {
            IpBlasCopy(Dim(), values_v1, 1, values_, 1);
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v1[i] + values_v2[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v1[i] - values_v2[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v1[i] + b * values_v2[i];
            }
         }
      }
      else if( a == -1. )
      {
         if( b == 0. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i];
            }
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i] + values_v2[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i] - values_v2[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i] + b * values_v2[i];
            }
         }
      }
      else if( a == 0. )
      {
         if( b == 0. )
         {
            Number zero = 0.;
            IpBlasCopy(Dim(), &zero, 0, values_, 1);
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               IpBlasCopy(Dim(), values_v2, 1, values_, 1);
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v2[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = b * values_v2[i];
            }
         }
      }
      else
      {
         if( b == 0. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i];
            }
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i] + values_v2[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i] - values_v2[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i] + b * values_v2[i];
            }
         }
      }
   }
   else if( c == 1. )
   {
      if( a == 1. )
      {
         if( b == 0. )
         {
            IpBlasAxpy(Dim(), 1., values_v1, 1, values_, 1);
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] += values_v1[i] + values_v2[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] += values_v1[i] - values_v2[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] += values_v1[i] + b * values_v2[i];
            }
         }
      }
      else if( a == -1. )
      {
         if( b == 0. )
         {
            IpBlasAxpy(Dim(), -1., values_v1, 1, values_, 1);
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] += -values_v1[i] + values_v2[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] += -values_v1[i] - values_v2[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] += -values_v1[i] + b * values_v2[i];
            }
         }
      }
      else if( a == 0. )
      {
         if( b == 0. )
         {
            /* Nothing */
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               IpBlasAxpy(Dim(), 1., values_v2, 1, values_, 1);
            }
         }
         else if( b == -1. )
         {
            IpBlasAxpy(Dim(), -1., values_v2, 1, values_, 1);
         }
         else
         {
            IpBlasAxpy(Dim(), b, values_v2, 1, values_, 1);
         }
      }
      else
      {
         if( b == 0. )
         {
            IpBlasAxpy(Dim(), a, values_v1, 1, values_, 1);
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] += a * values_v1[i] + values_v2[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] += a * values_v1[i] - values_v2[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] += a * values_v1[i] + b * values_v2[i];
            }
         }
      }
   }
   else if( c == -1. )
   {
      if( a == 1. )
      {
         if( b == 0. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v1[i] - values_[i];
            }
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v1[i] + values_v2[i] - values_[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v1[i] - values_v2[i] - values_[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v1[i] + b * values_v2[i] - values_[i];
            }
         }
      }
      else if( a == -1. )
      {
         if( b == 0. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i] - values_[i];
            }
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i] + values_v2[i] - values_[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i] - values_v2[i] - values_[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i] + b * values_v2[i] - values_[i];
            }
         }
      }
      else if( a == 0. )
      {
         if( b == 0. )
         {
            IpBlasScal(Dim(), -1., values_, 1);
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v2[i] - values_[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v2[i] - values_[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = b * values_v2[i] - values_[i];
            }
         }
      }
      else
      {
         if( b == 0. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i] - values_[i];
            }
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i] + values_v2[i] - values_[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i] - values_v2[i] - values_[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i] + b * values_v2[i] - values_[i];
            }
         }
      }
   }
   else
   {
      if( a == 1. )
      {
         if( b == 0. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v1[i] + c * values_[i];
            }
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v1[i] + values_v2[i] + c * values_[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v1[i] - values_v2[i] + c * values_[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v1[i] + b * values_v2[i] + c * values_[i];
            }
         }
      }
      else if( a == -1. )
      {
         if( b == 0. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i] + c * values_[i];
            }
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i] + values_v2[i] + c * values_[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i] - values_v2[i] + c * values_[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v1[i] + b * values_v2[i] + c * values_[i];
            }
         }
      }
      else if( a == 0. )
      {
         if( b == 0. )
         {
            IpBlasScal(Dim(), c, values_, 1);
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = values_v2[i] + c * values_[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = -values_v2[i] + c * values_[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = b * values_v2[i] + c * values_[i];
            }
         }
      }
      else
      {
         if( b == 0. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i] + c * values_[i];
            }
         }
         else if( b == 1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i] + values_v2[i] + c * values_[i];
            }
         }
         else if( b == -1. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i] - values_v2[i] + c * values_[i];
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = a * values_v1[i] + b * values_v2[i] + c * values_[i];
            }
         }
      }
   }
   initialized_ = true;
}

Number DenseVector::FracToBoundImpl(
   const Vector& delta,
   Number        tau
) const
{
   DBG_ASSERT(Dim() == delta.Dim());
   DBG_ASSERT(tau >= 0.);
   const DenseVector* dense_delta = static_cast<const DenseVector*>(&delta);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&delta));

   if( Dim() == 0 )
   {
      return 1.0;
   }

   Number alpha = 1.;
   Number* values_x = values_;
   Number* values_delta = dense_delta->values_;
   if( homogeneous_ )
   {
      if( dense_delta->homogeneous_ )
      {
         if( dense_delta->scalar_ < 0. )
         {
            alpha = Ipopt::Min(alpha, -tau / dense_delta->scalar_ * scalar_);
         }
      }
      else
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            if( values_delta[i] < 0. )
            {
               alpha = Ipopt::Min(alpha, -tau / values_delta[i] * scalar_);
            }
         }
      }
   }
   else
   {
      if( dense_delta->homogeneous_ )
      {
         if( dense_delta->scalar_ < 0. )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               alpha = Ipopt::Min(alpha, -tau / dense_delta->scalar_ * values_x[i]);
            }
         }
      }
      else
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            if( values_delta[i] < 0. )
            {
               alpha = Ipopt::Min(alpha, -tau / values_delta[i] * values_x[i]);
            }
         }
      }
   }

   DBG_ASSERT(alpha >= 0.);
   return alpha;
}

void DenseVector::AddVectorQuotientImpl(
   Number        a,
   const Vector& z,
   const Vector& s,
   Number        c
)
{
   DBG_ASSERT(Dim() == z.Dim());
   DBG_ASSERT(Dim() == s.Dim());
   const DenseVector* dense_z = static_cast<const DenseVector*>(&z);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&z));
   const DenseVector* dense_s = static_cast<const DenseVector*>(&s);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&s));

   DBG_ASSERT(dense_z->initialized_);
   DBG_ASSERT(dense_s->initialized_);

   DBG_ASSERT(c == 0. || initialized_);
   bool homogeneous_z = dense_z->homogeneous_;
   bool homogeneous_s = dense_s->homogeneous_;

   if( Dim() == 0 )
   {
      return;
   }

   if( (c == 0. || homogeneous_) && homogeneous_z && homogeneous_s )
   {
      if( c == 0. )
      {
         scalar_ = a * dense_z->scalar_ / dense_s->scalar_;
      }
      else
      {
         scalar_ = c * scalar_ + a * dense_z->scalar_ / dense_s->scalar_;
      }
      initialized_ = true;
      homogeneous_ = true;
      if( values_ )
      {
         owner_space_->FreeInternalStorage(values_);
         values_ = NULL;
      }
      return;
   }

   // At least one is not homogeneous
   // Make sure we have memory to store a non-homogeneous vector
   values_allocated();

   Number* values_z = dense_z->values_;
   Number* values_s = dense_s->values_;

   if( c == 0. )
   {
      if( homogeneous_z )
      {
         // then s is not homogeneous
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] = a * dense_z->scalar_ / values_s[i];
         }
      }
      else if( homogeneous_s )
      {
         // then z is not homogeneous
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] = values_z[i] * a / dense_s->scalar_;
         }
      }
      else
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] = a * values_z[i] / values_s[i];
         }
      }
   }
   else if( homogeneous_ )
   {
      Number val = c * scalar_;
      if( homogeneous_z )
      {
         // then s is not homogeneous
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] = val + a * dense_z->scalar_ / values_s[i];
         }
      }
      else if( homogeneous_s )
      {
         // then z is not homogeneous
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] = val + values_z[i] * a / dense_s->scalar_;
         }
      }
      else
      {
         for( Index i = 0; i < Dim(); i++ )
         {
            values_[i] = val + a * values_z[i] / values_s[i];
         }
      }
   }
   else
   {
      // ToDo could distinguish c = 1
      if( homogeneous_z )
      {
         if( homogeneous_s )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = c * values_[i] + a * dense_z->scalar_ / dense_s->scalar_;
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = c * values_[i] + a * dense_z->scalar_ / values_s[i];
            }
         }
      }
      else
      {
         if( homogeneous_s )
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = c * values_[i] + values_z[i] * a / dense_s->scalar_;
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               values_[i] = c * values_[i] + a * values_z[i] / values_s[i];
            }
         }
      }
   }

   initialized_ = true;
   homogeneous_ = false;
}

void DenseVector::CopyToPos(
   Index         Pos,
   const Vector& x
)
{
   Index dim_x = x.Dim();
   DBG_ASSERT(dim_x + Pos <= Dim());
   const DenseVector* dense_x = static_cast<const DenseVector*>(&x);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&x));

   Number* vals = values_allocated();
   homogeneous_ = false;

   if( dense_x->homogeneous_ )
   {
      IpBlasCopy(dim_x, &scalar_, 0, vals + Pos, 1);
   }
   else
   {
      IpBlasCopy(dim_x, dense_x->values_, 1, vals + Pos, 1);
   }
   initialized_ = true;
   ObjectChanged();
}

void DenseVector::CopyFromPos(
   Index         Pos,
   const Vector& x
)
{
   DBG_ASSERT(Dim() + Pos <= x.Dim());
   const DenseVector* dense_x = static_cast<const DenseVector*>(&x);
   DBG_ASSERT(dynamic_cast<const DenseVector*>(&x));

   if( dense_x->IsHomogeneous() )
   {
      Set(dense_x->Scalar());
   }
   else
   {
      IpBlasCopy(Dim(), dense_x->Values() + Pos, 1, Values(), 1);
      initialized_ = true;
      ObjectChanged();
   }
}

void DenseVector::PrintImplOffset(
   const Journalist&  jnlst,
   EJournalLevel      level,
   EJournalCategory   category,
   const std::string& name,
   Index              indent,
   const std::string& prefix,
   Index              offset /* = 1 */
) const
{
   jnlst.PrintfIndented(level, category, indent,
                        "%sDenseVector \"%s\" with %" IPOPT_INDEX_FORMAT " elements:\n", prefix.c_str(),
                        name.c_str(), Dim());

   if( initialized_ )
   {
      if( homogeneous_ )
      {
         jnlst.PrintfIndented(level, category, indent,
                              "%sHomogeneous vector, all elements have value %23.16e\n",
                              prefix.c_str(), scalar_);
      }
      else
      {
         if( owner_space_->HasStringMetaData("idx_names") )
         {
            const std::vector<std::string>& idx_names = owner_space_->GetStringMetaData("idx_names");
            for( Index i = 0; i < Dim(); i++ )
            {
               jnlst.PrintfIndented(level, category, indent,
                                    "%s%s[%5" IPOPT_INDEX_FORMAT "]{%s}=%23.16e\n", prefix.c_str(), name.c_str(),
                                    i + offset, idx_names[i].c_str(), values_[i]);
            }
         }
         else
         {
            for( Index i = 0; i < Dim(); i++ )
            {
               jnlst.PrintfIndented(level, category, indent,
                                    "%s%s[%5" IPOPT_INDEX_FORMAT "]=%23.16e\n", prefix.c_str(), name.c_str(),
                                    i + offset, values_[i]);
            }
         }
      }
   }
   else
   {
      jnlst.PrintfIndented(level, category, indent,
                           "%sUninitialized!\n", prefix.c_str());
   }
}

} // namespace Ipopt
