#include <cmath>
#include <limits>
#include <iostream>
#include <iomanip>
#include <fstream>

#include <dune/solvers/norms/energynorm.hh>
#include <dune/matrix-vector/genericvectortools.hh>


template <class MatrixType, class VectorType, class DGBasisType>
void OscCGSolver<MatrixType, VectorType, DGBasisType>::check() const
{
    if (preconditioner_)
        preconditioner_->check();

    if (!errorNorm_)
        DUNE_THROW(SolverError, "You need to supply a norm to a CG solver!");

}

template <class MatrixType, class VectorType, class DGBasisType>
void OscCGSolver<MatrixType, VectorType, DGBasisType>::writeSolution(const VectorType& iterate) const
{
    std::stringstream solFilename;
    solFilename << this->historyBuffer_;

    std::ofstream file(solFilename.str().c_str(), std::ios::out|std::ios::binary);
    if (not(file))
        DUNE_THROW(SolverError, "Couldn't open file " << solFilename.str() << " for writing");

    Dune::MatrixVector::Generic::writeBinary(file, iterate);

    file.close();
}

template <class MatrixType, class VectorType, class DGBasisType>
void OscCGSolver<MatrixType, VectorType, DGBasisType>::solve()
{

    EnergyNorm<MatrixType, VectorType> fineErrorNorm(*matrix_);


    int i;

    VectorType& b = *rhs_;

    // Check whether the solver is set up properly
    check();

    // set up preconditioner
    if (preconditioner_)
        preconditioner_->preprocess();

    if (this->verbosity_ != NumProc::QUIET)
        std::cout << "--- CGSolver ---\n";

    if (this->verbosity_ == NumProc::FULL)
    {
        std::cout << " iter";
        std::cout << "      discError";
        std::cout << "     rate";
        std::cout << "  criterion";
	std::cout << "        history";
        std::string header;
        if (preconditioner_) {
            header = preconditioner_->getOutput();
            std::cout << header;
        }
        std::cout << std::endl;
        std::cout << "-----";
        std::cout << "---------------";
        std::cout << "---------";
	std::cout << "-----------";
	std::cout << "---------------";
        for(size_t i=0; i<header.size(); ++i)
            std::cout << "-";
        std::cout << std::endl;
    }

    double discretizationError = std::numeric_limits<double>::max();

    VectorType finalDiscretizationError;
    mgTransfer_.prolong(*fineSol_, finalDiscretizationError);

    finalDiscretizationError -= *exactSol_;
    double normOfFinalDiscretizationError = errorNorm_->operator()(finalDiscretizationError);

    VectorType initialDiscretizationError;
    mgTransfer_.prolong(*x_, initialDiscretizationError);
    initialDiscretizationError -= *exactSol_;

    double normOfInitialDiscretizationError = errorNorm_->operator()(initialDiscretizationError);
    //double oldNormOfDiscretizationError = normOfInitialDiscretizationError;

    VectorType initialIterationError(*x_);
    initialIterationError -= *fineSol_;
    double normOfOldIterationError = fineErrorNorm.operator()(initialIterationError);

    double totalConvRate = 1;
    this->maxTotalConvRate_ = 0;
    int convRateCounter = 0;

    // overwrite b with residual
    matrix_->mmv(*x_,b);

    VectorType p(*x_);              // the search direction
    VectorType q(*x_);              // a temporary vector
    
    // some local variables
    double rho,rholast,lambda,alpha,beta;
    
    // determine initial search direction
    p = 0;                          // clear correction

    if (preconditioner_) {
        preconditioner_->setProblem(*matrix_,p,b);
        preconditioner_->iterate();
        p = preconditioner_->getSol();
    } else
        p = b;

    rholast = p*b;         // orthogonalization

    // Loop until desired tolerance or maximum number of iterations is reached
    for (i=0; i<this->maxIterations_ && (discretizationError>this->tolerance_ || std::isnan(discretizationError)); i++) {

        VectorType oldX;
        mgTransfer_.prolong(*x_, oldX);

        // Perform one iteration step
        // minimize in given search direction p

        matrix_->mv(p,q);       // q=Ap
        alpha = p*q;            // scalar product
        lambda = rholast/alpha; // minimization
        x_->axpy(lambda,p);     // update solution

        b.axpy(-lambda,q);      // update residual

        // determine new search direction
        q = 0;                  // clear correction

        // apply preconditioner
        if (preconditioner_) {
            preconditioner_->setProblem(*matrix_,q,b);
            preconditioner_->iterate();
            q = preconditioner_->getSol();

        } else
            q = b;
        
        rho = q*b;              // orthogonalization
        beta = rho/rholast;     // scaling factor
        p *= beta;              // scale old search direction
        p += q;                 // orthogonalization with correction
        rholast = rho;          // remember rho for recurrence

        // Compute error
        VectorType currentDiscretizationError;
        mgTransfer_.prolong(*x_, currentDiscretizationError);

        currentDiscretizationError -= *exactSol_;
        double normOfDiscretizationError = errorNorm_->operator()(currentDiscretizationError);

        if (this->useRelativeError_)
            discretizationError = normOfDiscretizationError / normOfInitialDiscretizationError;
        else
            discretizationError = normOfDiscretizationError;

	VectorType iterationError(*x_);
	iterationError -= *fineSol_;

	double normOfIterationError = fineErrorNorm.operator()(iterationError);
    double convRate = normOfIterationError / normOfOldIterationError;

    normOfOldIterationError = normOfIterationError;

	double xNorm = fineErrorNorm.operator()(*x_);
	double history = normOfIterationError/xNorm;

	bool criterion = ( normOfIterationError <= terminationFactor_*normOfFinalDiscretizationError);

        if (!std::isinf(convRate) && !std::isnan(convRate)) {
            totalConvRate *= convRate;
            this->maxTotalConvRate_ = std::max(this->maxTotalConvRate_, std::pow(totalConvRate, 1/((double)convRateCounter+1)));
            convRateCounter++;
        }

        // Output
        if (this->verbosity_ == NumProc::FULL) {
            std::cout << std::setw(5) << i;

            std::cout << std::setiosflags(std::ios::scientific);
            std::cout << std::setw(15) << std::setprecision(7) << discretizationError;
            std::cout << std::resetiosflags(std::ios::scientific);

            std::cout << std::setiosflags(std::ios::fixed);
            std::cout << std::setw(9) << std::setprecision(5) << convRate;
            std::cout << std::resetiosflags(std::ios::fixed);

            std::cout << std::setiosflags(std::ios::fixed);
            std::cout << std::setw(11) << criterion;
            std::cout << std::resetiosflags(std::ios::fixed);

            std::cout << std::setiosflags(std::ios::scientific);
            std::cout << std::setw(15) << std::setprecision(7) << history;
            std::cout << std::resetiosflags(std::ios::scientific);

            std::cout << std::setiosflags(std::ios::scientific);
            std::cout << std::setw(15) << std::setprecision(7) << normOfIterationError;
            std::cout << std::resetiosflags(std::ios::scientific);

            std::cout << std::setiosflags(std::ios::scientific);
            std::cout << std::setw(15) << std::setprecision(7) << (terminationFactor_*normOfFinalDiscretizationError);
            std::cout << std::resetiosflags(std::ios::scientific);

            std::cout << std::endl;
        }

    }

    // write last iterate to file, if requested
    if (this->historyBuffer_!="")
        this->writeSolution(*x_);


    if (this->verbosity_ != NumProc::QUIET) {
      std::cout << "maxTotalConvRate: " << this->maxTotalConvRate_ << ",   "
                << "totalConvRate: " <<  std::pow(totalConvRate, 1/((double)convRateCounter)) << ",  "
                << i << " iterations performed\n";
        std::cout << "--------------------\n";


       std::cout << "Norm of final discretization error: " << discretizationError << std::endl;
    }
}