/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Matrix-inl.h
 * @brief   
 * @author  Richard Roberts
 * @created Sep 26, 2010
 */

#pragma once

#include <gtsam/base/Matrix.h>

#include <iostream>

namespace gtsam {

using namespace std;

/* ************************************************************************* */
/**
 * backSubstitute U*x=b
 * @param U an upper triangular matrix
 * @param b an RHS vector
 * @return the solution x of U*x=b
 */
template<class MATRIXAE, class VECTORAE>
Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression<MATRIXAE>& _U,
    const boost::numeric::ublas::vector_expression<VECTORAE>& _b, bool unit=false) {
  const MATRIXAE& U((const MATRIXAE&)_U);
  const VECTORAE& b((const VECTORAE&)_b);
  size_t n = U.size2();
#ifndef NDEBUG
  size_t m = U.size1();
  if (m!=n)
    throw invalid_argument("backSubstituteUpper: U must be square");
#endif

  Vector result(n);
  for (size_t i = n; i > 0; i--) {
    double zi = b(i-1);
    for (size_t j = i+1; j <= n; j++)
      zi -= U(i-1,j-1) * result(j-1);
#ifndef NDEBUG
    if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
      stringstream ss;
      ss << "backSubstituteUpper: U is singular,\n";
      print(U, "U: ", ss);
      throw invalid_argument(ss.str());
    }
#endif
    if (!unit) zi /= U(i-1,i-1);
    result(i-1) = zi;
  }

  return result;
}

/* ************************************************************************* */
/**
 * backSubstitute x'*U=b'
 * @param U an upper triangular matrix
 * @param b an RHS vector
 * @param unit, set true if unit triangular
 * @return the solution x of x'*U=b'
 * TODO: use boost
 */
template<class VECTORAE, class MATRIXAE>
Vector backSubstituteUpper(const boost::numeric::ublas::vector_expression<VECTORAE>& _b,
    const boost::numeric::ublas::matrix_expression<MATRIXAE>& _U, bool unit=false) {
  const VECTORAE& b((const VECTORAE&)_b);
  const MATRIXAE& U((const MATRIXAE&)_U);
  size_t n = U.size2();
#ifndef NDEBUG
  size_t m = U.size1();
  if (m!=n)
    throw invalid_argument("backSubstituteUpper: U must be square");
#endif

  Vector result(n);
  for (size_t i = 1; i <= n; i++) {
    double zi = b(i-1);
    for (size_t j = 1; j < i; j++)
      zi -= U(j-1,i-1) * result(j-1);
#ifndef NDEBUG
    if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
      stringstream ss;
      ss << "backSubstituteUpper: U is singular,\n";
      print(U, "U: ", ss);
      throw invalid_argument(ss.str());
    }
#endif
    if (!unit) zi /= U(i-1,i-1);
    result(i-1) = zi;
  }

  return result;
}

}