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

  Author: Christine Xu

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

#include <itkTriangleCell.h>

#include "SphericalHarmonicMeshSource.h"

#include <math.h>
#include <stdio.h>
#include <iostream>

namespace neurolib
{

#define X .525731112119133606
#define Z .850650808352039932

#ifndef M_PI
#define M_PI 3.1415926535897932
#endif
#ifndef M_PI_2
#define M_PI_2 1.5707963267948966
#endif

// Vertices, triangles, edges of a single icosahedron
static double vert[12][3] =
  {
        {-X, 0.0, Z}, {X, 0.0, Z}, {-X, 0.0, -Z}, {X, 0.0, -Z},
        {0.0, Z, X}, {0.0, Z, -X}, {0.0, -Z, X}, {0.0, -Z, -X},
        {Z, X, 0.0}, {-Z, X, 0.0}, {Z, -X, 0.0}, {-Z, -X, 0.0}
  };
static int triang[20][3] =
  {
        {0, 4, 1}, {0, 9, 4}, {9, 5, 4}, {4, 5, 8}, {4, 8, 1},
        {8, 10, 1}, {8, 3, 10}, {5, 3, 8}, {5, 2, 3}, {2, 7, 3},
        {7, 10, 3}, {7, 6, 10}, {7, 11, 6}, {11, 0, 6}, {0, 1, 6},
        {6, 1, 10}, {9, 0, 11}, {9, 11, 2}, {9, 2, 5}, {7, 2, 11}
  };
static int edge[30][2] =
  {
        {0, 1}, {0, 4}, {0, 6}, {0, 9}, {0, 11}, {1, 4}, {1, 6}, {1, 8}, {1, 10}, {2, 3},
        {2, 5}, {2, 7}, {2, 9}, {2, 11}, {3, 5}, {3, 7}, {3, 8}, {3, 10}, {4, 5}, {4, 8},
        {4, 9}, {5, 8}, {5, 9}, {6, 7}, {6, 10}, {6, 11}, {7, 10}, {7, 11}, {8, 10}, {9, 11}
  };

typedef itk::TriangleCell<SphericalHarmonicMeshSource::CellType> TriangleType;

SphericalHarmonicMeshSource::SphericalHarmonicMeshSource()
{
  m_Dimension = 3;

  m_Level = 20;
  m_Degree = 0;
  m_FromL = 0;
  m_ToL = 0;

  outputParaMesh = NULL;
  m_icos = NULL;
}

SphericalHarmonicMeshSource::~SphericalHarmonicMeshSource()
{
  delete[] m_icos;
}

void SphericalHarmonicMeshSource::SetDegree(unsigned int d)
{
  if( m_Coefs.empty() || d > floor(sqrt( (double) m_Coefs.size() ) ) - 1 )
    {
    throw SphericalHarmonicPolynomialException(
            __FILE__, __LINE__, "The maximum degree of the coefficient exceeds the size of coefficient list.");
    }
  m_Degree = d;
  m_ToL = d;
}

void SphericalHarmonicMeshSource::SetCoefs(SphericalHarmonicMeshSource::CoefListType& coeflist)
{
  m_Coefs = coeflist;
  m_Degree =  (int) floor(sqrt( (double) coeflist.size() ) ) - 1;
  m_ToL  = m_Degree;
  // std::cout<<"m_Degree = "<<m_Degree <<std::endl;
}

void SphericalHarmonicMeshSource::GenerateData()
{
  if( m_Coefs.empty() )
    {
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Coefficients mustn't be empty.");
    }
  if( m_FromL > m_ToL )
    {
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__,
                                               "The starting degree should be smaller or equal to the ending degree.");
    }
  if( m_ToL > m_Degree )
    {
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__,
                                               "The evalueated degree mustn't exceed the size of the coefficients.");
    }
  if( m_Level <= 0 )
    {
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, "Subdivision level must be greater than zero.");
    }

  int n_phi, n_theta;
  int n_vert;
  int n_triag;

  // Calculate n_vertex, n_phi, n_theta, n_triag
  unsigned int n = 0;
  if( m_Level > 2 )
    {
    for( unsigned int i = 1; i < (m_Level - 1); i++ )
      {
      n += i;
      }
    }
  n_vert = 12 + (m_Level - 1) * 30 + n * 20;
  n_theta = (int) ceil(sqrt( ( (double) (n_vert) ) / 2.0) );
  n_phi = 2 * n_theta;
  int numtriags = 0;
  if( m_Level == 1 )
    {
    numtriags = 20;
    }
  else if( m_Level > 1 )
    {
    n = 1;

    do
      {
      for( unsigned int m = 1; m <= n; m++ )
        {
        numtriags = numtriags + 3;
        if( m != n )
          {
          numtriags = numtriags + 3;
          }

        }
      n++;
      }
    while( n <= m_Level );

    numtriags = numtriags * 20;
    numtriags = numtriags / 3;
    }
  n_triag = numtriags;

  // Allocate datas
  Point3* all_vert = new Point3[n_vert];
  Point3* all_triangs = new Point3[n_triag * 3]; // all possible vertices in triangs
  m_icos = new double[n_vert * 2];
  int *   triangs = new int[3 * n_triag];
  Point3* mesh = new Point3[n_phi * n_theta];
  Point3* vertex = new Point3[n_vert];

  // Set up icosahedral triangles on the sphere, all_vert & triangs
  set_up_icosahedron_triangs(all_vert, all_triangs, m_Level, n_vert, n_phi, n_theta, m_icos, n_triag, triangs);

  SphericalHarmonicPolynomial<3> SPHARM;
  try
    {
    SPHARM.SetCoefs(m_Coefs);
    SPHARM.SetDegree(m_Degree);
    // Calculate mesh
    // Ipek - changed - dont do interpolation, go straight to actual theta and phi's
    for( int i = 0; i < n_vert; i++ )
      {
      SPHARM.Evaluate(m_FromL, m_ToL, m_icos[2 * i], m_icos[2 * i + 1], vertex[i]);
      }
    }
  catch( SphericalHarmonicPolynomialException& ex )
    {
    throw SphericalHarmonicPolynomialException(__FILE__, __LINE__, ex.GetDescription() );

    }

  // for(int i=0; i<n_phi*n_theta; i++)
  //    std::cout<<mesh[i][0]<<","<<mesh[i][1]<<","<<mesh[i][2]<<std::endl;

  // Ipek - changed
  // interpolation of the real vertice phi and theta
  // interpol_vert(n_phi, n_theta, mesh, n_vert, icos, vertex);

  // for(int i=0; i<n_vert; i++)
  //  std::cout<<vertex[i][0]<<","<<vertex[i][1]<<","<<vertex[i][2]<<std::endl;

  // Construct the mesh in itk format
  MeshType::Pointer outputMesh = this->GetOutput();
  outputParaMesh = MeshType::New();

  PointsContainerPointer points = PointsContainer::New();
  PointsContainerPointer paraPoints = PointsContainer::New();
  for( int i = 0; i < n_vert; i++ )
    {
    points->InsertElement(i, PointType(vertex[i]) );
    paraPoints->InsertElement(i, PointType(all_vert[i]) );
    }
  outputMesh->SetPoints(points);
  outputParaMesh->SetPoints(paraPoints);

  /**
   * Specify the method used for allocating cells
   */
  outputMesh->SetCellsAllocationMethod( MeshType::CellsAllocatedDynamicallyCellByCell );
  outputParaMesh->SetCellsAllocationMethod( MeshType::CellsAllocatedDynamicallyCellByCell );
  for( int i = 0; i < n_triag; i++ )
    {
      {
      CellType::CellAutoPointer cellpointer;
      cellpointer.TakeOwnership(new TriangleType);
      /**
      * Assign the points to the tetrahedron through their identifiers.
      */
      uint64_t triPoints[3];
      triPoints[0] = triangs[3 * i];
      triPoints[1] = triangs[3 * i + 1];
      triPoints[2] = triangs[3 * i + 2];
      CellType::PointIdentifier itkPts[3];
      for (int ii = 0; ii < 3; ++ii)
        {
        itkPts[ii] = static_cast<CellType::PointIdentifier>(triPoints[ii]);
        }
      cellpointer->SetPointIds( itkPts );

      outputMesh->SetCell(i, cellpointer);
      }
      {
      CellType::CellAutoPointer cellpointer;
      cellpointer.TakeOwnership(new TriangleType);
      /**
      * Assign the points to the tetrahedron through their identifiers.
      */
      uint64_t triPoints[3];
      triPoints[0] = triangs[3 * i];
      triPoints[1] = triangs[3 * i + 1];
      triPoints[2] = triangs[3 * i + 2];
      CellType::PointIdentifier itkPts[3];
      for (int ii = 0; ii < 3; ++ii)
        {
        itkPts[ii] = static_cast<CellType::PointIdentifier>(triPoints[ii]);
        }
      cellpointer->SetPointIds( itkPts );

      outputParaMesh->SetCell(i, cellpointer);
      }
    }

  delete[] all_vert;
  delete[] all_triangs;
  // delete[] icos;
  delete[] triangs;
  delete[] mesh;
  delete[] vertex;

}

