/**
 * @file   givens.hpp
 * @author Julian Iseringhausen <isering@ins.uni-bonn.de>
 * @date   Mon Apr 18 13:10:15 2011
 *
 * @brief  Solves system of linear equations using Givens rotations.
 *         Compare to Meister, Numerik lineare Gleichungssysteme.
 *
 */

#ifndef GIVENS_HPP_
#define GIVENS_HPP_

#include <cfloat>

#include "solver/solver.hpp"

namespace VMG
{

template<class T>
class Givens : public T
{
public:
  Givens() :
    T()
  {}

  Givens(int size) :
    T(size)
  {}

protected:
  void Compute();
};

template<class T>
void Givens<T>::Compute()
{
  int n = this->Size();
  vmg_float c,s,t;

  for (int i=0; i<n-1; i++)
    for (int j=i+1; j<n; j++)
      if (fabs(this->Mat(j,i)) > DBL_EPSILON) {

	t = 1.0 / sqrt(this->Mat(i,i)*this->Mat(i,i) + this->Mat(j,i)*this->Mat(j,i));
	s = t * this->Mat(j,i);
	c = t * this->Mat(i,i);

	for (int k=i; k<n; k++) {

	  t = c * this->Mat(i,k) + s * this->Mat(j,k);

	  if (k != i)
	    this->Mat(j,k) = c * this->Mat(j,k) - s * this->Mat(i,k);

	  this->Mat(i,k) = t;

	}

	t = c * this->Rhs(i) + s * this->Rhs(j);

	this->Rhs(j) = c * this->Rhs(j) - s * this->Rhs(i);

	this->Rhs(i) = t;

	this->Mat(j,i) = 0.0;

      }

  for (int i=n-1; i>=0; i--) {

    for (int j=i+1; j<n; j++)
      this->Rhs(i) -= this->Mat(i,j) * this->Sol(j);

    this->Sol(i) = this->Rhs(i) / this->Mat(i,i);

  }
}

}

#endif /* GIVENS_HPP_ */