#include <config.h>

#include <dune/grid/onedgrid.hh>

#include <dune/fem/lagrangebase.hh>
#include <dune/grid/common/gridpart.hh>

#include <dune/istl/io.hh>

//#include "../common/boundarytreatment.hh"
#include "../common/boundarypatch.hh"
#include <dune/common/bitfield.hh>
//#include "../common/readbitfield.hh"

#include "src/rodassembler.hh"
//#include "../common/linearipopt.hh"

#include "../common/projectedblockgsstep.hh"
#include "../contact/src/contactmmgstep.hh"

#include <dune/solver/iterativesolver.hh>

#include "../common/geomestimator.hh"
#include "../common/energynorm.hh"
#include <dune/common/configparser.hh>
#include "src/rodwriter.hh"

// Choose a solver
//#define IPOPT
//#define GAUSS_SEIDEL
#define MULTIGRID

// Number of degrees of freedom: 
// 3 (x, y, theta) for a planar rod
const int blocksize = 3;

using namespace Dune;
using std::string;

int main (int argc, char *argv[]) try
{
    // Some types that I need
    typedef BCRSMatrix<FieldMatrix<double, blocksize, blocksize> > MatrixType;
    typedef BlockVector<FieldVector<double, blocksize> >     VectorType;

    // parse data file
    ConfigParser parameterSet;
    parameterSet.parseFile("staticrod.parset");

    // read solver settings
    const int maxLevel         = parameterSet.get("maxLevel", int(0));
    double loadIncrement       = parameterSet.get("loadIncrement", double(0));
    const int maxNewtonSteps   = parameterSet.get("maxNewtonSteps", int(0));
    const int numIt            = parameterSet.get("numIt", int(0));
    const int nu1              = parameterSet.get("nu1", int(0));
    const int nu2              = parameterSet.get("nu2", int(0));
    const int mu               = parameterSet.get("mu", int(0));
    const int baseIt           = parameterSet.get("baseIt", int(0));
    const double tolerance     = parameterSet.get("tolerance", double(0));
    const double baseTolerance = parameterSet.get("baseTolerance", double(0));
    
    // Problem settings
    const int numRodBaseElements = parameterSet.get("numRodBaseElements", int(0));

    // ///////////////////////////////////////
    //    Create the two grids
    // ///////////////////////////////////////
    typedef OneDGrid<1,1> RodGridType;
    RodGridType rod(numRodBaseElements, 0, 1);

    // refine uniformly until maxLevel
    for (int i=0; i<maxLevel; i++)
        rod.globalRefine(1);

    int maxlevel = rod.maxlevel();
    int numRodElements = rod.size(maxlevel, 0);

    
    Array<BitField> dirichletNodes;
    dirichletNodes.resize(maxLevel+1);
    for (int i=0; i<=maxlevel; i++) {

        dirichletNodes[i].resize( blocksize * rod.size(i,1) );

        dirichletNodes[i].unsetAll();

        for (int j=0; j<blocksize; j++) {
            dirichletNodes[i][j] = true;
            dirichletNodes[i][dirichletNodes[i].size()-1-j] = true;
        }
    }

    // //////////////////////////////////////////////////////////
    //    Create discrete function spaces
    // //////////////////////////////////////////////////////////

    typedef FunctionSpace < double , double, 1, 1 > RodFuncSpace;
    typedef LevelGridPart<RodGridType> RodGridPartType;
    typedef LagrangeDiscreteFunctionSpace < RodFuncSpace, RodGridPartType, 1> RodFuncSpaceType;

    Array<RodGridPartType*> rodGridPart(maxlevel+1);
    Array<const RodFuncSpaceType*> rodFuncSpace(maxlevel+1);

    for (int i=0; i<maxlevel+1; i++) {
        rodGridPart[i]  = new RodGridPartType(rod, i);
        rodFuncSpace[i] = new RodFuncSpaceType(*rodGridPart[i]);
    }


    // ////////////////////////////////////////////////////////////
    //    Create solution and rhs vectors
    // ////////////////////////////////////////////////////////////

    VectorType rhs;
    VectorType x;
    VectorType corr;

    MatrixType hessianMatrix;
    RodAssembler<RodFuncSpaceType, 4> rodAssembler(*rodFuncSpace[maxlevel]);
    rodAssembler.setParameters(1, 100, 100);

    MatrixIndexSet indices(numRodElements+1, numRodElements+1);
    rodAssembler.getNeighborsPerVertex(indices);
    indices.exportIdx(hessianMatrix);


    rhs.resize(rodFuncSpace[maxlevel]->size());
    x.resize(rodFuncSpace[maxlevel]->size());
    corr.resize(rodFuncSpace[maxlevel]->size());
    
    // Initial solution
    x = 0;

    for (int i=0; i<numRodElements+1; i++) {
        x[i][0] = i/((double)numRodElements);
        x[i][1] = 0;
        x[i][2] = M_PI/2;
    }

    x[0][0] = x[numRodElements][0] = 0;
    x[0][1] = x[numRodElements][1] = 0;

    x[0][2] = 0;
    x[numRodElements][2] = 2*M_PI;

    // //////////////////////////////////////////////////////////
    //   Create obstacles
    // //////////////////////////////////////////////////////////

    Array<BitField> hasObstacle;
    hasObstacle.resize(maxLevel+1);
    for (int i=0; i<hasObstacle.size(); i++) {
        hasObstacle[i].resize(rod.size(i, 1));
        hasObstacle[i].setAll();
    }

    Array<SimpleVector<BoxConstraint<3> > > obstacles(maxlevel+1);

    for (int i=0; i<obstacles.size(); i++)
        obstacles[i].resize(rod.size(i,1));

    for (int i=0; i<obstacles[maxlevel].size(); i++) {
        obstacles[maxlevel][i].clear();
        obstacles[maxlevel][i].val[0] =     - x[i][0];
        obstacles[maxlevel][i].val[1] = 0.1 - x[i][0];
    }

    // Create a solver
#if defined IPOPT

    typedef LinearIPOptSolver<VectorType> SolverType;
    
    SolverType solver;
    solver.dirichletNodes_ = &totalDirichletNodes[maxlevel];
    solver.hasObstacle_    = &contactAssembler.hasObstacle_[maxlevel];
    solver.obstacles_      = &contactAssembler.obstacles_[maxlevel];
    solver.verbosity_      = Solver::FULL;

#elif defined GAUSS_SEIDEL

    typedef ProjectedBlockGSStep<MatrixType, VectorType> SmootherType;
    SmootherType projectedBlockGSStep(hessianMatrix, corr, rhs);
    projectedBlockGSStep.dirichletNodes_ = &dirichletNodes[maxlevel];
    projectedBlockGSStep.hasObstacle_    = &hasObstacle[maxlevel];
    projectedBlockGSStep.obstacles_      = &obstacles;

    EnergyNorm<MatrixType, VectorType> energyNorm(projectedBlockGSStep);

    IterativeSolver<MatrixType, VectorType> solver;
    solver.iterationStep = &projectedBlockGSStep;
    solver.numIt = numIt;
    solver.verbosity_ = Solver::FULL;
    solver.errorNorm_ = &energyNorm;
    solver.tolerance_ = tolerance;
    
#elif defined MULTIGRID

    // First create a gauss-seidel base solver
    ProjectedBlockGSStep<MatrixType, VectorType> baseSolverStep;

    EnergyNorm<MatrixType, VectorType> baseEnergyNorm(baseSolverStep);

    IterativeSolver<MatrixType, VectorType> baseSolver;
    baseSolver.iterationStep = &baseSolverStep;
    baseSolver.numIt = baseIt;
    baseSolver.verbosity_ = Solver::QUIET;
    baseSolver.errorNorm_ = &baseEnergyNorm;
    baseSolver.tolerance_ = baseTolerance;

    // Make pre and postsmoothers
    ProjectedBlockGSStep<MatrixType, VectorType> presmoother;
    ProjectedBlockGSStep<MatrixType, VectorType> postsmoother;

    ContactMMGStep<MatrixType, VectorType, RodFuncSpaceType > contactMMGStep(maxlevel+1);

    contactMMGStep.setMGType(mu, nu1, nu2);
    contactMMGStep.dirichletNodes_    = &dirichletNodes;
    contactMMGStep.basesolver_        = &baseSolver;
    contactMMGStep.presmoother_       = &presmoother;
    contactMMGStep.postsmoother_      = &postsmoother;    
    contactMMGStep.hasObstacle_       = &hasObstacle;
    contactMMGStep.obstacles_         = &obstacles;

    // Create the transfer operators
    contactMMGStep.mgTransfer_.resize(maxlevel);
    for (int i=0; i<contactMMGStep.mgTransfer_.size(); i++){
        TruncatedMGTransfer<VectorType>* newTransferOp = new TruncatedMGTransfer<VectorType>;
        newTransferOp->setup(*rodFuncSpace[i], *rodFuncSpace[i+1]);
        contactMMGStep.mgTransfer_[i] = newTransferOp;
    }

    EnergyNorm<MatrixType, VectorType> energyNorm(contactMMGStep);

    IterativeSolver<MatrixType, VectorType> solver;
    solver.iterationStep = &contactMMGStep;
    solver.numIt = numIt;
    solver.verbosity_ = Solver::QUIET;
    solver.errorNorm_ = &energyNorm;
    solver.tolerance_ = tolerance;

#else
    #warning You have to specify a solver!
#endif

    // ///////////////////////////////////////////////////
    //   Do a homotopy of the Dirichlet boundary data
    // ///////////////////////////////////////////////////
    double loadFactor = 0;

    do {

        loadFactor += loadIncrement;

        std::cout << "####################################################" << std::endl;
        std::cout << "New load factor: " << loadFactor 
                  << "    new load increment: " << loadIncrement << std::endl;
        std::cout << "####################################################" << std::endl;

        // The continuation variable determines the material parameters
        double A1 = loadFactor * 1000;
        double A3 = loadFactor * 1000;
        rodAssembler.setParameters(1, A1, A3);

        // /////////////////////////////////////////////////////
        //   Newton Solver
        // /////////////////////////////////////////////////////

        for (int j=0; j<maxNewtonSteps; j++) {

            std::cout << "----------------------------------------------------" << std::endl;
            std::cout << "      Newton Step Number: " << j << std::endl;
            std::cout << "----------------------------------------------------" << std::endl;

            rhs = 0;
            corr = 0;

            //std::cout <<"Solution: " << x << std::endl;
            rodAssembler.assembleGradient(x, rhs);
            rodAssembler.assembleMatrix(x, hessianMatrix);

            rhs *= -1;

            //std::cout << "rhs: " << std::endl << rhs << std::endl;

#ifndef IPOPT
            solver.iterationStep->setProblem(hessianMatrix, corr, rhs);
#else
            solver.setProblem(hessianMatrix, corr, rhs);
#endif

            solver.preprocess();
#ifdef MULTIGRID

            contactMMGStep.preprocess();
#endif

            // /////////////////////////////
            //    Solve !
            // /////////////////////////////
             solver.solve();

#ifdef MULTIGRID
             corr = contactMMGStep.getSol();
#endif

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

             // line search
             printf("------  Line Search ---------\n");
             int lSSteps = 10;
             double smallestEnergy = std::numeric_limits<double>::max();
             double smallestFactor = 1;
             for (int k=0; k<lSSteps; k++) {

                 double factor = double(k)/(lSSteps-1);
                 VectorType sCorr = corr;
                 sCorr *= factor;
                 sCorr += x;

                 double energy = rodAssembler.computeEnergy(sCorr);

                 if (energy < smallestEnergy) {
                     smallestEnergy = energy;
                     smallestFactor = factor;
                 }
                 printf("factor: %g,  energy: %e\n", factor, energy);
             }

             std::cout << "Damping factor: " << smallestFactor << std::endl;
             //exit(0);
             //  Add correction to the current solution
             x.axpy(smallestFactor, corr);

             // Output result
             //std::cout << "Solution:" << std::endl << x << std::endl;

             printf("infinity norm of the correction: %g\n", smallestFactor*corr.infinity_norm());
             if (smallestFactor*corr.infinity_norm() < 1e-8)
                 break;

             // Subtract correction from the current obstacle
             for (int k=0; k<corr.size(); k++) {
                 FieldVector<double, blocksize> tmp = corr[k];
                 tmp *= smallestFactor;
                 obstacles[maxlevel][k] -= tmp;
             }

        }
        
        // Write Lagrange multiplyers
        std::stringstream a1AsAscii, a3AsAscii;
        a1AsAscii << A1;
        a3AsAscii << A3;
        std::string lagrangeFilename = "pressure/lagrange_" + a1AsAscii.str() + "_" + a3AsAscii.str();
        std::ofstream lagrangeFile(lagrangeFilename.c_str());
        
        VectorType lagrangeMultipliers;
        rodAssembler.assembleGradient(x, lagrangeMultipliers);
        lagrangeFile << lagrangeMultipliers << std::endl;
        
        // Write result grid
        std::string solutionFilename = "solutions/rod_" + a1AsAscii.str() 
            + "_" + a3AsAscii.str() + ".result";
        writeRod(x, solutionFilename);

    } while (loadFactor < 1);




 } catch (Exception e) {

    std::cout << e << std::endl;

 }