#include <unicorn/MeshQuality.h>
#include <unicorn/EquiAffineMap.h>
#include <dolfin/Cell.h>
#include <dolfin/Edge.h>
#include <dolfin/Vertex.h>

using namespace dolfin::unicorn;
using namespace dolfin;


MeshQuality::MeshQuality(Mesh& mesh) : mesh(mesh), orientation(mesh)
{
  // Initialize orientation
  orientation.init(mesh.topology().dim());

  for (CellIterator c(mesh); !c.end(); ++c)
  {
    Cell& cell = *c;
    orientation.set(cell.index(), (uint)cell.orientation());
  }
}

bool MeshQuality::isInverted(uint& first)
{
  for (CellIterator c(mesh); !c.end(); ++c)
  {
    if(orientation.get(c->index()) != c->orientation())
    {
      first = c->index();
      return true;
    }
  }  
  return false;
}

real MeshQuality::cellQuality(Cell& cell)
{
  EquiAffineMap map;
  map.update(cell);
  
  real det = fabs(map.det);
  real Fnorm = ublas::norm_frobenius(map.B);
  
  int d = cell.mesh().topology().dim();

  real mu = d * pow(det, 2.0 / d) / pow(Fnorm, 2.0);

  return mu;
}

void MeshQuality::meshQuality(real& mu_min, real& mu_max, real& mu_avg,
			      real& h_min, real& h_max, real& h_avg)
{
  mu_max = 0.0;
  mu_min = 1.0;

  h_max = 0.0;
  h_min = 1.0e12;

  real mu_sum = 0.0;
  real h_sum = 0.0;

  //int d = mesh.topology().dim();

  for (CellIterator c(mesh); !c.end(); ++c)
  {
    Cell& cell = *c;
    
    real mu = cellQuality(cell);
    //real h = cell.diameter();
    real h = myDiameter(cell);
    
    mu_sum += mu;
    h_sum += h;

    mu_max = std::max(mu_max, mu);
    mu_min = std::min(mu_min, mu);
    
    h_max = std::max(h_max, h);
    h_min = std::min(h_min, h);

//     if(h > 1.0)
//     {
//       cout << "Large h: " << h << endl;
//       cout << "id: " << cell.index() << endl;
//       cout << "mp: " << cell.midpoint() << endl;
//       for (VertexIterator n(cell); !n.end(); ++n) {
// 	cout << "vertex: " << n->point() << endl;
//       }
//     }

    
  }

  mu_avg = mu_sum / mesh.numCells();
  h_avg = h_sum / mesh.numCells();
}
//-----------------------------------------------------------------------------
real MeshQuality::myDiameter(Cell& cell)
{
  real hmax = 0.0;
  for (EdgeIterator edge(cell); !edge.end(); ++edge)
  {
    Edge& e = *edge;
    
    real h = e.length();
    
    hmax = std::max(hmax, h);
  }
  return hmax;
}
//-----------------------------------------------------------------------------
