#include <dune/common/bitsetvector.hh>

#include <dune/istl/io.hh>

#include <dune/ag-common/functionspacebases/p1nodalbasis.hh>
#include <dune/ag-common/assemblers/operatorassembler.hh>
#include <dune/ag-common/assemblers/localassemblers/laplaceassembler.hh>
#include <dune/ag-common/assemblers/localassemblers/massassembler.hh>

// For using a monotone multigrid as the inner solver
#include <dune-solvers/iterationsteps/trustregiongsstep.hh>
#include <dune-solvers/iterationsteps/mmgstep.hh>
#include <dune-solvers/transferoperators/truncatedcompressedmgtransfer.hh>
#include <dune-solvers/transferoperators/mandelobsrestrictor.hh>
#include <dune-solvers/solvers/iterativesolver.hh>
#include "maxnormtrustregion.hh"

// For using a truncated cg as the inner solver
#include <dune-solvers/solvers/tcgsolver.hh>

#include <dune-solvers/norms/energynorm.hh>
#include <dune-solvers/norms/h1seminorm.hh>


// for debugging
#include "../test/fdcheck.hh"

template <class GridType, class TargetSpace>
void RiemannianTrustRegionSolver<GridType,TargetSpace>::
setup(const GridType& grid,
         const GeodesicFEAssembler<typename GridType::LeafGridView,TargetSpace>* assembler,
         const SolutionType& x,
         const Dune::BitSetVector<blocksize>& dirichletNodes,
         double tolerance,
         int maxTrustRegionSteps,
         double initialTrustRegionRadius,
         int multigridIterations,
         double mgTolerance,
         int mu, 
         int nu1,
         int nu2,
         int baseIterations,
         double baseTolerance,
         bool instrumented)
{
    grid_                     = &grid;
    assembler_                = assembler;
    x_                        = x;
    this->tolerance_          = tolerance;
    maxTrustRegionSteps_      = maxTrustRegionSteps;
    initialTrustRegionRadius_ = initialTrustRegionRadius;
    innerIterations_          = multigridIterations;
    innerTolerance_           = mgTolerance;
    instrumented_             = instrumented;
    ignoreNodes_              = &dirichletNodes;

    int numLevels = grid_->maxLevel()+1;

    // ////////////////////////////////
    //   Create a multigrid solver
    // ////////////////////////////////

    // First create a Gauss-seidel base solver
    TrustRegionGSStep<MatrixType, CorrectionType>* baseSolverStep = new TrustRegionGSStep<MatrixType, CorrectionType>;

    EnergyNorm<MatrixType, CorrectionType>* baseEnergyNorm = new EnergyNorm<MatrixType, CorrectionType>(*baseSolverStep);

    ::LoopSolver<CorrectionType>* baseSolver = new ::LoopSolver<CorrectionType>(baseSolverStep,
                                                                            baseIterations,
                                                                            baseTolerance,
                                                                            baseEnergyNorm,
                                                                            Solver::QUIET);

    // Make pre and postsmoothers
    TrustRegionGSStep<MatrixType, CorrectionType>* presmoother  = new TrustRegionGSStep<MatrixType, CorrectionType>;
    TrustRegionGSStep<MatrixType, CorrectionType>* postsmoother = new TrustRegionGSStep<MatrixType, CorrectionType>;

    MonotoneMGStep<MatrixType, CorrectionType>* mmgStep = new MonotoneMGStep<MatrixType, CorrectionType>(numLevels);

    mmgStep->setMGType(mu, nu1, nu2);
    mmgStep->ignoreNodes_       = &dirichletNodes;
    mmgStep->basesolver_        = baseSolver;
    mmgStep->presmoother_       = presmoother;
    mmgStep->postsmoother_      = postsmoother; 
    mmgStep->obstacleRestrictor_= new MandelObstacleRestrictor<CorrectionType>();
    mmgStep->hasObstacle_       = &hasObstacle_;
    mmgStep->verbosity_         = Solver::QUIET;

    // //////////////////////////////////////////////////////////////////////////////////////
    //   Assemble a Laplace matrix to create a norm that's equivalent to the H1-norm
    // //////////////////////////////////////////////////////////////////////////////////////
    typedef P1NodalBasis<typename GridType::LeafGridView,double> FEBasis;
    FEBasis basis(grid.leafView());
    OperatorAssembler<FEBasis,FEBasis> operatorAssembler(basis, basis);

    LaplaceAssembler<GridType, typename FEBasis::LocalFiniteElement, typename FEBasis::LocalFiniteElement> laplaceStiffness;
    typedef Dune::BCRSMatrix<Dune::FieldMatrix<double,1,1> > ScalarMatrixType;
    ScalarMatrixType* A = new ScalarMatrixType;

    operatorAssembler.assemble(laplaceStiffness, *A);

    if (h1SemiNorm_)
        delete h1SemiNorm_;

    h1SemiNorm_ = new H1SemiNorm<CorrectionType>(*A);

    innerSolver_ = new ::LoopSolver<CorrectionType>(mmgStep,
                                                    innerIterations_,
                                                    innerTolerance_,
                                                    h1SemiNorm_,
                                                    Solver::QUIET);

    // Write all intermediate solutions, if requested
    if (instrumented_
        && dynamic_cast<IterativeSolver<CorrectionType>*>(innerSolver_))
        dynamic_cast<IterativeSolver<CorrectionType>*>(innerSolver_)->historyBuffer_ = "tmp/mgHistory";

    // ////////////////////////////////////////////////////////////
    //    Create Hessian matrix and its occupation structure
    // ////////////////////////////////////////////////////////////
    
    hessianMatrix_ = std::auto_ptr<MatrixType>(new MatrixType);
    Dune::MatrixIndexSet indices(grid_->size(1), grid_->size(1));
    assembler_->getNeighborsPerVertex(indices);
    indices.exportIdx(*hessianMatrix_);
    
    // //////////////////////////////////////////////////////////
    //   Create obstacles
    // //////////////////////////////////////////////////////////
    
    hasObstacle_.resize(numLevels);
    for (int i=0; i<hasObstacle_.size(); i++)
        hasObstacle_[i].resize(grid_->size(i, gridDim),true);
    
    // ////////////////////////////////////
    //   Create the transfer operators
    // ////////////////////////////////////
    for (int k=0; k<mmgStep->mgTransfer_.size(); k++)
        delete(mmgStep->mgTransfer_[k]);
    
    mmgStep->mgTransfer_.resize(numLevels-1);
    
    for (int i=0; i<mmgStep->mgTransfer_.size(); i++){
        TruncatedCompressedMGTransfer<CorrectionType>* newTransferOp = new TruncatedCompressedMGTransfer<CorrectionType>;
        newTransferOp->setup(*grid_,i,i+1);
        mmgStep->mgTransfer_[i] = newTransferOp;
    }
    
}