void SphericalHarmonicMeshSource::set_up_icosahedron_triangs(Point3* all_vert,
                                                             Point3* all_triangs,
                                                             int subdiv,
                                                             int n_vert,
                                                             int /* n_phi */,
                                                             int /* n_theta */,
                                                             double *icos,
                                                             int /* n_triangs */,
                                                             int *triangs)
{
  int    i, n, m, k, numtriags;
  double x1, x2, y1, y2, z1, z2, x3, y3, z3;
  double dx12, dy12, dz12, dx23, dy23, dz23;
  double length;

  double epsilon = 0.00001; // machine epsilon??

  memcpy(all_vert, vert, 12 * sizeof(Point3) );

  // std::cout<<"after memcpy"<<std::endl;

  k = 12;
  for( i = 0; i < 30; i++ )
    {
    x1 = vert[edge[i][0]][0];
    y1 = vert[edge[i][0]][1];
    z1 = vert[edge[i][0]][2];
    x2 = vert[edge[i][1]][0];
    y2 = vert[edge[i][1]][1];
    z2 = vert[edge[i][1]][2];
    dx12 = (x2 - x1) / subdiv;
    dy12 = (y2 - y1) / subdiv;
    dz12 = (z2 - z1) / subdiv;
    for( n = 1; n < subdiv; n++ )
      {
      all_vert[k][0] = x1 + n * dx12;
      all_vert[k][1] = y1 + n * dy12;
      all_vert[k][2] = z1 + n * dz12;
      length = sqrt( (double) all_vert[k][0] * all_vert[k][0]
                     + all_vert[k][1] * all_vert[k][1]
                     + all_vert[k][2] * all_vert[k][2]);
      all_vert[k][0] /= length;
      all_vert[k][1] /= length;
      all_vert[k][2] /= length;
      k++;
      }
    }

  if( subdiv > 2 )
    {
    for( i = 0; i < 20; i++ )
      {
      x1 = vert[triang[i][0]][0];
      y1 = vert[triang[i][0]][1];
      z1 = vert[triang[i][0]][2];
      x2 = vert[triang[i][1]][0];
      y2 = vert[triang[i][1]][1];
      z2 = vert[triang[i][1]][2];
      x3 = vert[triang[i][2]][0];
      y3 = vert[triang[i][2]][1];
      z3 = vert[triang[i][2]][2];
      dx12 = (x2 - x1) / subdiv;
      dy12 = (y2 - y1) / subdiv;
      dz12 = (z2 - z1) / subdiv;
      dx23 = (x3 - x2) / subdiv;
      dy23 = (y3 - y2) / subdiv;
      dz23 = (z3 - z2) / subdiv;

      n = 1;

      do
        {
        for( m = 1; m <= n; m++ )
          {
          all_vert[k][0] = x1 + (n + 1) * dx12 + m * dx23;
          all_vert[k][1] = y1 + (n + 1) * dy12 + m * dy23;
          all_vert[k][2] = z1 + (n + 1) * dz12 + m * dz23;
          length = sqrt( (double) all_vert[k][0] * all_vert[k][0]
                         + all_vert[k][1] * all_vert[k][1]
                         + all_vert[k][2] * all_vert[k][2]);
          all_vert[k][0] /= length;
          all_vert[k][1] /= length;
          all_vert[k][2] /= length;
          k++;
          }
        n++;
        }
      while( n <= (subdiv - 2) );
      }
    }
  numtriags = 0;

  // std::cout<<"before get triangulation"<<std::endl;
  // std::cout<<n_triangs<<std::endl;

  // get triangulation
  if( subdiv > 1 )
    {
    for( i = 0; i < 20; i++ )
      {
      x1 = vert[triang[i][0]][0];
      y1 = vert[triang[i][0]][1];
      z1 = vert[triang[i][0]][2];
      x2 = vert[triang[i][1]][0];
      y2 = vert[triang[i][1]][1];
      z2 = vert[triang[i][1]][2];
      x3 = vert[triang[i][2]][0];
      y3 = vert[triang[i][2]][1];
      z3 = vert[triang[i][2]][2];
      dx12 = (x2 - x1) / subdiv;
      dy12 = (y2 - y1) / subdiv;
      dz12 = (z2 - z1) / subdiv;
      dx23 = (x3 - x2) / subdiv;
      dy23 = (y3 - y2) / subdiv;
      dz23 = (z3 - z2) / subdiv;

      n = 1;

      do
        {
        for( m = 1; m <= n; m++ )
          {
          // Draw lower triangle
          all_triangs[numtriags][0] = x1 + n * dx12 + m * dx23;
          all_triangs[numtriags][1] = y1 + n * dy12 + m * dy23;
          all_triangs[numtriags][2] = z1 + n * dz12 + m * dz23;
          length = sqrt( (double) all_triangs[numtriags][0] * all_triangs[numtriags][0]
                         + all_triangs[numtriags][1] * all_triangs[numtriags][1]
                         + all_triangs[numtriags][2] * all_triangs[numtriags][2]);
          all_triangs[numtriags][0] /= length;
          all_triangs[numtriags][1] /= length;
          all_triangs[numtriags][2] /= length;
          numtriags++;
          all_triangs[numtriags][0] = x1 + (n - 1) * dx12 + (m - 1) * dx23;
          all_triangs[numtriags][1] = y1 + (n - 1) * dy12 + (m - 1) * dy23;
          all_triangs[numtriags][2] = z1 + (n - 1) * dz12 + (m - 1) * dz23;
          length = sqrt( (double) all_triangs[numtriags][0] * all_triangs[numtriags][0]
                         + all_triangs[numtriags][1] * all_triangs[numtriags][1]
                         + all_triangs[numtriags][2] * all_triangs[numtriags][2]);
          all_triangs[numtriags][0] /= length;
          all_triangs[numtriags][1] /= length;
          all_triangs[numtriags][2] /= length;
          numtriags++;
          all_triangs[numtriags][0] = x1 + n * dx12 + (m - 1) * dx23;
          all_triangs[numtriags][1] = y1 + n * dy12 + (m - 1) * dy23;
          all_triangs[numtriags][2] = z1 + n * dz12 + (m - 1) * dz23;
          length = sqrt( (double) all_triangs[numtriags][0] * all_triangs[numtriags][0]
                         + all_triangs[numtriags][1] * all_triangs[numtriags][1]
                         + all_triangs[numtriags][2] * all_triangs[numtriags][2]);
          all_triangs[numtriags][0] /= length;
          all_triangs[numtriags][1] /= length;
          all_triangs[numtriags][2] /= length;
          numtriags++;
          if( m != n )
            {
            // Draw lower left triangle
            all_triangs[numtriags][0] = x1 + n * dx12 + m * dx23;
            all_triangs[numtriags][1] = y1 + n * dy12 + m * dy23;
            all_triangs[numtriags][2] = z1 + n * dz12 + m * dz23;
            length = sqrt( (double) all_triangs[numtriags][0] * all_triangs[numtriags][0]
                           + all_triangs[numtriags][1] * all_triangs[numtriags][1]
                           + all_triangs[numtriags][2] * all_triangs[numtriags][2]);
            all_triangs[numtriags][0] /= length;
            all_triangs[numtriags][1] /= length;
            all_triangs[numtriags][2] /= length;
            numtriags++;
            all_triangs[numtriags][0] = x1 + (n - 1) * dx12 + m * dx23;
            all_triangs[numtriags][1] = y1 + (n - 1) * dy12 + m * dy23;
            all_triangs[numtriags][2] = z1 + (n - 1) * dz12 + m * dz23;
            length = sqrt( (double) all_triangs[numtriags][0] * all_triangs[numtriags][0]
                           + all_triangs[numtriags][1] * all_triangs[numtriags][1]
                           + all_triangs[numtriags][2] * all_triangs[numtriags][2]);
            all_triangs[numtriags][0] /= length;
            all_triangs[numtriags][1] /= length;
            all_triangs[numtriags][2] /= length;
            numtriags++;
            all_triangs[numtriags][0] = x1 + (n - 1) * dx12 + (m - 1) * dx23;
            all_triangs[numtriags][1] = y1 + (n - 1) * dy12 + (m - 1) * dy23;
            all_triangs[numtriags][2] = z1 + (n - 1) * dz12 + (m - 1) * dz23;
            length = sqrt( (double) all_triangs[numtriags][0] * all_triangs[numtriags][0]
                           + all_triangs[numtriags][1] * all_triangs[numtriags][1]
                           + all_triangs[numtriags][2] * all_triangs[numtriags][2]);
            all_triangs[numtriags][0] /= length;
            all_triangs[numtriags][1] /= length;
            all_triangs[numtriags][2] /= length;
            numtriags++;
            }
          }
        n++;
        }
      while( n <= subdiv );
      }
    }

  // std::cout<<"before indexing of triangs"<<std::endl;

  // indexing of triangs
  if( subdiv == 1 )
    {
    memcpy(triangs, triang, 20 * 3 * sizeof(int) );
    numtriags = 20;
    }
  else
    {
    // find for every point in triangle list the corresponding index in all_vert
    // initialize
    for( i = 0; i < numtriags; i++ )
      {
      triangs[i] = -1;
      }
    // find indexes
    for( i = 0; i < n_vert; i++ )
      {
      for( int j = 0; j < numtriags; j++ )
        {
        if( triangs[j] < 0 )
          {
          if( (fabs(all_vert[i][0] - all_triangs[j][0]) < epsilon) &&
              (fabs(all_vert[i][1] - all_triangs[j][1]) < epsilon) &&
              (fabs(all_vert[i][2] - all_triangs[j][2]) < epsilon ) )
            {
            triangs[j] = i;
            }
          }
        }
      }
    // for(i=0; i<n_vert; i++)
    //  std::cout<<triangs[3*i]<<","<<triangs[3*i+1]<<","<<triangs[3*i+2]<<std::endl;
    for( i = 0; i < numtriags; i++ )
      {
      if( triangs[i] == -1 )
        {
        std::cerr << " - " << i << " :" << all_triangs[i][0]
                  << "," << all_triangs[i][1] << "," << all_triangs[i][2] << std::endl;
        }
      }

    // numtriags is the number of vertices in triangles -> divide it by 3
    numtriags = numtriags / 3;
    }
  // std::cout<<"before get phi   heta"<<std::endl;
  // get phi   eta
  for( i = 0; i < n_vert; i++ )
    {
    icos[2 * i] = atan2(all_vert[i][1], all_vert[i][0]) + M_PI;
    icos[2 * i + 1] = atan(all_vert[i][2]
                           / sqrt( (double) all_vert[i][0] * all_vert[i][0]
                                   + all_vert[i][1] * all_vert[i][1]) ) + M_PI_2;
    // std::cout<<icos[2*i]<<","<<icos[2*i+1]<<std::endl;
    }
}

