/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

// disable debug warnings in MS compiler
#ifdef _MSC_VER
#  pragma warning(disable : 4786)
#endif

#include "itkFEMElement3DC0LinearTriangular.h"
#include "itkMath.h"

#include "vnl/algo/vnl_svd.h"
#include "vnl/algo/vnl_qr.h"
namespace itk
{
namespace fem
{
const Element3DC0LinearTriangular::Float Element3DC0LinearTriangular ::trigGaussRuleInfo[6][7][4] = {
  // order=0, never used
  { { 0.0 } },
  // order=1
  // <-------------------------- point ---------------------------> <-------weight----->
  { { 0.33333333333333333, 0.33333333333333333, 0.33333333333333333, 1.00000000000000000 } },
  // order=2
  { { 0.66666666666666667, 0.16666666666666667, 0.16666666666666667, 0.33333333333333333 },
    { 0.16666666666666667, 0.66666666666666667, 0.16666666666666667, 0.33333333333333333 },
    { 0.16666666666666667, 0.16666666666666667, 0.66666666666666667, 0.33333333333333333 } },
  // order=3, p=-3 in the book
  { { 0.00000000000000000, 0.50000000000000000, 0.50000000000000000, 0.33333333333333333 },
    { 0.50000000000000000, 0.00000000000000000, 0.50000000000000000, 0.33333333333333333 },
    { 0.50000000000000000, 0.50000000000000000, 0.00000000000000000, 0.33333333333333333 } },
  // order=4, p=6 in the book
  { { 0.10810301816807023, 0.44594849091596489, 0.44594849091596489, 0.22338158967801147 },
    { 0.44594849091596489, 0.10810301816807023, 0.44594849091596489, 0.22338158967801147 },
    { 0.44594849091596489, 0.44594849091596489, 0.10810301816807023, 0.22338158967801147 },
    { 0.81684757298045851, 0.09157621350977074, 0.09157621350977074, 0.10995174365532187 },
    { 0.09157621350977074, 0.81684757298045851, 0.09157621350977074, 0.10995174365532187 },
    { 0.09157621350977074, 0.09157621350977074, 0.81684757298045851, 0.10995174365532187 } },
  // order=5, p=7 in the book
  { { 0.33333333333333333, 0.33333333333333333, 0.33333333333333333, 0.22500000000000000 },
    { 0.79742698535308732, 0.10128650732345634, 0.10128650732345634, 0.12593918054482715 },
    { 0.10128650732345634, 0.79742698535308732, 0.10128650732345634, 0.12593918054482715 },
    { 0.10128650732345634, 0.10128650732345634, 0.79742698535308732, 0.12593918054482715 },
    { 0.05971587178976982, 0.47014206410511509, 0.47014206410511509, 0.13239415278850618 },
    { 0.47014206410511509, 0.05971587178976982, 0.47014206410511509, 0.13239415278850618 },
    { 0.47014206410511509, 0.47014206410511509, 0.05971587178976982, 0.13239415278850618 } }
};

const unsigned int Element3DC0LinearTriangular ::Nip[6] = { 0, 1, 3, 3, 6, 7 };

void
Element3DC0LinearTriangular ::GetIntegrationPointAndWeight(unsigned int i,
                                                           VectorType & pt,
                                                           Float &      w,
                                                           unsigned int order) const
{
  // FIXME: range checking

  // default integration order
  if (order == 0 || order > 5)
  {
    order = DefaultIntegrationOrder;
  }

  pt.set_size(3);

  /*
   * We provide implementation for 5 different integration rules
   * as defined in chapter 24 - Implementation of Iso-P Truangular
   * Elements, of http://titan.colorado.edu/courses.d/IFEM.d/.
   *
   * Note that the order parameter here does not correspond to the
   * actual order of integration, but rather the degree of polynomials
   * that are exactly integrated. In addition, there are two integration
   * rules for polynomials of 2nd degree. In order to allow using both of
   * them, we assign the index number 3 to the second one. Note that this
   * does not mean that the rule is capable of integrating the polynomials
   * of 3rd degree. It's just an index of a rule.
   */
  pt.copy_in(trigGaussRuleInfo[order][i]);

  // We scale the weight by 0.5, to take into account
  // the factor that must be applied when integrating.
  w = 0.5 * trigGaussRuleInfo[order][i][3];
}

unsigned int
Element3DC0LinearTriangular ::GetNumberOfIntegrationPoints(unsigned int order) const
{
  // FIXME: range checking

  // default integration order
  if (order == 0)
  {
    order = DefaultIntegrationOrder;
  }

  return Nip[order];
}

Element3DC0LinearTriangular::VectorType
Element3DC0LinearTriangular ::ShapeFunctions(const VectorType & pt) const
{
  // Linear triangular element has 3 shape functions
  VectorType shapeF(3);

  // Shape functions are equal to coordinates
  shapeF = pt;

  return shapeF;
}

void
Element3DC0LinearTriangular ::ShapeFunctionDerivatives(const VectorType &, MatrixType & shapeD) const
{
  // Matrix of shape functions derivatives is an
  // identity matrix for linear triangular element.
  shapeD.set_size(3, 3);
  shapeD.fill(0.0);
  shapeD[0][0] = 1.0;
  shapeD[1][1] = 1.0;
  shapeD[2][2] = 1.0;
}

bool
Element3DC0LinearTriangular ::GetLocalFromGlobalCoordinates(const VectorType & globalPt, VectorType & localPt) const
{
  Float x, x1, x2, x3, y, y1, y2, y3, z, z1, z2, z3, A;

  localPt.set_size(3);

  x = globalPt[0];
  y = globalPt[1];
  z = globalPt[2];
  x1 = this->m_node[0]->GetCoordinates()[0];
  y1 = this->m_node[0]->GetCoordinates()[1];
  x2 = this->m_node[1]->GetCoordinates()[0];
  y2 = this->m_node[1]->GetCoordinates()[1];
  x3 = this->m_node[2]->GetCoordinates()[0];
  y3 = this->m_node[2]->GetCoordinates()[1];
  z1 = this->m_node[0]->GetCoordinates()[2];
  z2 = this->m_node[1]->GetCoordinates()[2];
  z3 = this->m_node[2]->GetCoordinates()[2];

  // FIXME!
  A = x1 * y2 - x2 * y1 + x3 * y1 - x1 * y3 + x2 * y3 - x3 * y2;
  //  localPt[0]=((y2 - y3)*x + (x3 - x2)*y + x2*y3 - x3*y2)/A;
  //  localPt[1]=((y3 - y1)*x + (x1 - x3)*y + x3*y1 - x1*y3)/A;
  //  localPt[2]=((y1 - y2)*x + (x2 - x1)*y + x1*y2 - x2*y1)/A;

  if (localPt[0] < 0.0 || localPt[0] > 1.0 || localPt[1] < 0.0 || localPt[1] > 1.0 || localPt[2] < 0.0 ||
      localPt[2] > 1.0)
  {
    return false;
  }
  else
  {
    return true;
  }
}

Element3DC0LinearTriangular::Float
Element3DC0LinearTriangular ::JacobianDeterminant(const VectorType & pt, const MatrixType * pJ) const
{
  // use heron's formula
  int na = 0;
  int nb = 1;
  int nc = 2;

  VectorType A = this->GetNode(na)->GetCoordinates();
  VectorType B = this->GetNode(nb)->GetCoordinates();
  VectorType C = this->GetNode(nc)->GetCoordinates();
  VectorType BA = B - A;
  VectorType CA = C - A;
  VectorType CB = C - B;
  float      L1 = CB.magnitude();
  float      L2 = CA.magnitude();
  float      L3 = BA.magnitude();

  float s = (L1 + L2 + L3) * .5;
  Float det = sqrt(s * (s - L1) * (s - L2) * (s - L3));

  /*
  // use the formula for tri pqr, area is mag( vec(pq) cross vec(pr) )
    VectorType a=this->GetNode(2)->GetCoordinates()-this->GetNode(0)->GetCoordinates();
    VectorType b=this->GetNode(1)->GetCoordinates()-this->GetNode(0)->GetCoordinates();

    VectorType c;
    c.set_size(3);

    c[0] = a[1] * b[2] - a[2] * b[1];
    c[1] = a[2] * b[0] - a[0] * b[2];
    c[2] = a[0] * b[1] - a[1] * b[0];

    Float det=0.5*c.magnitude();
    */
  //  ::std::cout << " area " << det << std::endl;
  return det;
}

void
Element3DC0LinearTriangular ::JacobianInverse(const VectorType & pt, MatrixType & invJ, const MatrixType * pJ) const
{
  MatrixType * pJlocal = 0;

  // If Jacobian was not provided, we
  // need to compute it here
  if (pJ == 0)
  {
    pJlocal = new MatrixType();
    this->Jacobian(pt, *pJlocal);
    pJ = pJlocal;
  }

  //  invJ=vnl_svd_inverse<Float>(*pJ);
  invJ = vnl_qr<Float>(*pJ).inverse();

  /*
// Note that inverse of Jacobian is not quadratic matrix
MatrixType invJ2;
invJ2.set_size(3,3);
invJ2.fill(0);

Float idet=1.0/this->JacobianDeterminant( pt, pJ );
invJ2[0][0]=idet*((*pJ)[1][1]-(*pJ)[2][1]);
invJ2[0][1]=idet*((*pJ)[2][1]-(*pJ)[0][1]);
invJ2[0][2]=idet*((*pJ)[0][1]-(*pJ)[1][1]);
invJ2[1][0]=idet*((*pJ)[2][0]-(*pJ)[1][0]);
invJ2[1][1]=idet*((*pJ)[0][0]-(*pJ)[2][0]);
invJ2[1][2]=idet*((*pJ)[1][0]-(*pJ)[0][0]);

::std::cout << " pJ " << std::endl;
::std::cout << (*pJ) << std::endl;

::std::cout << " invJ " << std::endl;
::std::cout << (invJ) << std::endl;

::std::cout << " invJ2 " << std::endl;
::std::cout << (invJ2) << std::endl;*/

  delete pJlocal;
}

/*
 * Draw the element on device context pDC.
 */
#ifdef FEM_BUILD_VISUALIZATION
void
Element3DC0LinearTriangular ::Draw(CDC * pDC, Solution::ConstPointer sol) const
{
  int x1 = m_node[0]->GetCoordinates()[0] * DC_Scale;
  int y1 = m_node[0]->GetCoordinates()[1] * DC_Scale;

  int x2 = m_node[1]->GetCoordinates()[0] * DC_Scale;
  int y2 = m_node[1]->GetCoordinates()[1] * DC_Scale;

  int x3 = m_node[2]->GetCoordinates()[0] * DC_Scale;
  int y3 = m_node[2]->GetCoordinates()[1] * DC_Scale;

  x1 += sol->GetSolutionValue(this->m_node[0]->GetDegreeOfFreedom(0)) * DC_Scale;
  y1 += sol->GetSolutionValue(this->m_node[0]->GetDegreeOfFreedom(1)) * DC_Scale;
  x2 += sol->GetSolutionValue(this->m_node[1]->GetDegreeOfFreedom(0)) * DC_Scale;
  y2 += sol->GetSolutionValue(this->m_node[1]->GetDegreeOfFreedom(1)) * DC_Scale;
  x3 += sol->GetSolutionValue(this->m_node[2]->GetDegreeOfFreedom(0)) * DC_Scale;
  y3 += sol->GetSolutionValue(this->m_node[2]->GetDegreeOfFreedom(1)) * DC_Scale;

  pDC->MoveTo(x1, y1);
  pDC->LineTo(x2, y2);
  pDC->LineTo(x3, y3);
  pDC->LineTo(x1, y1);
}

#endif
} // namespace fem
} // namespace itk