template <class GridType, class TargetSpace>
void RiemannianTrustRegionSolver<GridType,TargetSpace>::
setupTCG(const GridType& grid,
         const GeodesicFEAssembler<typename GridType::LeafGridView,TargetSpace>* assembler,
         const SolutionType& x,
         const Dune::BitSetVector<blocksize>& dirichletNodes,
         double tolerance,
         int maxTrustRegionSteps,
         double initialTrustRegionRadius,
         int innerIterations,
         double innerTolerance,
         bool instrumented)
{
    grid_                     = &grid;
    assembler_                = assembler;
    x_                        = x;
    this->tolerance_          = tolerance;
    maxTrustRegionSteps_      = maxTrustRegionSteps;
    initialTrustRegionRadius_ = initialTrustRegionRadius;
    innerIterations_          = innerIterations;
    innerTolerance_           = innerTolerance;
    instrumented_             = instrumented;
    ignoreNodes_              = &dirichletNodes;

    // ////////////////////////////////////////////////////////////
    //    Create Hessian matrix and its occupation structure
    // ////////////////////////////////////////////////////////////
    
    hessianMatrix_ = std::auto_ptr<MatrixType>(new MatrixType);
    Dune::MatrixIndexSet indices(grid_->size(1), grid_->size(1));
    assembler_->getNeighborsPerVertex(indices);
    indices.exportIdx(*hessianMatrix_);
    
    // //////////////////////////////////////////////////////////////////////////////////////
    //   Assemble a Laplace matrix to create a norm that's equivalent to the H1-norm
    //   This is used to measure convergence of the inner solver
    // //////////////////////////////////////////////////////////////////////////////////////
    typedef P1NodalBasis<typename GridType::LeafGridView,double> FEBasis;
    FEBasis basis(grid.leafView());
    OperatorAssembler<FEBasis,FEBasis> operatorAssembler(basis, basis);

    LaplaceAssembler<GridType, typename FEBasis::LocalFiniteElement, typename FEBasis::LocalFiniteElement> laplaceStiffness;
    typedef Dune::BCRSMatrix<Dune::FieldMatrix<double,1,1> > ScalarMatrixType;
    ScalarMatrixType* A = new ScalarMatrixType;

    operatorAssembler.assemble(laplaceStiffness, *A);

    if (h1SemiNorm_)
        delete h1SemiNorm_;

    h1SemiNorm_ = new H1SemiNorm<CorrectionType>(*A);

    // //////////////////////////////////////////////////////////////////////////////////////
    //   Assemble a mass matrix to create a norm that's equivalent to the L2-norm
    //   This is used to to define the trust region
    // //////////////////////////////////////////////////////////////////////////////////////

    MassAssembler<GridType, typename FEBasis::LocalFiniteElement, typename FEBasis::LocalFiniteElement> massMatrixStiffness;
    MatrixType* B = new MatrixType;

    operatorAssembler.assemble(massMatrixStiffness, *B);

    // ////////////////////////////////////////////////////
    //   Create a truncated conjugate gradient solver
    // ////////////////////////////////////////////////////

    innerSolver_ = new TruncatedCGSolver<MatrixType,CorrectionType>(NULL,   // the preconditioner
                                                                    innerIterations_,
                                                                    innerTolerance_,
                                                                    h1SemiNorm_,
                                                                    B,  // the norm of the trust region
                                                                    Solver::FULL);

    // Write all intermediate solutions, if requested
    if (instrumented_
        && dynamic_cast<IterativeSolver<CorrectionType>*>(innerSolver_))
        dynamic_cast<IterativeSolver<CorrectionType>*>(innerSolver_)->historyBuffer_ = "tmp/mgHistory";

}