void SphericalHarmonicMeshSource::interpol_vert(int n_phi,
                                                int n_theta,
                                                Point3 *mesh,
                                                int n_vertex,
                                                double *icos,
                                                Point3 *vertex)
{
  int    i;
  double phi, theta;
  double x, y;
  int    xu, xd, yu, yd;
  double xi, yi, zi;
  double ksi, eta;

  for( i = 0; i < n_vertex; i++ )
    {
    phi = icos[2 * i]; // phi and theta for every vertex
    theta = icos[2 * i + 1];

    x = (phi - 1e-5) * ( (float)n_phi) / (2 * M_PI);
    xd = (int) x;
    xu = xd + 1;
    ksi = x - xd;
    if( xu >= n_phi )
      {
      xd = mod(xd, n_phi);
      xu = mod(xu, n_phi);
      }

    y = theta * n_theta / M_PI - 0.5;
    yd = (int) y;
    yu = yd + 1;
    eta = y - yd;
    if( yu == n_theta )
      {
      yu = yu - 1;
      xd = mod(xd + n_theta, n_phi);
      xu = mod(xu + n_theta, n_phi);
      }
    if( yd == -1 )
      {
      yd = 0;
      xd = mod(xd + n_theta, n_phi);
      xu = mod(xu + n_theta, n_phi);
      }

    interpol_2d(n_phi, n_theta, xd, xu, yd, yu, ksi, eta, mesh, &xi, &yi, &zi);  // vertex
    vertex[i][0] = xi;
    vertex[i][1] = yi;
    vertex[i][2] = zi;

    }
}

