#include <config.h>

#include <fenv.h>
#include <iostream>
#include <array>

#include <dune/common/fvector.hh>
#include <dune/localfunctions/lagrange/lagrangelfecache.hh>

#include <dune/gfe/spaces/productmanifold.hh>
#include <dune/gfe/spaces/realtuple.hh>
#include <dune/gfe/spaces/rotation.hh>
#include <dune/gfe/spaces/unitvector.hh>

#include <dune/gfe/localgeodesicfefunction.hh>
#include <dune/gfe/localgfetestfunctionbasis.hh>

#include "multiindex.hh"
#include "valuefactory.hh"

const double eps = 1e-6;

using namespace Dune;

template <class TargetSpace, int domainDim>
void test()
{
  std::cout << " --- Testing " << className<TargetSpace>() << ", domain dimension: " << domainDim << " ---" << std::endl;

  std::vector<TargetSpace> testPoints;
  ValueFactory<TargetSpace>::get(testPoints);

  int nTestPoints = testPoints.size();

  // Set up elements of SO(3)
  std::vector<TargetSpace> coefficients(domainDim+1);

  MultiIndex index(domainDim+1, nTestPoints);
  int numIndices = index.cycle();

  LagrangeLocalFiniteElementCache<double,double,domainDim,1> feCache;
  typedef typename LagrangeLocalFiniteElementCache<double,double,domainDim,1>::FiniteElementType LocalFiniteElement;

  for (int i=0; i<numIndices; i++, ++index) {

    for (int j=0; j<domainDim+1; j++)
      coefficients[j] = testPoints[index[j]];

    LocalGfeTestFunctionFiniteElement<LocalFiniteElement,TargetSpace> testFunctionSet(feCache.get(GeometryTypes::simplex(domainDim)),coefficients);

    FieldVector<double,domainDim> stupidTestPoint(0);

    // test whether evaluation of the shape functions works
    std::vector<std::array<typename TargetSpace::EmbeddedTangentVector, TargetSpace::TangentVector::dimension> > values;
    testFunctionSet.localBasis().evaluateFunction(stupidTestPoint, values);

    // test whether evaluation of the shape function derivatives works
    std::vector<std::array<FieldMatrix<double, TargetSpace::EmbeddedTangentVector::dimension, domainDim>, TargetSpace::TangentVector::dimension> > derivatives;
    testFunctionSet.localBasis().evaluateJacobian(stupidTestPoint, derivatives);

  }

}


int main() try
{
  // choke on NaN -- don't enable this by default, as there are
  // a few harmless NaN in the loopsolver
  //feenableexcept(FE_INVALID);

  std::cout << std::setw(15) << std::setprecision(12);

  test<RealTuple<double,1>, 1>();

  test<UnitVector<double,2>, 1>();
  test<UnitVector<double,3>, 1>();
  test<UnitVector<double,2>, 2>();
  test<UnitVector<double,3>, 2>();

  test<Rotation<double,3>, 1>();
  test<Rotation<double,3>, 2>();
  typedef Dune::GFE::ProductManifold<RealTuple<double,1>,Rotation<double,3>,UnitVector<double,2> > CrazyManifold;
  test<CrazyManifold, 2>();

}
catch (Exception& e) {
  std::cout << e.what() << std::endl;
}