template <class GridType, class TargetSpace>
void RiemannianTrustRegionSolver<GridType,TargetSpace>::solve()
{
    MonotoneMGStep<MatrixType,CorrectionType>* mgStep = NULL;

    // if the inner solver is a monotone multigrid set up a max-norm trust-region
    if (dynamic_cast<LoopSolver<CorrectionType>*>(innerSolver_)) {
        mgStep = dynamic_cast<MonotoneMGStep<MatrixType,CorrectionType>*>(dynamic_cast<LoopSolver<CorrectionType>*>(innerSolver_)->iterationStep_);
    
    }    

    MaxNormTrustRegion<blocksize> trustRegion(x_.size(), initialTrustRegionRadius_);

    std::vector<std::vector<BoxConstraint<field_type,blocksize> > > trustRegionObstacles((mgStep) 
                                                                                         ? mgStep->numLevels_
                                                                                         : 0);

   // /////////////////////////////////////////////////////
    //   Set up the log file, if requested
    // /////////////////////////////////////////////////////
    FILE* fp;
    if (instrumented_) {

        fp = fopen("statistics", "w");
        if (!fp)
            DUNE_THROW(Dune::IOError, "Couldn't open statistics file for writing!");
    
    }

    // /////////////////////////////////////////////////////
    //   Trust-Region Solver
    // /////////////////////////////////////////////////////
    for (int i=0; i<maxTrustRegionSteps_; i++) {
        
        if (this->verbosity_ == Solver::FULL) {
            std::cout << "----------------------------------------------------" << std::endl;
            std::cout << "      Trust-Region Step Number: " << i 
                      << ",     radius: " << trustRegion.radius()
                      << ",     energy: " << assembler_->computeEnergy(x_) << std::endl;
            std::cout << "----------------------------------------------------" << std::endl;
        }

        CorrectionType rhs;
        CorrectionType corr(x_.size());
        corr = 0;

        assembler_->assembleGradient(x_, rhs);
        assembler_->assembleMatrix(x_, 
                                   *hessianMatrix_, 
                                   i==0    // assemble occupation pattern only for the first call
                                   );

        //gradientFDCheck(x_, rhs, *rodAssembler_);
        //hessianFDCheck(x_, *hessianMatrix_, *rodAssembler_);

        // The right hand side is the _negative_ gradient
        rhs *= -1;


        // //////////////////////////////////////////////////////////////////////
        //   Modify matrix and right-hand side to account for Dirichlet values
        // //////////////////////////////////////////////////////////////////////

        typedef typename MatrixType::row_type::Iterator ColumnIterator;
  
        for (size_t j=0; j<ignoreNodes_->size(); j++) {
            
            if (ignoreNodes_->operator[](j).count() > 0) {
                
                // make matrix row an identity row
                ColumnIterator cIt    = (*hessianMatrix_)[j].begin();
                ColumnIterator cEndIt = (*hessianMatrix_)[j].end();
                
                for (; cIt!=cEndIt; ++cIt) {
                    for (int k=0; k<blocksize; k++) {
                        if (ignoreNodes_->operator[](j)[k])
                            (*cIt)[k] = 0;
                        if (j==cIt.index())
                            (*cIt)[k][k] = 1;
                    }
                }

                // Dirichlet value.  Zero, because we are solving defect problems
                for (int k=0; k<blocksize; k++)
                    if (ignoreNodes_->operator[](j)[k])
                        rhs[j][k] = 0;
            }

        }

        // The cg solver overwrites the rhs.  Therefore it is given a copy
        CorrectionType backupRhs = rhs;

        if (mgStep) {  // inner solver is a monotone multigrid

            mgStep->setProblem(*hessianMatrix_, corr, rhs, grid_->maxLevel()+1);

            trustRegionObstacles.back() = trustRegion.obstacles();
            mgStep->obstacles_ = &trustRegionObstacles;
        
        } else {       // inner solver is a truncated cg

            assert((dynamic_cast<TruncatedCGSolver<MatrixType,CorrectionType>*>(innerSolver_)));
            dynamic_cast<TruncatedCGSolver<MatrixType,CorrectionType>*>(innerSolver_)->setProblem(*hessianMatrix_, 
                                                                                                  &corr, 
                                                                                                  &backupRhs, 
                                                                                                  trustRegion.radius());

        }

        innerSolver_->preprocess();
        
        // /////////////////////////////
        //    Solve !
        // /////////////////////////////
        
        innerSolver_->solve();
        
        if (mgStep)
            corr = mgStep->getSol();

        //std::cout << "Correction: " << std::endl << corr << std::endl;

        if (instrumented_) {

            fprintf(fp, "Trust-region step: %d, trust-region radius: %g\n",
                    i, trustRegion.radius());
                    
            // ///////////////////////////////////////////////////////////////
            //   Compute and measure progress against the exact solution
            //   for each trust region step
            // ///////////////////////////////////////////////////////////////
    
            CorrectionType exactSolution = corr;

            // Start from 0
            double oldError = 0;
            double totalConvRate = 1;
            double convRate = 1;
    
            // Write statistics of the initial solution
            // Compute the energy norm
            oldError = h1SemiNorm_->operator()(exactSolution);
    
            for (int j=0; j<innerIterations_; j++) {
        
                // read iteration from file
                CorrectionType intermediateSol(grid_->size(1));
                intermediateSol = 0;
                char iSolFilename[100];
                sprintf(iSolFilename, "tmp/mgHistory/intermediatesolution_%04d", j);
                
                FILE* fpInt = fopen(iSolFilename, "rb");
                if (!fpInt)
                    DUNE_THROW(Dune::IOError, "Couldn't open intermediate solution");
                for (int k=0; k<intermediateSol.size(); k++)
                    for (int l=0; l<blocksize; l++)
                        fread(&intermediateSol[k][l], sizeof(double), 1, fpInt);
                
                fclose(fpInt);
                //std::cout << "intermediateSol\n" << intermediateSol << std::endl;

                // Compute errors
                intermediateSol -= exactSolution;
      
                //std::cout << "error\n" << intermediateSol << std::endl;

                // Compute the H1 norm
                double error = h1SemiNorm_->operator()(intermediateSol);

                convRate = error / oldError;
                totalConvRate *= convRate;
                
                if (error < 1e-12)
                    break;
                
                std::cout << "Iteration: " << j << "  ";
                std::cout << "Errors:  error " << error << ", convergence rate: " << convRate
                          << ",  total conv rate " << pow(totalConvRate, 1/((double)j+1)) << std::endl;
                

                fprintf(fp, "%d %g %g %g\n", j+1, error, convRate, pow(totalConvRate, 1/((double)j+1)));
                
                
                oldError = error;
                
            }
            
            
        }

        if (this->verbosity_ == NumProc::FULL)
            std::cout << "Infinity norm of the correction: " << corr.infinity_norm() << std::endl;

        if (corr.infinity_norm() < this->tolerance_) {
            if (this->verbosity_ == NumProc::FULL)
                std::cout << "CORRECTION IS SMALL ENOUGH" << std::endl;

            if (this->verbosity_ != NumProc::QUIET)
                std::cout << i+1 << " trust-region steps were taken." << std::endl;
            break;
        }
        
        // ////////////////////////////////////////////////////
        //   Check whether trust-region step can be accepted
        // ////////////////////////////////////////////////////
        
        SolutionType newIterate = x_;
        for (int j=0; j<newIterate.size(); j++) 
            newIterate[j] = TargetSpace::exp(newIterate[j], corr[j]);
        
        /** \todo Don't always recompute oldEnergy */
        double oldEnergy = assembler_->computeEnergy(x_);
        double energy    = assembler_->computeEnergy(newIterate); 
        
        // compute the model decrease
        // It is $ m(x) - m(x+s) = -<g,s> - 0.5 <s, Hs>
        // Note that rhs = -g
        CorrectionType tmp(corr.size());
        tmp = 0;
        hessianMatrix_->umv(corr, tmp);
        double modelDecrease = (rhs*corr) - 0.5 * (corr*tmp);
        
        if (this->verbosity_ == NumProc::FULL) {
            std::cout << "Absolute model decrease: " << modelDecrease 
                      << ",  functional decrease: " << oldEnergy - energy << std::endl;
            std::cout << "Relative model decrease: " << modelDecrease / energy
                      << ",  functional decrease: " << (oldEnergy - energy)/energy << std::endl;
        }            

        assert(modelDecrease >= 0);
        
        if (energy >= oldEnergy) {
            if (this->verbosity_ == NumProc::FULL)
                printf("Richtung ist keine Abstiegsrichtung!\n");
        }

        if (energy >= oldEnergy &&
            (std::abs(oldEnergy-energy)/energy < 1e-9 || modelDecrease/energy < 1e-9)) {
            if (this->verbosity_ == NumProc::FULL)
                std::cout << "Suspecting rounding problems" << std::endl;

            if (this->verbosity_ != NumProc::QUIET)
                std::cout << i+1 << " trust-region steps were taken." << std::endl;

            x_ = newIterate;
            break;
        }

        // //////////////////////////////////////////////
        //   Check for acceptance of the step
        // //////////////////////////////////////////////
        if ( (oldEnergy-energy) / modelDecrease > 0.9) {
            // very successful iteration
            
            x_ = newIterate;
            trustRegion.scale(2);
            
        } else if ( (oldEnergy-energy) / modelDecrease > 0.01
                    || std::abs(oldEnergy-energy) < 1e-12) {
            // successful iteration
            x_ = newIterate;
            
        } else {
            // unsuccessful iteration
            trustRegion.scale(0.5);
            if (this->verbosity_ == NumProc::FULL)
                std::cout << "Unsuccessful iteration!" << std::endl;
        }
        
        //  Write current energy
        if (this->verbosity_ == NumProc::FULL)
            std::cout << "--- Current energy: " << energy << " ---" << std::endl;

        // /////////////////////////////////////////////////////////////////////
        //   Write the iterate to disk for later convergence rate measurement
        // /////////////////////////////////////////////////////////////////////

        if (instrumented_) {

            char iRodFilename[100];
            sprintf(iRodFilename, "tmp/intermediateSolution_%04d", i);

            FILE* fpRod = fopen(iRodFilename, "wb");
            if (!fpRod)
                DUNE_THROW(SolverError, "Couldn't open file " << iRodFilename << " for writing");
            
            for (int j=0; j<x_.size(); j++)
                fwrite(&x_[j], sizeof(TargetSpace), 1, fpRod);

            fclose(fpRod);

        }

    }

    // //////////////////////////////////////////////
    //   Close logfile
    // //////////////////////////////////////////////
    if (instrumented_)
        fclose(fp);
}