void SphericalHarmonicMeshSource::interpol_2d(int n_phi,
                                              int /* n_theta */,
                                              int xd,
                                              int xu,
                                              int yd,
                                              int yu,
                                              double ksi,
                                              double eta,
                                              Point3 *mesh,
                                              double *xi,
                                              double *yi,
                                              double *zi)
{
  double f00, f10, f01, f11;

  f00 = mesh[xd + n_phi * yd][0];
  f10 = mesh[xu + n_phi * yd][0];
  f01 = mesh[xd + n_phi * yu][0];
  f11 = mesh[xu + n_phi * yu][0];

  *xi = f00 + (f10 - f00) * ksi + (f01 - f00) * eta
    + (f11 + f00 - f10 - f01) * ksi * eta;

  f00 = mesh[xd + n_phi * yd][1];
  f10 = mesh[xu + n_phi * yd][1];
  f01 = mesh[xd + n_phi * yu][1];
  f11 = mesh[xu + n_phi * yu][1];

  *yi = f00 + (f10 - f00) * ksi + (f01 - f00) * eta
    + (f11 + f00 - f10 - f01) * ksi * eta;

  f00 = mesh[xd + n_phi * yd][2];
  f10 = mesh[xu + n_phi * yd][2];
  f01 = mesh[xd + n_phi * yu][2];
  f11 = mesh[xu + n_phi * yu][2];

  *zi = f00 + (f10 - f00) * ksi + (f01 - f00) * eta
    + (f11 + f00 - f10 - f01) * ksi * eta;
}

int SphericalHarmonicMeshSource::mod(int x, int y)
{
  if( (x % y) >= 0 )
    {
    return x % y;
    }
  else
    {
    return y + (x % y);
    }
}

} // end namespace neurolib
