#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <sys/time.h>
#ifdef _WIN32
#else
#include <sys/resource.h>
#endif
#include <unistd.h>
#include <signal.h>
#include <gsl/gsl_rng.h>
#ifndef NOMPI
#include <mpi.h>
#endif

#include "allvars.h"
#include "proto.h"

/*! \file system.c
 *  \brief contains miscellaneous routines, e.g. elapsed time measurements
 */

/*! This routine returns a random number taken from a table of random numbers,
 *  which is refilled every timestep.  This method is used to allow random
 *  number application to particles independent of the number of processors
 *  used, and independent of the particular order the particles have. In order
 *  to work properly, the particle IDs should be set properly to unique
 *  integer values.
 */
double get_random_number(int id)
{
  return RndTable[(id % RNDTABLE)];
}

/*! This routine fills the random number table.
 */
void set_random_numbers(void)
{
  int i;

  for(i = 0; i < RNDTABLE; i++)
    RndTable[i] = gsl_rng_uniform(random_generator);
}

/*! returns the number of cpu-ticks in seconds that have elapsed, or the
 *  wall-clock time obtained with MPI_Wtime().
 */
double second(void)
{
#ifdef WALLCLOCK
#ifndef NOMPI
  return MPI_Wtime();
#else
  return ((double) clock()) / CLOCKS_PER_SEC;
#endif
#else
  return ((double) clock()) / CLOCKS_PER_SEC;
#endif

  /* note: on AIX and presumably many other 32bit systems,
   * clock() has only a resolution of 10ms=0.01sec
   */
}

/*! returns the time difference between two measurements obtained with
 *  second(). The routine takes care of the possible overflow of the tick
 *  counter on 32bit systems, but depending on the system, this may not always
 *  work properly. Similarly, in some MPI implementations, the MPI_Wtime()
 *  function may also overflow, in which case a negative time difference would
 *  be returned. The routine returns instead a time difference equal to 0.
 */
double timediff(double t0, double t1)
{
  double dt;

  dt = t1 - t0;

  if(dt < 0)	/* overflow has occured (for systems with 32bit tick counter) */
    {
#ifdef WALLCLOCK
      dt = 0;
#else
      dt = t1 + pow(2, 32) / CLOCKS_PER_SEC - t0;
#endif
    }

  return dt;
}

/*! returns the maximum of two double
 */
double dmax(double x, double y)
{
  if(x > y)
    return x;
  else
    return y;
}

/*! returns the minimum of two double
 */
double dmin(double x, double y)
{
  if(x < y)
    return x;
  else
    return y;
}

/*! returns the maximum of two integers
 */
int imax(int x, int y)
{
  if(x > y)
    return x;
  else
    return y;
}

/*! returns the minimum of two integers
 */
int imin(int x, int y)
{
  if(x < y)
    return x;
  else
    return y;
}


#include <mpi.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_math.h>

#include "allvars.h"
#include "proto.h"

/*! \file snIa_heating.c
 *  \brief routines for heating by type Ia supernovae
 */

#if defined(SNIA_HEATING)

static struct snheatingdata_in
{
  MyDouble Pos[3];
  MyFloat Density;
  MyFloat Energy;
  MyFloat Hsml;
  int NodeList[NODELISTLENGTH];
}
 *SnheatingdataIn, *SnheatingdataGet;

static double hubble_a, ascale, a3inv;

void snIa_heating(void)
{
  int i, j, k;
  int ndone_flag, ndone;
  int ngrp, sendTask, recvTask, place, nexport, nimport, dummy;
  double dt;
  MPI_Status status;

  if(ThisTask == 0)
    {
      printf("Beginning snIa heating\n");
      fflush(stdout);
    }

  CPU_Step[CPU_MISC] += measure_time();

  if(All.ComovingIntegrationOn)
    {
      ascale = All.Time;
      a3inv = 1.0 / (ascale * ascale * ascale);
      hubble_a = hubble_function(All.Time);
    }
  else
    hubble_a = ascale = a3inv = 1;

  /* allocate buffers to arrange communication */

  Ngblist = (int *) mymalloc(NumPart * sizeof(int));

  All.BunchSize =
    (int) ((All.BufferSize * 1024 * 1024) / (sizeof(struct data_index) + sizeof(struct data_nodelist) +
					     2 * sizeof(struct snheatingdata_in)));
  DataIndexTable = (struct data_index *) mymalloc(All.BunchSize * sizeof(struct data_index));
  DataNodeList = (struct data_nodelist *) mymalloc(All.BunchSize * sizeof(struct data_nodelist));

  /** Let's first spread the feedback energy */

  i = FirstActiveParticle;	/* first particle for this task */

  do
    {
      for(j = 0; j < NTask; j++)
	{
	  Send_count[j] = 0;
	  Exportflag[j] = -1;
	}

      /* do local particles and prepare export list */

      for(nexport = 0; i >= 0; i = NextActiveParticle[i])
	if(P[i].Type == 4)
	  if(snIaheating_evaluate(i, 0, &nexport, Send_count) < 0)
	    break;

#ifdef MYSORT
      mysort_dataindex(DataIndexTable, nexport, sizeof(struct data_index), data_index_compare);
#else
      qsort(DataIndexTable, nexport, sizeof(struct data_index), data_index_compare);
#endif

      MPI_Allgather(Send_count, NTask, MPI_INT, Sendcount_matrix, NTask, MPI_INT, MPI_COMM_WORLD);

      for(j = 0, nimport = 0, Recv_offset[0] = 0, Send_offset[0] = 0; j < NTask; j++)
	{
	  Recv_count[j] = Sendcount_matrix[j * NTask + ThisTask];
	  nimport += Recv_count[j];

	  if(j > 0)
	    {
	      Send_offset[j] = Send_offset[j - 1] + Send_count[j - 1];
	      Recv_offset[j] = Recv_offset[j - 1] + Recv_count[j - 1];
	    }
	}

      SnheatingdataGet = (struct snheatingdata_in *) mymalloc(nimport * sizeof(struct snheatingdata_in));
      SnheatingdataIn = (struct snheatingdata_in *) mymalloc(nexport * sizeof(struct snheatingdata_in));

      for(j = 0; j < nexport; j++)
	{
	  place = DataIndexTable[j].Index;

	  for(k = 0; k < 3; k++)
	    SnheatingdataIn[j].Pos[k] = P[place].Pos[k];

	  SnheatingdataIn[j].Hsml = PPP[place].Hsml;
	  SnheatingdataIn[j].Density = P[place].DensAroundStar;

	  dt = (P[place].TimeBin ? (1 << P[place].TimeBin) : 0) * All.Timebase_interval / hubble_a;

	  SnheatingdataIn[j].Energy = All.SnIaHeatingRate * dt * P[place].Mass;

	  memcpy(SnheatingdataIn[j].NodeList,
		 DataNodeList[DataIndexTable[j].IndexGet].NodeList, NODELISTLENGTH * sizeof(int));
	}

      /* exchange particle data */
      for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
	{
	  sendTask = ThisTask;
	  recvTask = ThisTask ^ ngrp;

	  if(recvTask < NTask)
	    {
	      if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
		{
		  /* get the particles */
		  MPI_Sendrecv(&SnheatingdataIn[Send_offset[recvTask]],
			       Send_count[recvTask] * sizeof(struct snheatingdata_in), MPI_BYTE,
			       recvTask, TAG_DENS_A,
			       &SnheatingdataGet[Recv_offset[recvTask]],
			       Recv_count[recvTask] * sizeof(struct snheatingdata_in), MPI_BYTE,
			       recvTask, TAG_DENS_A, MPI_COMM_WORLD, &status);
		}
	    }
	}

      myfree(SnheatingdataIn);

      /* now do the particles that were sent to us */

      for(j = 0; j < nimport; j++)
	snIaheating_evaluate(j, 1, &dummy, &dummy);

      if(i < 0)
	ndone_flag = 1;
      else
	ndone_flag = 0;

      MPI_Allreduce(&ndone_flag, &ndone, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

      myfree(SnheatingdataGet);
    }
  while(ndone < NTask);

  myfree(DataNodeList);
  myfree(DataIndexTable);
  myfree(Ngblist);

  CPU_Step[CPU_COOLINGSFR] += measure_time();
}

int snIaheating_evaluate(int target, int mode, int *nexport, int *nsend_local)
{
  int startnode, numngb, j, n, listindex = 0;
  MyFloat *pos, h_i, dt, rho;
  double dx, dy, dz, h_i2, r2, r, u, hinv, hinv3, wk, energy, egy;

  if(mode == 0)
    {
      pos = P[target].Pos;
      rho = P[target].DensAroundStar;

      dt = (P[target].TimeBin ? (1 << P[target].TimeBin) : 0) * All.Timebase_interval / hubble_a;

      energy = All.SnIaHeatingRate * dt * P[target].Mass;

      h_i = PPP[target].Hsml;
    }
  else
    {
      pos = SnheatingdataGet[target].Pos;
      rho = SnheatingdataGet[target].Density;
      energy = SnheatingdataGet[target].Energy;
      h_i = SnheatingdataGet[target].Hsml;
    }

  /* initialize variables before SPH loop is started */
  h_i2 = h_i * h_i;

  /* Now start the actual SPH computation for this particle */
  if(mode == 0)
    {
      startnode = All.MaxPart;	/* root node */
    }
  else
    {
      startnode = SnheatingdataGet[target].NodeList[0];
      startnode = Nodes[startnode].u.d.nextnode;	/* open it */
    }

  while(startnode >= 0)
    {
      while(startnode >= 0)
	{
	  numngb = ngb_treefind_variable(pos, h_i, target, &startnode, mode, nexport, nsend_local);

	  if(numngb < 0)
	    return -1;

	  for(n = 0; n < numngb; n++)
	    {
	      j = Ngblist[n];

	      if(P[j].Type == 0 && P[j].Mass > 0)
		{
		  dx = pos[0] - P[j].Pos[0];
		  dy = pos[1] - P[j].Pos[1];
		  dz = pos[2] - P[j].Pos[2];
#ifdef PERIODIC			/*  now find the closest image in the given box size  */
		  if(dx > boxHalf_X)
		    dx -= boxSize_X;
		  if(dx < -boxHalf_X)
		    dx += boxSize_X;
		  if(dy > boxHalf_Y)
		    dy -= boxSize_Y;
		  if(dy < -boxHalf_Y)
		    dy += boxSize_Y;
		  if(dz > boxHalf_Z)
		    dz -= boxSize_Z;
		  if(dz < -boxHalf_Z)
		    dz += boxSize_Z;
#endif
		  r2 = dx * dx + dy * dy + dz * dz;

		  if(r2 < h_i2)
		    {
		      r = sqrt(r2);
		      hinv = 1 / h_i;
#ifndef  TWODIMS
		      hinv3 = hinv * hinv * hinv;
#else
		      hinv3 = hinv * hinv / boxSize_Z;
#endif

		      u = r * hinv;

		      if(u < 0.5)
			wk = hinv3 * (KERNEL_COEFF_1 + KERNEL_COEFF_2 * (u - 1) * u * u);
		      else
			wk = hinv3 * KERNEL_COEFF_5 * (1.0 - u) * (1.0 - u) * (1.0 - u);

		      egy = P[j].Mass * wk / rho * energy;

		      SphP[j].Entropy += egy / P[j].Mass * GAMMA_MINUS1 /
			pow(SphP[j].d.Density * a3inv, GAMMA_MINUS1);

		    }
		}
	    }
	}

      if(mode == 1)
	{
	  listindex++;
	  if(listindex < NODELISTLENGTH)
	    {
	      startnode = SnheatingdataGet[target].NodeList[listindex];
	      if(startnode >= 0)
		startnode = Nodes[startnode].u.d.nextnode;	/* open it */
	    }
	}
    }

  return 0;
}

#endif


#pragma once

#include <utility>
#include <array>
#include <vector>
#include <unordered_map>
#include <DirectXMathExtend.h>
#include <gsl.h>
#include <VertexTraits.h>
#include <minmax>
#include "bvh.h"
#include <hlslm\hlsl.hpp>
#include <iostream>

namespace Geometrics
{
	template <typename IndexType>
	struct Triangle
	{
		static constexpr size_t VertexCount = 3;
		typedef std::array<IndexType, VertexCount> container_type;
		container_type v;

		typedef IndexType value_type;
		typedef size_t size_type;
		typedef ptrdiff_t difference_type;
		typedef IndexType *pointer;
		typedef const IndexType *const_pointer;
		typedef IndexType& reference;
		typedef const IndexType& const_reference;

		typedef typename container_type::iterator iterator;
		typedef typename container_type::const_iterator const_iterator;

		typedef typename container_type::reverse_iterator reverse_iterator;
		typedef typename container_type::const_reverse_iterator const_reverse_iterator;

		Triangle() {}
		Triangle(const IndexType &v0, const IndexType &v1, const IndexType &v2)
		{
			v[0] = v0; v[1] = v1; v[2] = v2;
		}
		Triangle(const IndexType *v_)
		{
			v[0] = v_[0]; v[1] = v_[1]; v[2] = v_[2];
		}
		template <class S> explicit Triangle(const S &x)
		{
			v[0] = x[0];  v[1] = x[1];  v[2] = x[2];
		}
		IndexType &operator[] (int i) { return v[i]; }
		const IndexType &operator[] (int i) const { return v[i]; }
		operator const IndexType * () const { return &(v[0]); }
		operator IndexType * () { return &(v[0]); }

		void rewind() {
			std::swap(v[0], v[2]);
		}

		int indexof(IndexType v_) const
		{
			return (v[0] == v_) ? 0 :
				(v[1] == v_) ? 1 :
				(v[2] == v_) ? 2 : -1;
		}

		// contrainer access
		auto begin() { return v.begin(); }
		auto end() { return v.end(); }
		auto begin() const { return v.begin(); }
		auto end() const { return v.end(); }

		auto rbegin() { return v.rbegin(); }
		auto rend() { return v.rend(); }
		auto rbegin() const { return v.rbegin(); }
		auto rend() const { return v.rend(); }

		size_t size() const { return VertexCount; }
	};

	struct submesh_data
	{
		uint32_t vertex_offset; // start vertices index of this geometry
		uint32_t vertex_count;
		uint32_t index_offset; // start index position of this geometry
		uint32_t index_count;
	};

	// generate per vertex normal for given triangle mesh
	template <typename VertexType, typename IndexType>
	bool generate_normal(gsl::span<VertexType> vertices, gsl::span<const Triangle<IndexType>> facets)
	{
		using namespace DirectX::VertexTraits;
		using namespace DirectX;

		if (!has_normal<VertexType>::value)
			return false;

		//static_assert(has_normal<VertexType>::value, "The vertex type dose not contains normal field");

		std::vector<XMVECTOR, XMAllocator> normals(vertices.size());

		// set zero
		std::memset(normals.data(), 0, normals.size() * sizeof(XMVECTOR));

		XMVECTOR v0, v1, v2, n;
		for (const auto& face : facets)
		{
			v0 = get_position(vertices[face[0]]);
			v1 = get_position(vertices[face[1]]);
			v2 = get_position(vertices[face[2]]);
			v1 -= v0;
			v2 -= v0;

			v1 = XMVector3Normalize(v1);
			v2 = XMVector3Normalize(v2);
			n = XMVector3Cross(v1, v2); // weighted normal

			normals[face[0]] += n;
			normals[face[1]] += n;
			normals[face[2]] += n;
		}

		for (size_t i = 0; i < vertices.size(); i++)
		{
			n = XMVector3Normalize(normals[i]);
			set_normal(vertices[i], n);
		}

		return true;
	}

	// generate per vertex tangent for given triangle mesh
	template <typename VertexType, typename IndexType>
	bool generate_tangent(gsl::span<VertexType> vertices, gsl::span<const Triangle<IndexType>> facets)
	{
		using namespace DirectX::VertexTraits;
		using namespace DirectX;

		if (!has_tangent<VertexType>::value)
			return false;
		//static_assert(has_tangent<VertexType>::value, "The vertex type dose not contains tangent field");

		std::vector<XMVECTOR, XMAllocator> tan1(vertices.size() * 2);
		XMVECTOR* tan2 = &tan1[vertices.size()];
		std::memset(tan1.data(), 0, tan1.size() * sizeof(XMVECTOR));

		for (const auto& face : facets)
		{
			{
				XMVECTOR v0, v1, v2, w0, w1, w2;
				v0 = get_position(vertices[face[0]]);
				w0 = get_uv(vertices[face[0]]);

				v1 = get_position(vertices[face[1]]);
				w1 = get_uv(vertices[face[1]]);

				v2 = get_position(vertices[face[2]]);
				w2 = get_uv(vertices[face[2]]);
			}
			XMFLOAT4A v_1, v_2, w_1, w_2;

			XMStoreA(v_1, v1 - v0);
			XMStoreA(v_2, v2 - v0);
			XMStoreA(w_1, w1 - w0);
			XMStoreA(w_2, w2 - w0);

			float x1 = v_1.x; float x2 = v_2.x;
			float y1 = v_1.y; float y2 = v_2.y;
			float z1 = v_1.z; float z2 = v_2.z;

			float s1 = w_1.x; float s2 = w_2.x;
			float t1 = w_1.y; float t2 = w_2.y;

			float r = 1.0F / (s1 * t2 - s2 * t1);
			XMVECTOR sdir = XMVectorSet(
				(t2 * x1 - t1 * x2) * r,
				(t2 * y1 - t1 * y2) * r,
				(t2 * z1 - t1 * z2) * r, .0f);
			XMVECTOR tdir = XMVectorSet(
				(s1 * x2 - s2 * x1) * r,
				(s1 * y2 - s2 * y1) * r,
				(s1 * z2 - s2 * z1) * r, .0f);

			tan1[face[0]] += sdir;
			tan1[face[1]] += sdir;
			tan1[face[2]] += sdir;

			tan2[face[0]] += tdir;
			tan2[face[1]] += tdir;
			tan2[face[2]] += tdir;
		}

		for (size_t i = 0; i < vertices.size(); i++)
		{
			XMVECTOR n = get_normal(vertices[i], n);
			XMVECTOR t = tan1[i];

			// Gram-Schmidt orthogonalize
			XMVECTOR nt = XMVector3Normalize(t - n * XMVector3Dot(n, t));

			XMVECTOR w = XMVectorLess(XMVector3Dot(XMVector3Cross(n, t), tan2[i]), XMVectorZero());
			w = XMVectorSelect(g_XMNegativeOne.v, g_XMOne.v, w);

			nt = _DXMEXT XMVectorSelect<0, 0, 0, 1>(nt, w);
			set_tangent(vertices[i], nt);
		}

		return true;
	}

	namespace Detail
	{
		XM_HAS_MEMBER(vertices, has_vertices);
		XM_HAS_MEMBER(indices, has_indices);
	}

	template <typename MeshType>
	struct concept_mesh_type : public std::integral_constant<bool, Detail::has_vertices<MeshType>::value && Detail::has_indices<MeshType>::value> {};

	template <typename _MeshType>
	class SubMesh : public submesh_data
	{
	public:
		typedef _MeshType MeshType; // Parent MeshTypw
		typedef typename MeshType::VertexType	VertexType;
		typedef typename MeshType::IndexType	IndexType;
		typedef typename MeshType::FaceType		FaceType;

		static constexpr size_t PolygonVertex = FaceType::VertexCount;

		SubMesh(MeshType& mesh, const submesh_data& metric)
			: parent(mesh), submesh_data(metric)
		{
			vertices = gsl::span<VertexType>(&mesh.vertices[metric.vertex_offset], metric.vertex_count);
			indices = gsl::span<IndexType>(&mesh.indices[metric.index_offset], metric.index_count);
		}

		MeshType&						parent;
		gsl::span<VertexType>			vertices;
		gsl::span<IndexType>			indices;

		gsl::span<Triangle<IndexType>>	facets()
		{
			return gsl::span<const FaceType>(
				reinterpret_cast<const FaceType*>(indices.data()),
				indices.size() / FaceType::VertexCount);
		}

		bool empty() const {
			return vertices.empty() || indices.empty();
		}

	};

	template <typename _VertexType, typename _IndexType = uint16_t, typename _FaceType = Triangle<_IndexType>>
	class PolygonSoup
	{
	public:
		typedef _VertexType VertexType;
		typedef _IndexType	IndexType;
		typedef _FaceType	FaceType;

		static constexpr size_t PolygonVertex = FaceType::VertexCount;

		PolygonSoup() {}

		bool empty() const {
			return vertices.empty() || indices.empty();
		}

		void clear()
		{
			vertices.clear();
			indices.clear();
		}

		inline const FaceType& facet(int idx) const
		{
			return reinterpret_cast<const FaceType&>(indices[idx * FaceType::VertexCount]);
		}

		inline FaceType& facet(int idx)
		{
			return reinterpret_cast<FaceType&>(indices[idx * FaceType::VertexCount]);
		}

		void add_facet(const FaceType& new_facet)
		{
			indices.insert(indices.end(), new_facet.begin(), new_facet.end());
		}

		template <class... _TIndecies>
		std::enable_if_t<sizeof...(_TIndecies) == FaceType::VertexCount> add_facet(_TIndecies... _indices)
		{
			indices.insert(indices.end(), { static_cast<IndexType>(_indices)... });
		}

		gsl::span<FaceType> facets() {
			return gsl::span<FaceType>(
				reinterpret_cast<FaceType*>(indices.data()),
				indices.size() / FaceType::VertexCount);
		}

		gsl::span<const FaceType> facets() const {
			return gsl::span<const FaceType>(
				reinterpret_cast<const FaceType*>(indices.data()),
				indices.size() / FaceType::VertexCount);
		}

		const VertexType& vertex(int facet, int vidx) const
		{
			return this->vertices[this->indices[facet * FaceType::VertexCount + vidx]];
		}

		VertexType& vertex(int facet, int vidx)
		{
			return this->vertices[this->indices[facet * FaceType::VertexCount + vidx]];
		}

		// flip all the polygons' winding and vertices' normal
		void flip()
		{
			using namespace DirectX::VertexTraits;

			if (has_normal<VertexType>::value)
			{
				for (auto& v : vertices)
					set_normal(v, -get_normal(v));
			}

			static constexpr int vc = FaceType::VertexCount;
			for (int i = 0; i < indices.size() / vc; i++)
				std::reverse(indices.begin() + i * vc, indices.begin() + (i + 1)* vc);
		}

		void generate_normal()
		{
			static_assert(PolygonVertex == 3, "This mesh is not trigle mesh, please trianglize it first");
			generate_normal<VertexType, IndexType>(this->vertices, this->facets());
		}

		void generate_tangent()
		{
			static_assert(PolygonVertex == 3, "This mesh is not trigle mesh, please trianglize it first");
			generate_tangent<VertexType, IndexType>(this->vertices, this->facets());
		}

		// applies an uniform transform to all vertices in the mesh
		void XM_CALLCONV transform(DirectX::FXMMATRIX M)
		{
			using namespace DirectX::VertexTraits;
			for (auto& v : vertices)
			{
				XMVECTOR p = get_position(v);
				p = _DXMEXT XMVector3TransformCoord(p, M);
				set_position(v, p);

				if (has_normal<VertexType>::value)
				{
					p = get_normal(v);
					p = _DXMEXT XMVector3TransformNormal(p, M);
					set_normal(v, p);

					if (has_tangent<VertexType>::value)
					{
						p = get_tangent(v);
						p = _DXMEXT XMVector3TransformNormal(p, M);
						set_tangent(v, p);
					}
				}
			}
		}

		std::vector<VertexType, DirectX::AlignedAllocator<VertexType>> vertices;
		std::vector<IndexType>	indices;
	};

	template <class _VertexType, class _IndexType>
	int XM_CALLCONV intersect(const PolygonSoup<_VertexType, _IndexType, Triangle<_IndexType>> &Mesh, DirectX::FXMVECTOR Origin, DirectX::FXMVECTOR Direction, std::vector<float>* distances = nullptr)
	{
		using namespace DirectX;
		using namespace DirectX::VertexTraits;
		int count = 0;
		XMVECTOR vDir = XMVector3Normalize(Direction);
		for (const auto& tri : Mesh.facets())
		{
			float distance;

			XMVECTOR v0 = get_position(&Mesh.vertices[tri[0]]);
			XMVECTOR v1 = get_position(&Mesh.vertices[tri[1]]);
			XMVECTOR v2 = get_position(&Mesh.vertices[tri[2]]);

			bool hr = DirectX::TriangleTests::Intersects(Origin, vDir, v0, v1, v2, distance);
			if (hr) {
				++count;
				if (distances) {
					distances->push_back(distances);
				}
			}
		}
		return count;
	}

	template <typename _TVertex>
	struct MeshRayIntersectedVertex : public _TVertex
	{
		using Vector3 = DirectX::Vector3;

		float	distance;
		Vector3 barycentric;
		int		facet; // facet id

		inline bool operator < (const MeshRayIntersectedVertex& rhs)
		{
			return this->distance < rhs.distance;
		}

		inline bool is_valiad() const {return  facet >= 0; }
		inline operator bool() const { return is_valiad(); }
	};

	template <size_t _Dim>
	struct PolygonContainmentInfo
	{
		static constexpr size_t Dim = _Dim;
		DirectX::ContainmentType containment; // overall polygon containment type
		DirectX::ContainmentType vertex_containments[Dim];
		operator DirectX::ContainmentType() const { return containment; }
	};

	struct XM_ALIGNATTR aabb_t {
		::hlsl::xmvector3f min;
		::hlsl::xmvector3f max;
		mutable int	 intersected;

		template <size_t dim>
		static bool less(const aabb_t& lhs, const aabb_t & rhs)
		{
			using namespace hlsl;
			return hlsl::less(lhs.min, rhs.min).get<dim>();
		}

		aabb_t operator+(const aabb_t& rhs) const
		{
			const aabb_t& lhs = *this;
			using namespace ::hlsl;
			aabb_t ret{ ::hlsl::min(lhs.min,rhs.min),::hlsl::max(lhs.max,rhs.max) };
			return ret;
		}

		// Convert min-max representation of this box to center-extent representation
		DirectX::BoundingBox get_dxbox() const
		{
			const float epsilon = 1e-3;
			using namespace ::hlsl;
			auto& aabb = *this;
			DirectX::BoundingBox box;
			xmvector3f extent = (aabb.max - aabb.min) * 0.5f;
			extent = ::hlsl::max(extent, xmfloat(epsilon));
			box.Center << (aabb.max + aabb.min) * 0.5f;
			box.Extents << extent;
			return box;
		}

		operator DirectX::BoundingBox() const { return get_dxbox(); }
	};

	// A struct stores fixed size polygon (position only)
	template <size_t _Size>
	struct PlainPolygon {
		hlsl::xmvector3f v[_Size];
	};

	/// <summary>
	/// Basic triangle mesh, each index represent an edge, which is the edge oppsite to the vertex in it's owner triangle
	/// </summary>
	template <typename _VertexType, typename _IndexType = uint16_t>
	class TriangleMesh : public PolygonSoup<_VertexType, _IndexType, Triangle<_IndexType>>
	{
	public:
		static const size_t VertexCount = FaceType::VertexCount;
		using triangle_handle = IndexType;
		using PlainTriangle = PlainPolygon<3>;
		using ContainmentType = DirectX::ContainmentType;
		using BaseType = PolygonSoup<_VertexType, _IndexType, Triangle<_IndexType>>;
		using BaseType::PolygonVertex;
		using typename BaseType::VertexType;
		using typename BaseType::IndexType;
		using IntersectionVertex = MeshRayIntersectedVertex<_VertexType>;

	public:
		// edge's reverse edge
		// stores the adjacent edges of a edge in a triangle
		std::vector<IndexType>	revedges;
		DirectX::BoundingBox	aabb;
		DirectX::BoundingOrientedBox obb;

		PlainTriangle XM_CALLCONV get_triangle_position(IndexType fid) const
		{
			using namespace DirectX::VertexTraits;
			using namespace hlsl;
			auto f = this->indices.begin() + fid * 3;
			PlainTriangle tri = {
			xmvector3f(get_position(this->vertices[f[0]]))
			,xmvector3f(get_position(this->vertices[f[1]]))
			,xmvector3f(get_position(this->vertices[f[2]])) };
			return tri;
		}

#define _EXPANDTRI(tri) tri.v[0].v, tri.v[1].v, tri.v[2].v

		aabb_t XM_CALLCONV get_triangle_aabb(triangle_handle t) const
		{
			using namespace hlsl;
			PlainTriangle tri = get_triangle_position(t);
			xmvector3f vmax = hlsl::max(tri.v[0], tri.v[1]);
			vmax = hlsl::max(vmax, tri.v[2]);
			xmvector3f vmin = hlsl::min(tri.v[0], tri.v[1]);
			vmin = hlsl::min(vmin, tri.v[2]);
			aabb_t ret;
			ret.min = vmin;
			ret.max = vmax;
			ret.intersected = true;
			return ret;
		}

		using triangle_bvh_t = kd_bvh<triangle_handle, float, 3, aabb_t>;
		triangle_bvh_t				triangle_bvh;

		using vertex_bvh_t = kd_bvh<::hlsl::xmvector3f, float, 3, aabb_t>;
		vertex_bvh_t				vertex_bvh;
		// An Lookup table to find the 'Ienditification' of a vertex
		// Make sense when multiple vertex shares the same spatial location but have different index
		std::vector<IndexType>		vertex_id;

		TriangleMesh() : triangle_bvh([this](const triangle_handle& t) {
			return this->get_triangle_aabb(t);
		}) {}

		// vertex's first adjacant edge
		// std::vector<IndexType> vedges;

		union EdgeType
		{
			_IndexType v[2];
			struct {
				_IndexType v0, v1;
			};
		};

		inline EdgeType edge(int facet, int edge) const
		{
			EdgeType e;
			auto& tri = this->facet(facet);
			switch (edge)
			{
			case 0:
				e.v0 = tri[1];
				e.v1 = tri[2];
				break;
			case 1:
				e.v0 = tri[2];
				e.v1 = tri[0];
				break;
			case 2:
				e.v0 = tri[0];
				e.v1 = tri[1];
				break;
			default:
				e.v0 = -1;
				e.v1 = -1;
				break;
			}
			return e;
		}

		inline EdgeType edge(int eid) const
		{
			return edge(eid / VertexCount, eid % VertexCount);
		}

		inline int adjacentFacet(int facet, int edge) const
		{
			return revedges[facet * VertexCount + edge] / VertexCount;
		}

		inline int adjacentFacet(int eid) const
		{
			return revedges[eid] / VertexCount;
		}

		void build_reverse_edges()
		{
			// intialize all adjacant edges to -1
			revedges.resize(this->indices.size(), -1);
			_IndexType vsize = this->vertices.size();
			int esize = this->indices.size();
			int fsize = this->indices.size() / VertexCount;

			using hash_type = uint64_t;
			// make sure int for hash is enough
			assert(vsize*vsize < std::numeric_limits<hash_type>::max());
			std::unordered_map<hash_type, _IndexType> edges(esize * 2);
			// max items inside this table should be less than (esize / 2)

			for (_IndexType fid = 0; fid < fsize; fid++)
			{
				for (_IndexType i = 0; i < VertexCount; i++)
				{
					_IndexType eid = i + fid * VertexCount;
					auto e = this->edge(fid, i);
					e.v0 = vertex_id[e.v0];
					e.v1 = vertex_id[e.v1];

					hash_type ehash = e.v0 * vsize + e.v1;
					hash_type revehash = e.v0 + e.v1 * vsize;

					auto revItr = edges.find(revehash);
					if (revItr == edges.end())
						edges[ehash] = eid;
					else // find reversed edge, remove from edges map
					{
						auto revEid = revItr->second;
						revedges[eid] = revEid;
						revedges[revEid] = eid;
						auto reve = this->edge(revEid);
						reve.v0 = vertex_id[reve.v0];
						reve.v1 = vertex_id[reve.v1];
						assert(reve.v0 == e.v1 && reve.v1 == e.v0);
						edges.erase(revItr);
					}
				}
			}
		}

		// build the adjacent map so we can access all the 1 rings
		void build()
		{
			int vsize = this->vertices.size();
			int esize = this->indices.size();
			int fsize = this->indices.size() / VertexCount;

			// rebuild the triangle-bvh
			triangle_bvh.resize(fsize);
			std::iota(triangle_bvh.begin(), triangle_bvh.end(), 0);
			triangle_bvh.rebuild();

			// build the vertex-bvh
			const aabb_t& bb = this->triangle_bvh.get_volumn(this->triangle_bvh.root());
			::hlsl::xmvector3f voxel_ext(1e-4f);

			vertex_bvh.set_volumn_getter([voxel_ext](const ::hlsl::xmvector3f v) {
				return aabb_t{ v - voxel_ext , v + voxel_ext, 0 };
			});
			vertex_bvh.resize(vsize);
			for (int i = 0; i < vsize; i++)
				vertex_bvh[i] = ::hlsl::xmvector3f(::DirectX::VertexTraits::get_position(this->vertices[i]));
			vertex_bvh.rebuild();

			vertex_id.resize(vsize);
			std::iota(vertex_id.begin(), vertex_id.end(), 0);
			std::unordered_map<uint64_t, IndexType> vids(vsize);

			// Use 20-bits to hash a float component, plus 1 bit signbit
			// Generate a total 63-bits
			auto hash_position = [](const ::hlsl::xmvector3f vp) -> uint64_t{
				auto vf = vp * 3e4f; // 30 * 3e4 = 9e5 < 1e6 = 2^20
				auto vi = vf.cast<uint32_t>();
				uint32_t signbits32 = ::hlsl::detail::move_mask(vi);
				uint64_t signbits = signbits32;
				vi &= ::hlsl::xmuint((1ULL << 20ULL) - 1ULL);
				uint32_t vis32[3];
				vis32 << vi;
				uint64_t vis[3] = { vis32[0], vis32[1], vis32[2]};
				uint64_t vhash = ((signbits&7) << 60ULL) | (vis[0] << 40ULL) | (vis[1] << 20ULL) | (vis[2]);
				return vhash;
			};

			for (int i = 0; i < vsize; i++)
			{
				auto vp = *vertex_bvh.get_object(i);
				std::pair<uint64_t, IndexType> p = { hash_position(vp), i };
				auto rib = vids.insert(p);
				IndexType vid = rib.first->second;
				if (!rib.second && vid != i)
				{
					// query-merge-set merge algorithm
					int root = vid;
					// find the ultimate parent (root)
					while (vertex_id[root] != root) root = vertex_id[root];

					while (vid != root)
					{
						int cvid = vid;
						vid = vertex_id[vid];
						vertex_id[cvid] = root;
					}
					vertex_id[i] = root;
					//if (vid != i)
					//	std::cout << "Glued vertex pair (" << i << ',' << vid << ')' << std::endl;
				}
			}

			build_reverse_edges();

			//for (int fid = 0; fid < this->indices.size() / 3; ++fid)
			//{
			//	auto tri = this->facet(fid);
			//	using namespace DirectX::VertexTraits;
			//	DirectX::XMVECTOR v0 = get_position(this->vertices[tri[0]]);
			//	DirectX::XMVECTOR v1 = get_position(this->vertices[tri[1]]);
			//	DirectX::XMVECTOR v2 = get_position(this->vertices[tri[2]]);
			//	auto&& vol = triangle_bvh.get_volumn(fid);
			//	bool cond = true;
			//	assert(DirectX::XMVector3LessOrEqual(v0, vol.max.v));
			//	assert(DirectX::XMVector3LessOrEqual(v1, vol.max.v));
			//	assert(DirectX::XMVector3LessOrEqual(v2, vol.max.v));
			//	assert(DirectX::XMVector3LessOrEqual(vol.min.v, v0));
			//	assert(DirectX::XMVector3LessOrEqual(vol.min.v, v1));
			//	assert(DirectX::XMVector3LessOrEqual(vol.min.v, v2));
			//}
		}

		// generate a persoude vertex from the interpolation of the trianlge
		VertexType persudo_vertex(int fid, DirectX::FXMVECTOR baycentric) const
		{
			const auto& tri = this->facet(fid);

			const VertexType& v0 = this->vertices[tri[0]];
			const VertexType& v1 = this->vertices[tri[1]];
			const VertexType& v2 = this->vertices[tri[2]];

			using namespace DirectX::VertexTraits;
			return interpolate_vertex(baycentric, v0, v1, v2);
		}

		protected:
			IntersectionVertex XM_CALLCONV get_ray_intersected_vertex(DirectX::FXMVECTOR vOri, DirectX::FXMVECTOR vDir, IndexType fid) const
			{
				using DirectX::XMVECTOR;
				IntersectionVertex info;
				float distance = .0f;
				info.facet = -1;
				PlainTriangle tri = this->get_triangle_position(fid);
				bool hr = DirectX::TriangleTests::Intersects(vOri, vDir, _EXPANDTRI(tri), distance);
				if (!hr) return info; // No intersection, return with info.facet == -1
				// Fill the persudo-vertex
				XMVECTOR pos = distance * vDir + vOri;
				XMVECTOR baryc = DirectX::TriangleTests::BarycentricCoordinate(pos, _EXPANDTRI(tri));
				info.barycentric = baryc;
				info.facet = fid;
				info.distance = distance;
				(VertexType&)info = this->persudo_vertex(fid, baryc);
				return info;
			}

			template <typename _Ty>
			inline static void push_back_output_itr(nullptr_t itr, _Ty&& element)
			{
			}

			template <typename _OutItr, typename _Ty>
			inline static std::enable_if_t<!std::is_same<_OutItr,nullptr_t>::value>
				push_back_output_itr(_OutItr& itr, _Ty&& element)
			{
				static_assert(std::is_same<std::decay_t<decltype(*itr)>,std::decay_t<_Ty>>::value,"Output Iterator's Type is not expected");
				*(itr++) = element;
			}

		public:

		IntersectionVertex XM_CALLCONV first_intersect(DirectX::FXMVECTOR Origin, DirectX::FXMVECTOR Direction) const
		{
			using namespace DirectX;
			assert(this->triangle_bvh.valiad() && "This triangle mesh is modified but not rebuild");

			XMVECTOR vOri = Origin;
			XMVECTOR vDir = XMVector3Normalize(Direction);
			auto ray_aabb_inter = [vOri, vDir](const aabb_t& aabb) -> float
			{
				float f;
				auto box = aabb.get_dxbox();
				bool succ = box.Intersects(vOri, vDir, f);
				aabb.intersected = succ;
				return succ ? f : -1.0f;
			};
			auto ray_triangle_inter = [this, vOri, vDir](const int fid) -> float
			{
				PlainTriangle tri = this->get_triangle_position(fid);
				float f;
				bool succ = DirectX::TriangleTests::Intersects(vOri, vDir, _EXPANDTRI(tri), f);
				return succ ? f : -1.0f;
			};

			//std::cout << counter << std::endl;
			auto pfid = find_first_of(this->triangle_bvh, ray_triangle_inter, ray_aabb_inter);
			IntersectionVertex info;
			info.facet = -1;
			if (pfid)
				info = get_ray_intersected_vertex(vOri, vDir, *pfid);
			return info;
		}

		// Ray intersection test with advanced infomation
		template <typename _TOutItr>
		int XM_CALLCONV all_intersections(DirectX::FXMVECTOR Origin, DirectX::FXMVECTOR Direction, _TOutItr outItr) const
		{
			using namespace DirectX;
			assert(this->triangle_bvh.valiad() && "This triangle mesh is modified but not rebuild");

			size_t count = 0;
			XMVECTOR vOri = Origin;
			XMVECTOR vDir = XMVector3Normalize(Direction);

			struct XM_ALIGNATTR ray_aabb_intersector {
				DirectX::XMVECTOR origin;
				DirectX::XMVECTOR direction;
				bool XM_CALLCONV operator ()(const aabb_t& aabb) const
				{
					float f;
					auto box = aabb.get_dxbox();
					bool succ = box.Intersects(origin, direction, f);
					aabb.intersected = succ;
					return succ;
				}
			} intersector{ vOri , vDir };

			for (auto triangles = find_all_of(triangle_bvh, intersector);
				triangles;
				++triangles)
			{
				int fid = *triangles;
				IntersectionVertex info = get_ray_intersected_vertex(vOri, vDir, fid);
				if (info.is_valiad()) {
					++count;
					push_back_output_itr(outItr, std::move(info));
				}
			}
			return count;
		}

		template <typename _TPred, typename _TContainer>
		void find_adjacant_facets_of(IndexType facet_id, _TPred&& pred, _TContainer& result_map) const
		{
			auto containment = pred(facet_id);
			result_map.insert(std::make_pair(facet_id, containment));
			if (!containment) return;
			for (int e = 0; e < PolygonVertex; e++)
			{
				int eid = facet_id * PolygonVertex + e;
				auto rev_edge = this->revedges[eid];
				auto adj_face = rev_edge / PolygonVertex;
				// if not visited yet
				if (rev_edge != -1 && result_map.find(adj_face) == result_map.end())
					this->find_adjacant_facets_of(adj_face, pred, result_map);
			}
		}

	};

	namespace Internal
	{
#ifndef min
		template <typename T>
		inline T clamp(T value, T minV, T maxV)
		{
			return std::max(std::min(value, maxV), minV);
		}
#else
		template <typename T>
		inline T clamp(T value, T minV, T maxV)
		{
			return max(min(value, maxV), minV);
		}
#endif
	}

	/// <summary>
	/// Compute the closest projection point from P0 to triangle(V0,V1,V2).
	/// </summary>
	/// <param name="P0">The p0.</param>
	/// <param name="V0">The v0.</param>
	/// <param name="V1">The v1.</param>
	/// <param name="V2">The v2.</param>
	/// <returns></returns>
	inline DirectX::XMVECTOR XM_CALLCONV Projection(DirectX::FXMVECTOR P0, DirectX::FXMVECTOR V0, DirectX::FXMVECTOR V1, DirectX::GXMVECTOR V2)
	{
		using namespace DirectX;
		using namespace Geometrics::Internal;
		XMVECTOR edge0 = V1 - V0;
		XMVECTOR edge1 = V2 - V0;
		XMVECTOR p0 = V0 - P0;

		//XMVectorBaryCentric();

		float a = XMVectorGetX(_DXMEXT XMVector3LengthSq(edge0));
		float b = XMVectorGetX(_DXMEXT XMVector3Dot(edge0, edge1));
		float c = XMVectorGetX(_DXMEXT XMVector3LengthSq(edge1));
		float d = XMVectorGetX(_DXMEXT XMVector3Dot(edge0, p0));
		float e = XMVectorGetX(_DXMEXT XMVector3Dot(edge1, p0));

		float det = a*c - b*b;
		float s = b*e - c*d;
		float t = b*d - a*e;

		if (s + t < det)
		{
			if (s < 0.f)
			{
				if (t < 0.f)
				{
					if (d < 0.f)
					{
						s = clamp(-d / a, 0.f, 1.f);
						t = 0.f;
					}
					else
					{
						s = 0.f;
						t = clamp(-e / c, 0.f, 1.f);
					}
				}
				else
				{
					s = 0.f;
					t = clamp(-e / c, 0.f, 1.f);
				}
			}
			else if (t < 0.f)
			{
				s = clamp(-d / a, 0.f, 1.f);
				t = 0.f;
			}
			else
			{
				float invDet = 1.f / det;
				s *= invDet;
				t *= invDet;
			}
		}
		else
		{
			if (s < 0.f)
			{
				float tmp0 = b + d;
				float tmp1 = c + e;
				if (tmp1 > tmp0)
				{
					float numer = tmp1 - tmp0;
					float denom = a - 2 * b + c;
					s = clamp(numer / denom, 0.f, 1.f);
					t = 1 - s;
				}
				else
				{
					t = clamp(-e / c, 0.f, 1.f);
					s = 0.f;
				}
			}
			else if (t < 0.f)
			{
				if (a + d > b + e)
				{
					float numer = c + e - b - d;
					float denom = a - 2 * b + c;
					s = clamp(numer / denom, 0.f, 1.f);
					t = 1 - s;
				}
				else
				{
					s = clamp(-e / c, 0.f, 1.f);
					t = 0.f;
				}
			}
			else
			{
				float numer = c + e - b - d;
				float denom = a - 2 * b + c;
				s = clamp(numer / denom, 0.f, 1.f);
				t = 1.f - s;
			}
		}

		return V0 + s * edge0 + t * edge1;
	}

	inline float XM_CALLCONV Distance(DirectX::FXMVECTOR P0, DirectX::FXMVECTOR V0, DirectX::FXMVECTOR V1, DirectX::GXMVECTOR V2)
	{
		using namespace DirectX;
		XMVECTOR vProj = Projection(P0, V0, V1, V2);
		vProj -= P0;
		return XMVectorGetX(_DXMEXT XMVector3Length(vProj));
	}

	template <typename _VertexType, typename _IndexType>
	inline float XM_CALLCONV Distance(const TriangleMesh<_VertexType, _IndexType> &Mesh, DirectX::FXMVECTOR Point)
	{
		using namespace DirectX;
		float minDis = numeric_limits<float>::max();
		for (const auto& tri : Mesh.facets())
		{
			float dis;

			XMVECTOR v0 = XMLoadFloat3(&Mesh.vertices[tri[0]].position);
			XMVECTOR v1 = XMLoadFloat3(&Mesh.vertices[tri[1]].position);
			XMVECTOR v2 = XMLoadFloat3(&Mesh.vertices[tri[2]].position);

			dis = Distance(Point, v0, v1, v2);
			if (dis < minDis) minDis = dis;
			//minDis = std::min(dis,minDis);
		}

		//XMVECTOR vDis = XMVectorReplicate(numeric_limits<float>::max());
		//for (const auto& vertex : Mesh.vertices)
		//{
		//	XMVECTOR vPos = XMLoadFloat3(&vertex.position);
		//	vPos -= Point;
		//	XMVECTOR dis = XMVector3Length(vPos);
		//	vDis = XMVectorMin(vDis,dis);
		//}

		//float minDisV = XMVectorGetX(vDis);
		//assert(minDisV >= minDis);
		return minDis;
		//return XMVectorGetX(minDis);
	}

	template <typename _VertexType, typename _IndexType>
	bool XM_CALLCONV Inside(const TriangleMesh<_VertexType, _IndexType> &Mesh, DirectX::FXMVECTOR Point)
	{
		XMFLOAT3A Direction;
		Direction.x = (float)std::rand() / (RAND_MAX + 1);
		Direction.y = (float)std::rand() / (RAND_MAX + 1);
		Direction.z = (float)std::rand() / (RAND_MAX + 1);
		XMVECTOR vDir = XMLoadFloat3A(&Direction);
		auto count = intersect(Mesh, Point, vDir, nullptr);
		return count & 1; //count % 2
	}

}


/**
 *
 * @file qwrapper_cpotrf.c
 *
 *  PLASMA core_blas quark wrapper
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Hatem Ltaief
 * @author Mathieu Faverge
 * @author Jakub Kurzak
 * @date 2010-11-15
 * @generated c Tue Jan  7 11:44:56 2014
 *
 **/
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 **/
void QUARK_CORE_cpotrf(Quark *quark, Quark_Task_Flags *task_flags,
                       PLASMA_enum uplo, int n, int nb,
                       PLASMA_Complex32_t *A, int lda,
                       PLASMA_sequence *sequence, PLASMA_request *request,
                       int iinfo)
{
    DAG_CORE_POTRF;
    QUARK_Insert_Task(quark, CORE_cpotrf_quark, task_flags,
        sizeof(PLASMA_enum),                &uplo,      VALUE,
        sizeof(int),                        &n,         VALUE,
        sizeof(PLASMA_Complex32_t)*nb*nb,    A,                 INOUT,
        sizeof(int),                        &lda,       VALUE,
        sizeof(PLASMA_sequence*),           &sequence,  VALUE,
        sizeof(PLASMA_request*),            &request,   VALUE,
        sizeof(int),                        &iinfo,     VALUE,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_cpotrf_quark = PCORE_cpotrf_quark
#define CORE_cpotrf_quark PCORE_cpotrf_quark
#endif
void CORE_cpotrf_quark(Quark *quark)
{
    PLASMA_enum uplo;
    int n;
    PLASMA_Complex32_t *A;
    int lda;
    PLASMA_sequence *sequence;
    PLASMA_request *request;
    int iinfo;

    int info;

    quark_unpack_args_7(quark, uplo, n, A, lda, sequence, request, iinfo);
    info = LAPACKE_cpotrf_work(
        LAPACK_COL_MAJOR,
        lapack_const(uplo),
        n, A, lda);
    if (sequence->status == PLASMA_SUCCESS && info != 0)
        plasma_sequence_flush(quark, sequence, request, iinfo+info);
}


// Geocode a DEM stored in ASF .meta format.

// Standard libraries.
#include <limits.h>
#include <math.h>
#include <string.h>

// Libraries from packages outside ASF.
#include <gsl/gsl_math.h>

// Libraries developed at ASF.
#include <asf.h>
#include <asf_meta.h>
#include <asf_raster.h>
#include <float_image.h>
#include <libasf_proj.h>

// Headers defined by this library.
#include "asf_geocode.h"

// Return true iff (x, y) falls in the region
// ([0, image->size_x - 1), [0, image->size_y - 1))
static int
in_image (FloatImage *image, ssize_t x, ssize_t y)
{
  g_assert (image->size_x <= SSIZE_MAX && image->size_y <= SSIZE_MAX);
  ssize_t sz_x = image->size_x, sz_y = image->size_y;

  return 0 <= x && x < sz_x && 0 <= y && y < sz_y;
}

// Given a projected image with metadata md, an unproject_input
// function, and pixel indicies (x, y), return the latitude and
// longitude of x, y (relative to whatever libasf_proj input spheroid
// is in effect at the time this function is called.  Probably easiest
// to see the purpose of this routine from the context in which it is
// called.
static int
get_pixel_lat_long (const meta_parameters *md, projector_t unproject_input,
		    size_t x, size_t y, double *lat, double *lon)
{
  // Convenience aliases.
  meta_projection *pb = md->projection;
  project_parameters_t *pp = &(md->projection->param);

  double xpc = pb->startX + pb->perX * x;
  double ypc = pb->startY + pb->perY * y;

  // Pseudoprojected images are in degrees and the projection and
  // unprojection function deal in radians.
  if ( md->projection->type == LAT_LONG_PSEUDO_PROJECTION ) {
    xpc *= D2R;
    ypc *= D2R;
  }

  int return_code = unproject_input (pp, xpc, ypc, ASF_PROJ_NO_HEIGHT,
				     lat, lon, NULL, md->projection->datum);
  g_assert (return_code);

  return return_code;
}

int
geocode_dem (projection_type_t projection_type,	// What we are projection to.
	     project_parameters_t *pp,    // Parameters we project to.
	     datum_type_t datum,                // Datum we project to.
	     // Pixel size of output image, in output projection units
	     // (meters or possibly degrees, if we decide to support
	     // projecting to pseudoprojected form).
	     double pixel_size,
	     resample_method_t resample_method,	// How to resample pixels.
	     const char *input_image, // Base name of input image.
	     const meta_parameters *imd, // Input DEM image metadata.
	     const char *output_image  // Base name of output image.
	     )
{
  int return_code;		// Holds return codes from functions.

  // Function to use to project or unproject between latlon and input
  // or output coordinates.
  projector_t project_input; 	// latlon => input image map projection
  projector_t unproject_input;	// input image_map_projection => latlon
  projector_t project_output;	// latlon => output image map projection
  projector_t unproject_output;	// output image map projection => latlon
  // Like the above, but act on arrays.
  array_projector_t array_project_input, array_unproject_input;
  array_projector_t array_project_output, array_unproject_output;

  // We only deal with reprojection map projected DEMs.
  g_assert (imd->projection != NULL);

  // FIXME: what to do with background value is something that still
  // needs to be determined (probably in consultation with the guys
  // working on terrain correction).
  const float background_value = 0.0;

  // Geocoding to pseudoprojected form presents issues, for example
  // with the meaning of the pixel_size argument, which is taken as a
  // distance in map projection coordinates for all other projections
  // (deciding how to interpret it when projecting to pseudoprojected
  // form is tough), and since there probably isn't much need, we
  // don't allow it.
  g_assert (projection_type != LAT_LONG_PSEUDO_PROJECTION);

  // Get the functions we want to use for projecting and unprojecting.
  set_projection_functions (imd->projection->type, &project_input,
			    &unproject_input, &array_project_input,
			    &array_unproject_input);
  set_projection_functions (projection_type, &project_output,
			    &unproject_output, &array_project_output,
			    &array_unproject_output);

  // Input image dimensions in pixels in x and y directions.
  size_t ii_size_x = imd->general->sample_count;
  size_t ii_size_y = imd->general->line_count;

  // Convenience aliases.
  meta_projection *ipb = imd->projection;
  project_parameters_t *ipp = &imd->projection->param;

  // First we march around the entire outside of the image and compute
  // projection coordinates for every pixel, keeping track of the
  // minimum and maximum projection coordinates in each dimension.
  // This lets us determine the exact extent of the DEM in
  // output projection coordinates.
  asfPrintStatus ("Determining input image extent in projection coordinate "
		  "space... ");

  double min_x = DBL_MAX;
  double max_x = -DBL_MAX;
  double min_y = DBL_MAX;
  double max_y = -DBL_MAX;

  // In going around the edge, we are just trying to determine the
  // extent of the image in the horizontal, so we don't care about
  // height yet.
  { // Scoping block.
    // Number of pixels in the edge of the image.
    size_t edge_point_count = 2 * ii_size_x + 2 * ii_size_y - 4;
    double *lats = g_new0 (double, edge_point_count);
    double *lons = g_new0 (double, edge_point_count);
    size_t current_edge_point = 0;
    size_t ii = 0, jj = 0;
    for ( ; ii < ii_size_x - 1 ; ii++ ) {
      return_code = get_pixel_lat_long (imd, unproject_input, ii, jj,
					&(lats[current_edge_point]),
					&(lons[current_edge_point]));
      g_assert (return_code);
      current_edge_point++;
    }
    for ( ; jj < ii_size_y - 1 ; jj++ ) {
      return_code = get_pixel_lat_long (imd, unproject_input, ii, jj,
					&(lats[current_edge_point]),
					&(lons[current_edge_point]));
      g_assert (return_code);
      current_edge_point++;
    }
    for ( ; ii > 0 ; ii-- ) {
      return_code = get_pixel_lat_long (imd, unproject_input, ii, jj,
					&(lats[current_edge_point]),
					&(lons[current_edge_point]));
      g_assert (return_code);
      current_edge_point++;
    }
    for ( ; jj > 0 ; jj-- ) {
      return_code = get_pixel_lat_long (imd, unproject_input, ii, jj,
					&(lats[current_edge_point]),
					&(lons[current_edge_point]));
      g_assert (return_code);
      current_edge_point++;
    }
    g_assert (current_edge_point == edge_point_count);
    // Pointers to arrays of projected coordinates to be filled in.
    // The projection function will allocate this memory itself.
    double *x = NULL, *y = NULL;
    // Project all the edge pixels.
    return_code = array_project_output (pp, lats, lons, NULL, &x, &y, NULL,
					edge_point_count, datum);
    g_assert (return_code == TRUE);
    // Find the extents of the image in projection coordinates.
    for ( ii = 0 ; ii < edge_point_count ; ii++ ) {
      if ( x[ii] < min_x ) { min_x = x[ii]; }
      if ( x[ii] > max_x ) { max_x = x[ii]; }
      if ( y[ii] < min_y ) { min_y = y[ii]; }
      if ( y[ii] > max_y ) { max_y = y[ii]; }
    }

    free (y);
    free (x);
    g_free (lons);
    g_free (lats);
  }

  asfPrintStatus ("done.\n\n");

  // Issue a warning when the chosen pixel size is smaller than the
  // input pixel size.  FIXME: this condition will really never fire
  // for pseudoprojected image, since the pixels size of the input is
  // tiny (degrees per pixel) and the pixel_size has already been
  // computed in asf_geocode function itself as an arc length on the
  // ground.
  if ( GSL_MIN(imd->general->x_pixel_size,
	       imd->general->y_pixel_size) > pixel_size ) {
    asfPrintWarning
      ("Requested pixel size %lf is smaller then the input image resolution "
       "(%le meters).\n", pixel_size,
       GSL_MIN (imd->general->x_pixel_size, imd->general->y_pixel_size));
  }

  // The pixel size requested by the user better not oversample by the
  // factor of 2.  Specifying --force will skip this check.  FIXME:
  // same essential problem as the above condition, but in this case
  // it always goes off.
  //  if (!force_flag && GSL_MIN(imd->general->x_pixel_size,
  //	       imd->general->y_pixel_size) > (2*pixel_size) ) {
  //    report_func
  //      ("Requested pixel size %lf is smaller then the minimum implied by half \n"
  //       "the input image resolution (%le meters), this is not supported.\n",
  //       pixel_size, GSL_MIN (imd->general->x_pixel_size,
  //			    imd->general->y_pixel_size));
  //  }

  asfPrintStatus ("Opening input DEM image... ");
  char *input_data_file = (char *) MALLOC(sizeof(char)*(strlen(input_image)+5));
  sprintf(input_data_file, "%s.img", input_image);
  FloatImage *iim
    = float_image_new_from_file (ii_size_x, ii_size_y, input_data_file, 0,
				 FLOAT_IMAGE_BYTE_ORDER_BIG_ENDIAN);
  FREE(input_data_file);
  asfPrintStatus ("done.\n\n");

  // Maximum pixel indicies in output image.
  size_t oix_max = ceil ((max_x - min_x) / pixel_size);
  size_t oiy_max = ceil ((max_y - min_y) / pixel_size);

  // Output image dimensions.
  size_t oi_size_x = oix_max + 1;
  size_t oi_size_y = oiy_max + 1;

  // Output image.
  FloatImage *oim = float_image_new (oi_size_x, oi_size_y);

  // Translate the command line notion of the resampling method into
  // the lingo known by the float_image class.  The compiler is
  // reassured with a default.
  float_image_sample_method_t float_image_sample_method
    = FLOAT_IMAGE_SAMPLE_METHOD_BILINEAR;
  switch ( resample_method ) {
  case RESAMPLE_NEAREST_NEIGHBOR:
    float_image_sample_method = FLOAT_IMAGE_SAMPLE_METHOD_NEAREST_NEIGHBOR;
    break;
  case RESAMPLE_BILINEAR:
    float_image_sample_method = FLOAT_IMAGE_SAMPLE_METHOD_BILINEAR;
    break;
  case RESAMPLE_BICUBIC:
    float_image_sample_method = FLOAT_IMAGE_SAMPLE_METHOD_BICUBIC;
    break;
  default:
    g_assert_not_reached ();
  }

  // We need to find the z coordinates in the output projection of all
  // the pixels in the input DEM.  We store these values in their own
  // FloatImage instance.

  //FloatImage *x_coords = float_image_new (ii_size_x, ii_size_y);
  //FloatImage *y_coords = float_image_new (ii_size_x, ii_size_y);
  FloatImage *z_coords = float_image_new (ii_size_x, ii_size_y);

  // We transform the points using the array transformation function
  // for efficiency, but we don't want to do them all at once, since
  // that would require huge gobs of memory.
  const size_t max_transform_chunk_pixels = 5000000;
  size_t rows_per_chunk = max_transform_chunk_pixels / ii_size_x;
  size_t chunk_pixels = rows_per_chunk * ii_size_x;
  double *chunk_x = g_new0 (double, chunk_pixels);
  double *chunk_y = g_new0 (double, chunk_pixels);
  double *chunk_z = g_new0 (double, chunk_pixels);
  double *lat = g_new0 (double, chunk_pixels);
  double *lon = g_new0 (double, chunk_pixels);
  double *height = g_new0 (double, chunk_pixels);

  asfPrintStatus ("Determining Z coordinates of input pixels in output "
		  "projection space... ");

  // Transform all the chunks, storing results in the z coordinate image.
  size_t ii, jj, kk;		// Index variables.
  for ( ii = 0 ; ii < ii_size_y ; ) {
    size_t rows_remaining = ii_size_y - ii;
    size_t rows_to_load
      = rows_per_chunk < rows_remaining ? rows_per_chunk : rows_remaining;
    for ( jj = 0 ; jj < rows_to_load ; jj++ ) {
      size_t current_image_row = ii + jj;
      for ( kk = 0 ; kk < ii_size_x ; kk++ ) {
	size_t current_chunk_pixel = jj * ii_size_x + kk;
	chunk_x[current_chunk_pixel] = ipb->startX + kk * ipb->perX;
	chunk_y[current_chunk_pixel]
	  = ipb->startY + current_image_row * ipb->perY;
	if ( imd->projection->type == LAT_LONG_PSEUDO_PROJECTION ) {
	  chunk_x[current_chunk_pixel] *= D2R;
	  chunk_y[current_chunk_pixel] *= D2R;
	}
	chunk_z[current_chunk_pixel]
	  = float_image_get_pixel (iim, kk, current_image_row);
      }
    }
    long current_chunk_pixels = rows_to_load * ii_size_x;
    array_unproject_input (ipp, chunk_x, chunk_y, chunk_z, &lat, &lon,
			   &height, current_chunk_pixels, ipb->datum);
    array_project_output (pp, lat, lon, height, &chunk_x, &chunk_y, &chunk_z,
			  current_chunk_pixels, datum);
    for ( jj = 0 ; jj < rows_to_load ; jj++ ) {
      size_t current_image_row = ii + jj;
      for ( kk = 0 ; kk < ii_size_x ; kk++ ) {
	size_t current_chunk_pixel = jj * ii_size_x + kk;
	// Current pixel x, y, z coordinates.
	//float cp_x = (float) chunk_x[current_chunk_pixel];
	//float cp_y = (float) chunk_y[current_chunk_pixel];
	float cp_z = (float) chunk_z[current_chunk_pixel];
	//float_image_set_pixel (x_coords, kk, current_image_row, cp_x);
	//float_image_set_pixel (y_coords, kk, current_image_row, cp_y);
	float_image_set_pixel (z_coords, kk, current_image_row, cp_z);
      }
    }

    ii += rows_to_load;
  }

  asfPrintStatus ("done.\n\n");

#ifdef DEBUG_GEOCODE_DEM_Z_COORDS_IMAGE_AS_JPEG
  // Take a look at the z_coordinate image (for debugging).
  float_image_export_as_jpeg_with_mask_interval (z_coords, "z_coords.jpg",
						 GSL_MAX (z_coords->size_x,
							  z_coords->size_y),
						 -FLT_MAX, -100);
#endif

  g_free (chunk_x);
  g_free (chunk_y);
  g_free (chunk_z);
  g_free (lat);
  g_free (lon);
  g_free (height);

  // Now we want to determine the pixel coordinates in the input which
  // correspond to each of the output pixels.  We can then sample the
  // new height value already computed for that input pixel to
  // determine the pixel value to use as output.

  // We want to proceed in chunks as we did when going in the other
  // direction.
  rows_per_chunk = max_transform_chunk_pixels / oi_size_x;
  chunk_pixels = rows_per_chunk * oi_size_x;
  chunk_x = g_new0 (double, chunk_pixels);
  chunk_y = g_new0 (double, chunk_pixels);
  // We don't have height information in this direction, nor do we care.
  chunk_z = NULL;
  lat = g_new0 (double, chunk_pixels);
  lon = g_new0 (double, chunk_pixels);
  // We don't have height information in this direction, nor do we care.
  height = NULL;

  asfPrintStatus ("Sampling Z coordinates to form pixels in output projection "
		  "space... ");

  // Transform all the chunks, using the results to form the output image.
  for ( ii = 0 ; ii < oi_size_y ; ) {
    size_t rows_remaining = oi_size_y - ii;
    size_t rows_to_load
      = rows_per_chunk < rows_remaining ? rows_per_chunk : rows_remaining;
    for ( jj = 0 ; jj < rows_to_load ; jj++ ) {
      size_t current_image_row = ii + jj;
      for ( kk = 0 ; kk < oi_size_x ; kk++ ) {
	size_t current_chunk_pixel = jj * oi_size_x + kk;
	chunk_x[current_chunk_pixel] = min_x + kk * pixel_size;
	chunk_y[current_chunk_pixel] = max_y - current_image_row * pixel_size;
      }
    }
    long current_chunk_pixels = rows_to_load * oi_size_x;
    array_unproject_output (pp, chunk_x, chunk_y, NULL, &lat, &lon, NULL,
			    current_chunk_pixels, datum);
    array_project_input (ipp, lat, lon, NULL, &chunk_x, &chunk_y, NULL,
			 current_chunk_pixels, ipb->datum);
    if ( imd->projection->type == LAT_LONG_PSEUDO_PROJECTION ) {
      ssize_t ll;     // For (semi)clarity we don't reuse index variable :)
      for ( ll = 0 ; ll < current_chunk_pixels ; ll++ ) {
	chunk_x[ll] *= R2D;
	chunk_y[ll] *= R2D;
      }
    }

    for ( jj = 0 ; jj < rows_to_load ; jj++ ) {
      size_t current_image_row = ii + jj;
      for ( kk = 0 ; kk < oi_size_x ; kk++ ) {
	size_t current_chunk_pixel = jj * oi_size_x + kk;

	// Compute pixel coordinates in input image.
	ssize_t in_x
	  = (chunk_x[current_chunk_pixel] - ipb->startX) / ipb->perX;
	ssize_t in_y
	  = (chunk_y[current_chunk_pixel] - ipb->startY) / ipb->perY;

	if ( in_image (z_coords, in_x, in_y) ) {
	  // FIXME: something needs to be done somewhere about
	  // propogating no data values.
	  float_image_set_pixel (oim, kk, current_image_row,
				 float_image_sample (z_coords, in_x, in_y,
						     resample_method));
	}
	else {
	  float_image_set_pixel (oim, kk, current_image_row, background_value);
	}
      }
    }

    ii += rows_to_load;
  }

  asfPrintStatus ("done.\n\n");

  g_free (chunk_x);
  g_free (chunk_y);
  g_free (lat);
  g_free (lon);

#ifdef DEBUG_GEOCODE_DEM_OUTPUT_IMAGE_AS_JPEG
  // Take a look at the output image (for debugging).
  float_image_export_as_jpeg_with_mask_interval (oim, "oim.jpg",
						 GSL_MAX (oim->size_x,
							  oim->size_y),
						 -FLT_MAX, -100);
#endif

  // Store the output image.
  asfPrintStatus ("Storing output image... ");
  char *output_data_file =
    (char *) MALLOC(sizeof(char)*(strlen(output_image)+5));
  sprintf(output_data_file, "%s.img", output_image);
  return_code = float_image_store (oim, output_data_file,
				   FLOAT_IMAGE_BYTE_ORDER_BIG_ENDIAN);
  g_assert (return_code == 0);
  asfPrintStatus ("done.\n\n");

  // Now we need some metadata for the output image.  We will just
  // start with the metadata from the input image and add the
  // geocoding parameters.

  char *input_meta_file = (char *) MALLOC(sizeof(char)*(strlen(input_image)+6));
  sprintf(input_meta_file, "%s.meta", input_image);

  char *output_meta_file =
    (char *) MALLOC(sizeof(char)*(strlen(output_image)+6));
  sprintf(output_meta_file, "%s.meta", output_image);

  meta_parameters *omd = meta_read (input_meta_file);

  // Adjust the metadata to correspond to the output image instead of
  // the input image.

  omd->general->x_pixel_size = pixel_size;
  omd->general->y_pixel_size = pixel_size;
  omd->general->line_count = oi_size_y;
  omd->general->sample_count = oi_size_x;

  // SAR block is not really appropriate for map projected images, but
  // since it ended up with this value that can signify map
  // projectedness in it somehow, we fill it in for safety.
  omd->sar->image_type = 'P';

  // Note that we have already verified that the input image is
  // projected, and since we initialize the output metadata from there
  // we know we will have a projection block.
  omd->projection->type = projection_type;
  omd->projection->startX = min_x;
  omd->projection->startY = max_y;
  omd->projection->perX = pixel_size;
  omd->projection->perY = -pixel_size;
  strcpy (omd->projection->units, "meters");

  // Set the spheroid axes lengths as appropriate for the output datum.
  spheroid_axes_lengths (datum_spheroid (datum), &(omd->projection->re_major),
			 &(omd->projection->re_minor));

  // What the heck, might as well set the ones in the general block as
  // well.
  spheroid_axes_lengths (datum_spheroid (datum), &(omd->general->re_major),
			 &(omd->general->re_minor));

  // Latitude and longitude at center of the output image.  We will
  // set these relative to the spheroid underlying the datum in use
  // for the projected image.  Yeah, that seems appropriate.
  double lat_0, lon_0;
  double center_x = omd->projection->startX + (omd->projection->perX
					       * omd->general->line_count / 2);
  double center_y = (omd->projection->startY
		     + (omd->projection->perY
			* omd->general->sample_count / 2));
  unproject_output (pp, center_x, center_y, ASF_PROJ_NO_HEIGHT, &lat_0, &lon_0,
		    NULL, datum);
  omd->general->center_latitude = R2D * lat_0;
  omd->general->center_longitude = R2D * lon_0;

  // FIXME: We are ignoring the meta_location fields for now since I'm
  // not sure whether they are supposed to refer to the corner pixels
  // or the corners of the data itself.

  if ( lat_0 > 0.0 ) {
    omd->projection->hem = 'N';
  }
  else {
    omd->projection->hem = 'S';
  }

  // Convert the projection parameter values back into degrees.
  to_degrees (projection_type, pp);
  omd->projection->param = *pp;
  meta_write (omd, output_meta_file);

  float_image_free (oim);
  FREE(output_data_file);
  meta_free (omd);
  FREE(input_meta_file);
  FREE(output_meta_file);

  return 0;
}


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* This file was automatically generated --- DO NOT EDIT */
/* Generated on Sun Nov  7 20:43:56 EST 1999 */

#include <fftw-int.h>
#include <fftw.h>

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -real2hc 13 */

/*
 * This function contains 76 FP additions, 34 FP multiplications,
 * (or, 57 additions, 15 multiplications, 19 fused multiply/add),
 * 34 stack variables, and 26 memory accesses
 */
static const fftw_real K083333333 = FFTW_KONST(+0.083333333333333333333333333333333333333333333);
static const fftw_real K075902986 = FFTW_KONST(+0.075902986037193865983102897245103540356428373);
static const fftw_real K251768516 = FFTW_KONST(+0.251768516431883313623436926934233488546674281);
static const fftw_real K503537032 = FFTW_KONST(+0.503537032863766627246873853868466977093348562);
static const fftw_real K113854479 = FFTW_KONST(+0.113854479055790798974654345867655310534642560);
static const fftw_real K265966249 = FFTW_KONST(+0.265966249214837287587521063842185948798330267);
static const fftw_real K387390585 = FFTW_KONST(+0.387390585467617292130675966426762851778775217);
static const fftw_real K300462606 = FFTW_KONST(+0.300462606288665774426601772289207995520941381);
static const fftw_real K258260390 = FFTW_KONST(+0.258260390311744861420450644284508567852516811);
static const fftw_real K132983124 = FFTW_KONST(+0.132983124607418643793760531921092974399165133);
static const fftw_real K2_000000000 = FFTW_KONST(+2.000000000000000000000000000000000000000000000);
static const fftw_real K1_732050807 = FFTW_KONST(+1.732050807568877293527446341505872366942805254);
static const fftw_real K156891391 = FFTW_KONST(+0.156891391051584611046832726756003269660212636);
static const fftw_real K256247671 = FFTW_KONST(+0.256247671582936600958684654061725059144125175);
static const fftw_real K011599105 = FFTW_KONST(+0.011599105605768290721655456654083252189827041);
static const fftw_real K300238635 = FFTW_KONST(+0.300238635966332641462884626667381504676006424);
static const fftw_real K174138601 = FFTW_KONST(+0.174138601152135905005660794929264742616964676);
static const fftw_real K575140729 = FFTW_KONST(+0.575140729474003121368385547455453388461001608);
static const fftw_real K866025403 = FFTW_KONST(+0.866025403784438646763723170752936183471402627);
static const fftw_real K500000000 = FFTW_KONST(+0.500000000000000000000000000000000000000000000);

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.41 1999/05/26 15:44:14 fftw Exp $
 * $Id: fft.ml,v 1.43 1999/05/17 19:44:18 fftw Exp $
 * $Id: to_c.ml,v 1.25 1999/10/26 21:41:32 stevenj Exp $
 */

void fftw_real2hc_13(const fftw_real *input, fftw_real *real_output, fftw_real *imag_output, int istride, int real_ostride, int imag_ostride)
{
     fftw_real tmp65;
     fftw_real tmp11;
     fftw_real tmp28;
     fftw_real tmp37;
     fftw_real tmp51;
     fftw_real tmp62;
     fftw_real tmp22;
     fftw_real tmp58;
     fftw_real tmp59;
     fftw_real tmp66;
     fftw_real tmp56;
     fftw_real tmp63;
     fftw_real tmp35;
     fftw_real tmp38;
     ASSERT_ALIGNED_DOUBLE;
     tmp65 = input[0];
     {
	  fftw_real tmp3;
	  fftw_real tmp53;
	  fftw_real tmp21;
	  fftw_real tmp30;
	  fftw_real tmp26;
	  fftw_real tmp16;
	  fftw_real tmp29;
	  fftw_real tmp25;
	  fftw_real tmp6;
	  fftw_real tmp32;
	  fftw_real tmp9;
	  fftw_real tmp33;
	  fftw_real tmp10;
	  fftw_real tmp54;
	  fftw_real tmp1;
	  fftw_real tmp2;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp1 = input[8 * istride];
	  tmp2 = input[5 * istride];
	  tmp3 = tmp1 - tmp2;
	  tmp53 = tmp1 + tmp2;
	  {
	       fftw_real tmp17;
	       fftw_real tmp18;
	       fftw_real tmp19;
	       fftw_real tmp20;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp17 = input[12 * istride];
	       tmp18 = input[4 * istride];
	       tmp19 = input[10 * istride];
	       tmp20 = tmp18 + tmp19;
	       tmp21 = tmp17 + tmp20;
	       tmp30 = tmp17 - (K500000000 * tmp20);
	       tmp26 = tmp18 - tmp19;
	  }
	  {
	       fftw_real tmp12;
	       fftw_real tmp13;
	       fftw_real tmp14;
	       fftw_real tmp15;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp12 = input[istride];
	       tmp13 = input[3 * istride];
	       tmp14 = input[9 * istride];
	       tmp15 = tmp13 + tmp14;
	       tmp16 = tmp12 + tmp15;
	       tmp29 = tmp12 - (K500000000 * tmp15);
	       tmp25 = tmp13 - tmp14;
	  }
	  {
	       fftw_real tmp4;
	       fftw_real tmp5;
	       fftw_real tmp7;
	       fftw_real tmp8;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp4 = input[6 * istride];
	       tmp5 = input[11 * istride];
	       tmp6 = tmp4 - tmp5;
	       tmp32 = tmp4 + tmp5;
	       tmp7 = input[2 * istride];
	       tmp8 = input[7 * istride];
	       tmp9 = tmp7 - tmp8;
	       tmp33 = tmp7 + tmp8;
	  }
	  tmp10 = tmp6 + tmp9;
	  tmp54 = tmp32 + tmp33;
	  tmp11 = tmp3 - tmp10;
	  {
	       fftw_real tmp24;
	       fftw_real tmp27;
	       fftw_real tmp49;
	       fftw_real tmp50;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp24 = tmp3 + (K500000000 * tmp10);
	       tmp27 = K866025403 * (tmp25 + tmp26);
	       tmp28 = tmp24 - tmp27;
	       tmp37 = tmp27 + tmp24;
	       tmp49 = tmp9 - tmp6;
	       tmp50 = tmp25 - tmp26;
	       tmp51 = tmp49 - tmp50;
	       tmp62 = tmp50 + tmp49;
	  }
	  tmp22 = tmp16 - tmp21;
	  tmp58 = tmp16 + tmp21;
	  tmp59 = tmp53 + tmp54;
	  tmp66 = tmp58 + tmp59;
	  {
	       fftw_real tmp52;
	       fftw_real tmp55;
	       fftw_real tmp31;
	       fftw_real tmp34;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp52 = tmp29 + tmp30;
	       tmp55 = tmp53 - (K500000000 * tmp54);
	       tmp56 = tmp52 - tmp55;
	       tmp63 = tmp52 + tmp55;
	       tmp31 = tmp29 - tmp30;
	       tmp34 = K866025403 * (tmp32 - tmp33);
	       tmp35 = tmp31 + tmp34;
	       tmp38 = tmp31 - tmp34;
	  }
     }
     real_output[0] = tmp65 + tmp66;
     {
	  fftw_real tmp23;
	  fftw_real tmp45;
	  fftw_real tmp40;
	  fftw_real tmp48;
	  fftw_real tmp44;
	  fftw_real tmp46;
	  fftw_real tmp41;
	  fftw_real tmp47;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp23 = (K575140729 * tmp11) - (K174138601 * tmp22);
	  tmp45 = (K575140729 * tmp22) + (K174138601 * tmp11);
	  {
	       fftw_real tmp36;
	       fftw_real tmp39;
	       fftw_real tmp42;
	       fftw_real tmp43;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp36 = (K300238635 * tmp28) + (K011599105 * tmp35);
	       tmp39 = (K256247671 * tmp37) + (K156891391 * tmp38);
	       tmp40 = tmp36 - tmp39;
	       tmp48 = K1_732050807 * (tmp39 + tmp36);
	       tmp42 = (K300238635 * tmp35) - (K011599105 * tmp28);
	       tmp43 = (K156891391 * tmp37) - (K256247671 * tmp38);
	       tmp44 = K1_732050807 * (tmp42 - tmp43);
	       tmp46 = tmp43 + tmp42;
	  }
	  imag_output[imag_ostride] = tmp23 + (K2_000000000 * tmp40);
	  tmp41 = tmp23 - tmp40;
	  imag_output[3 * imag_ostride] = tmp41 - tmp44;
	  imag_output[4 * imag_ostride] = -(tmp41 + tmp44);
	  imag_output[5 * imag_ostride] = -(tmp45 + (K2_000000000 * tmp46));
	  tmp47 = tmp46 - tmp45;
	  imag_output[2 * imag_ostride] = tmp47 - tmp48;
	  imag_output[6 * imag_ostride] = tmp48 + tmp47;
     }
     {
	  fftw_real tmp61;
	  fftw_real tmp70;
	  fftw_real tmp74;
	  fftw_real tmp76;
	  fftw_real tmp68;
	  fftw_real tmp69;
	  fftw_real tmp57;
	  fftw_real tmp60;
	  fftw_real tmp71;
	  fftw_real tmp75;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp57 = (K132983124 * tmp51) + (K258260390 * tmp56);
	  tmp60 = K300462606 * (tmp58 - tmp59);
	  tmp61 = (K2_000000000 * tmp57) + tmp60;
	  tmp70 = tmp60 - tmp57;
	  {
	       fftw_real tmp72;
	       fftw_real tmp73;
	       fftw_real tmp64;
	       fftw_real tmp67;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp72 = (K387390585 * tmp51) - (K265966249 * tmp56);
	       tmp73 = (K113854479 * tmp62) - (K503537032 * tmp63);
	       tmp74 = tmp72 + tmp73;
	       tmp76 = tmp73 - tmp72;
	       tmp64 = (K251768516 * tmp62) + (K075902986 * tmp63);
	       tmp67 = tmp65 - (K083333333 * tmp66);
	       tmp68 = (K2_000000000 * tmp64) + tmp67;
	       tmp69 = tmp67 - tmp64;
	  }
	  real_output[real_ostride] = tmp61 + tmp68;
	  real_output[5 * real_ostride] = tmp68 - tmp61;
	  tmp71 = tmp69 - tmp70;
	  real_output[2 * real_ostride] = tmp71 - tmp74;
	  real_output[6 * real_ostride] = tmp74 + tmp71;
	  tmp75 = tmp70 + tmp69;
	  real_output[3 * real_ostride] = tmp75 - tmp76;
	  real_output[4 * real_ostride] = tmp76 + tmp75;
     }
}

fftw_codelet_desc fftw_real2hc_13_desc =
{
     "fftw_real2hc_13",
     (void (*)()) fftw_real2hc_13,
     13,
     FFTW_FORWARD,
     FFTW_REAL2HC,
     288,
     0,
     (const int *) 0,
};


/* Copyright (C) 2021 Atsushi Togo */
/* All rights reserved. */

/* This file is part of phonopy. */

/* Redistribution and use in source and binary forms, with or without */
/* modification, are permitted provided that the following conditions */
/* are met: */

/* * Redistributions of source code must retain the above copyright */
/*   notice, this list of conditions and the following disclaimer. */

/* * Redistributions in binary form must reproduce the above copyright */
/*   notice, this list of conditions and the following disclaimer in */
/*   the documentation and/or other materials provided with the */
/*   distribution. */

/* * Neither the name of the phonopy project nor the names of its */
/*   contributors may be used to endorse or promote products derived */
/*   from this software without specific prior written permission. */

/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */
/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */
/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS */
/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE */
/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, */
/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, */
/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; */
/* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER */
/* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT */
/* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN */
/* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE */
/* POSSIBILITY OF SUCH DAMAGE. */

#ifndef __phono3py_H__
#define __phono3py_H__

#ifdef MKL_LAPACKE
#include <mkl.h>
#else
#include <lapacke.h>
#endif
#include "phonoc_array.h"

long ph3py_get_interaction(Darray *fc3_normal_squared,
                           const char *g_zero,
                           const Darray *frequencies,
                           const lapack_complex_double *eigenvectors,
                           const long (*triplets)[3],
                           const long num_triplets,
                           const long (*bz_grid_addresses)[3],
                           const long D_diag[3],
                           const long Q[3][3],
                           const double *fc3,
                           const long is_compact_fc3,
                           const double (*svecs)[3],
                           const long multi_dims[2],
                           const long (*multi)[2],
                           const double *masses,
                           const long *p2s_map,
                           const long *s2p_map,
                           const long *band_indices,
                           const long symmetrize_fc3_q,
                           const double cutoff_frequency);
long ph3py_get_pp_collision(double *imag_self_energy,
                            const long relative_grid_address[24][4][3], /* thm */
                            const double *frequencies,
                            const lapack_complex_double *eigenvectors,
                            const long (*triplets)[3],
                            const long num_triplets,
                            const long *triplet_weights,
                            const long (*bz_grid_addresses)[3], /* thm */
                            const long *bz_map,                 /* thm */
                            const long bz_grid_type,
                            const long D_diag[3],
                            const long Q[3][3],
                            const double *fc3,
                            const long is_compact_fc3,
                            const double (*svecs)[3],
                            const long multi_dims[2],
                            const long (*multi)[2],
                            const double *masses,
                            const long *p2s_map,
                            const long *s2p_map,
                            const Larray *band_indices,
                            const Darray *temperatures,
                            const long is_NU,
                            const long symmetrize_fc3_q,
                            const double cutoff_frequency);
long ph3py_get_pp_collision_with_sigma(
    double *imag_self_energy,
    const double sigma,
    const double sigma_cutoff,
    const double *frequencies,
    const lapack_complex_double *eigenvectors,
    const long (*triplets)[3],
    const long num_triplets,
    const long *triplet_weights,
    const long (*bz_grid_addresses)[3],
    const long D_diag[3],
    const long Q[3][3],
    const double *fc3,
    const long is_compact_fc3,
    const double (*svecs)[3],
    const long multi_dims[2],
    const long (*multi)[2],
    const double *masses,
    const long *p2s_map,
    const long *s2p_map,
    const Larray *band_indices,
    const Darray *temperatures,
    const long is_NU,
    const long symmetrize_fc3_q,
    const double cutoff_frequency);
void ph3py_get_imag_self_energy_at_bands_with_g(
    double *imag_self_energy,
    const Darray *fc3_normal_squared,
    const double *frequencies,
    const long (*triplets)[3],
    const long *triplet_weights,
    const double *g,
    const char *g_zero,
    const double temperature,
    const double cutoff_frequency,
    const long num_frequency_points,
    const long frequency_point_index);
void ph3py_get_detailed_imag_self_energy_at_bands_with_g(
    double *detailed_imag_self_energy,
    double *imag_self_energy_N,
    double *imag_self_energy_U,
    const Darray *fc3_normal_squared,
    const double *frequencies,
    const long (*triplets)[3],
    const long *triplet_weights,
    const long (*bz_grid_addresses)[3],
    const double *g,
    const char *g_zero,
    const double temperature,
    const double cutoff_frequency);
void ph3py_get_real_self_energy_at_bands(double *real_self_energy,
                                         const Darray *fc3_normal_squared,
                                         const long *band_indices,
                                         const double *frequencies,
                                         const long (*triplets)[3],
                                         const long *triplet_weights,
                                         const double epsilon,
                                         const double temperature,
                                         const double unit_conversion_factor,
                                         const double cutoff_frequency);
void ph3py_get_real_self_energy_at_frequency_point(
    double *real_self_energy,
    const double frequency_point,
    const Darray *fc3_normal_squared,
    const long *band_indices,
    const double *frequencies,
    const long (*triplets)[3],
    const long *triplet_weights,
    const double epsilon,
    const double temperature,
    const double unit_conversion_factor,
    const double cutoff_frequency);
void ph3py_get_collision_matrix(double *collision_matrix,
                                const Darray *fc3_normal_squared,
                                const double *frequencies,
                                const long (*triplets)[3],
                                const long *triplets_map,
                                const long *map_q,
                                const long *rotated_grid_points,
                                const double *rotations_cartesian,
                                const double *g,
                                const long num_ir_gp,
                                const long num_gp,
                                const long num_rot,
                                const double temperature,
                                const double unit_conversion_factor,
                                const double cutoff_frequency);
void ph3py_get_reducible_collision_matrix(double *collision_matrix,
                                          const Darray *fc3_normal_squared,
                                          const double *frequencies,
                                          const long (*triplets)[3],
                                          const long *triplets_map,
                                          const long *map_q,
                                          const double *g,
                                          const long num_gp,
                                          const double temperature,
                                          const double unit_conversion_factor,
                                          const double cutoff_frequency);
void ph3py_get_isotope_scattering_strength(
    double *gamma,
    const long grid_point,
    const double *mass_variances,
    const double *frequencies,
    const lapack_complex_double *eigenvectors,
    const long num_grid_points,
    const long *band_indices,
    const long num_band,
    const long num_band0,
    const double sigma,
    const double cutoff_frequency);
void ph3py_get_thm_isotope_scattering_strength(
    double *gamma,
    const long grid_point,
    const long *ir_grid_points,
    const long *weights,
    const double *mass_variances,
    const double *frequencies,
    const lapack_complex_double *eigenvectors,
    const long num_ir_grid_points,
    const long *band_indices,
    const long num_band,
    const long num_band0,
    const double *integration_weights,
    const double cutoff_frequency);
void ph3py_distribute_fc3(double *fc3,
                          const long target,
                          const long source,
                          const long *atom_mapping,
                          const long num_atom,
                          const double *rot_cart);
void ph3py_rotate_delta_fc2(double (*fc3)[3][3][3],
                            const double (*delta_fc2s)[3][3],
                            const double *inv_U,
                            const double (*site_sym_cart)[3][3],
                            const long *rot_map_syms,
                            const long num_atom,
                            const long num_site_sym,
                            const long num_disp);
void ph3py_get_permutation_symmetry_fc3(double *fc3, const long num_atom);
void ph3py_get_permutation_symmetry_compact_fc3(double *fc3,
                                                const long p2s[],
                                                const long s2pp[],
                                                const long nsym_list[],
                                                const long perms[],
                                                const long n_satom,
                                                const long n_patom);
void ph3py_transpose_compact_fc3(double *fc3,
                                 const long p2s[],
                                 const long s2pp[],
                                 const long nsym_list[],
                                 const long perms[],
                                 const long n_satom,
                                 const long n_patom,
                                 const long t_type);
long ph3py_get_triplets_reciprocal_mesh_at_q(long *map_triplets,
                                             long *map_q,
                                             const long grid_point,
                                             const long mesh[3],
                                             const long is_time_reversal,
                                             const long num_rot,
                                             const long (*rec_rotations)[3][3],
                                             const long swappable);
long ph3py_get_BZ_triplets_at_q(long (*triplets)[3],
                                const long grid_point,
                                const long (*bz_grid_addresses)[3],
                                const long *bz_map,
                                const long *map_triplets,
                                const long num_map_triplets,
                                const long D_diag[3],
                                const long Q[3][3],
                                const long bz_grid_type);
long ph3py_get_integration_weight(double *iw,
                                  char *iw_zero,
                                  const double *frequency_points,
                                  const long num_band0,
                                  const long relative_grid_address[24][4][3],
                                  const long mesh[3],
                                  const long (*triplets)[3],
                                  const long num_triplets,
                                  const long (*bz_grid_addresses)[3],
                                  const long *bz_map,
                                  const long bz_grid_type,
                                  const double *frequencies1,
                                  const long num_band1,
                                  const double *frequencies2,
                                  const long num_band2,
                                  const long tp_type,
                                  const long openmp_per_triplets,
                                  const long openmp_per_bands);
void ph3py_get_integration_weight_with_sigma(double *iw,
                                             char *iw_zero,
                                             const double sigma,
                                             const double sigma_cutoff,
                                             const double *frequency_points,
                                             const long num_band0,
                                             const long (*triplets)[3],
                                             const long num_triplets,
                                             const double *frequencies,
                                             const long num_band,
                                             const long tp_type);
long ph3py_get_grid_index_from_address(const long address[3],
                                       const long mesh[3]);
void ph3py_get_gr_grid_addresses(long gr_grid_addresses[][3],
                                 const long D_diag[3]);
long ph3py_get_reciprocal_rotations(long rec_rotations[48][3][3],
                                    const long (*rotations)[3][3],
                                    const long num_rot,
                                    const long is_time_reversal);
long ph3py_transform_rotations(long (*transformed_rots)[3][3],
                               const long (*rotations)[3][3],
                               const long num_rot,
                               const long D_diag[3],
                               const long Q[3][3]);
long ph3py_get_snf3x3(long D_diag[3],
                      long P[3][3],
                      long Q[3][3],
                      const long A[3][3]);
long ph3py_transform_rotations(long (*transformed_rots)[3][3],
                               const long (*rotations)[3][3],
                               const long num_rot,
                               const long D_diag[3],
                               const long Q[3][3]);
long ph3py_get_ir_grid_map(long *ir_grid_map,
                           const long D_diag[3],
                           const long PS[3],
                           const long (*grg_rotations)[3][3],
                           const long num_rot);
long ph3py_get_bz_grid_addresses(long (*bz_grid_addresses)[3],
                                 long *bz_map,
                                 long *bzg2grg,
                                 const long D_diag[3],
                                 const long Q[3][3],
                                 const long PS[3],
                                 const double rec_lattice[3][3],
                                 const long type);
long ph3py_rotate_bz_grid_index(const long bz_grid_index,
                                const long rotation[3][3],
                                const long (*bz_grid_addresses)[3],
                                const long *bz_map,
                                const long D_diag[3],
                                const long PS[3],
                                const long bz_grid_type);
void ph3py_symmetrize_collision_matrix(double *collision_matrix,
                                       const long num_column,
                                       const long num_temp,
                                       const long num_sigma);
void ph3py_expand_collision_matrix(double *collision_matrix,
                                   const long *rot_grid_points,
                                   const long *ir_grid_points,
                                   const long num_ir_gp,
                                   const long num_grid_points,
                                   const long num_rot,
                                   const long num_sigma,
                                   const long num_temp,
                                   const long num_band);
long ph3py_get_neighboring_gird_points(long *relative_grid_points,
                                       const long *grid_points,
                                       const long (*relative_grid_address)[3],
                                       const long mesh[3],
                                       const long (*bz_grid_addresses)[3],
                                       const long *bz_map,
                                       const long bz_grid_type,
                                       const long num_grid_points,
                                       const long num_relative_grid_address);
long ph3py_get_thm_integration_weights_at_grid_points(
    double *iw,
    const double *frequency_points,
    const long num_band0,
    const long num_band,
    const long num_gp,
    const long (*relative_grid_address)[4][3],
    const long D_diag[3],
    const long *grid_points,
    const long (*bz_grid_addresses)[3],
    const long *bz_map,
    const long bz_grid_type,
    const double *frequencies,
    const long *gp2irgp_map,
    const char function);

#endif


/*****************************************************************\
           __
          / /
		 / /                     __  __
		/ /______    _______    / / / / ________   __       __
	   / ______  \  /_____  \  / / / / / _____  | / /      / /
	  / /      | / _______| / / / / / / /____/ / / /      / /
	 / /      / / / _____  / / / / / / _______/ / /      / /
	/ /      / / / /____/ / / / / / / |______  / |______/ /
   /_/      /_/ |________/ / / / /  \_______/  \_______  /
                          /_/ /_/                     / /
			                                         / /
		       High Level Game Framework            /_/

  ---------------------------------------------------------------

  Copyright (c) 2007-2011 - Rodrigo Braz Monteiro.
  This file is subject to the terms of halley_license.txt.

\*****************************************************************/

#pragma once

#include <iostream>
#include <iomanip>
#include <sstream>
#include <cmath>
#include "halley/text/halleystring.h"
#include <gsl/gsl_assert>
#include <cstdint>

namespace Halley {
	// This whole class is TERRIBLE
	// There should be a type distinction between linear and gamma space, with conversions,
	// and the default Colour type should be Colour4c

	template <typename T>
	constexpr T colMinValue()
	{
		return 0;
	}

	template <typename T>
	constexpr T colMaxValue()
	{
		return 255;
	}

	template <>
	constexpr float colMaxValue<float>()
	{
		return 1.0f;
	}

	template <typename T, typename U>
	constexpr U convertColour(T x)
	{
		return U(float(x) * colMaxValue<U>() / colMaxValue<T>());
	}

	template <>
	constexpr uint8_t convertColour(int x)
	{
		return static_cast<uint8_t>(x);
	}

	template <typename T>
	class Colour4 {
	public:
		T r = colMinValue<T>();
		T g = colMinValue<T>();
		T b = colMinValue<T>();
		T a = colMaxValue<T>();

		constexpr Colour4() = default;

		constexpr Colour4(T luma)
			: r(luma)
			, g(luma)
			, b(luma)
		{}

		constexpr Colour4(T r, T g, T b, T a=colMaxValue<T>())
			: r(r)
			, g(g)
			, b(b)
			, a(a)
		{}

		Colour4(const String& str)
		{
			*this = fromString(str);
		}

		template <typename U>
		Colour4(const Colour4<U> c)
		{
			r = convertColour<U, T>(c.r);
			g = convertColour<U, T>(c.g);
			b = convertColour<U, T>(c.b);
			a = convertColour<U, T>(c.a);
		}

		String toString() const
		{
			std::stringstream ss;
			ss << "#" << std::hex;
			writeByte(ss, r);
			writeByte(ss, g);
			writeByte(ss, b);
			if (byteRep(a) != 255) writeByte(ss, a);
			ss.flush();
			return ss.str();
		}

		static Colour4 fromString(String str)
		{
			Colour4 col;
			size_t len = str.length();
			if (len >= 1 && str[0] == '#') {
				if (len >= 3) {
					col.r = parseHex(str.substr(1, 2));
				}
				if (len >= 5) {
					col.g = parseHex(str.substr(3, 2));
				}
				if (len >= 7) {
					col.b = parseHex(str.substr(5, 2));
				}
				if (len >= 9) {
					col.a = parseHex(str.substr(7, 2));
				}
			}
			return col;
		}

		static Colour4 fromHSV(float h, float s, float v)
		{
			float r = 0;
			float g = 0;
			float b = 0;
			if (s == 0) {
				r = v * 255;
				if (r < 0) r = 0;
				if (r > 255) r = 255;
				g = b = r;
			} else {
				h = float(std::fmod(h, 1.0f));
				int hi = int(h * 6.0f);
				float f = h*6 - hi;
				float p = v*(1-s);
				float q = v*(1-f*s);
				float t = v*(1-(1-f)*s);

				switch (hi) {
				case 0:	r = v; g = t; b = p; break;
				case 1:	r = q; g = v; b = p; break;
				case 2:	r = p; g = v; b = t; break;
				case 3:	r = p; g = q; b = v; break;
				case 4:	r = t; g = p; b = v; break;
				case 5:	r = v; g = p; b = q; break;
				//default: throw Exception("Invalid value!");
				}
			}

			return Colour4<T>(Colour4<float>(r, g, b));
		}

		constexpr Colour4 multiplyLuma(float t) const
		{
			return Colour4(r*t, g*t, b*t, a);
		}

		constexpr Colour4 inverseMultiplyLuma(float t) const
		{
			return Colour4(1.0f - ((1.0f - r) * t), 1.0f - ((1.0f - g) * t), 1.0f - ((1.0f - b) * t), a);
		}

		constexpr Colour4 multiplyAlpha(float t) const
		{
			return Colour4(r, g, b, a * t);
		}

		constexpr Colour4 operator+(const Colour4& c) const
		{
			return Colour4(r+c.r, g+c.g, b+c.b, a+c.a);
		}

		constexpr Colour4 operator*(const Colour4& c) const
		{
			return Colour4(r * c.r, g * c.g, b * c.b, a * c.a);
		}

		constexpr Colour4 operator*(float t) const
		{
			return Colour4(r*t, g*t, b*t, a*t);
		}

		constexpr bool operator==(const Colour4& c) const
		{
			return r == c.r && g == c.g && b == c.b && a == c.a;
		}

		constexpr bool operator!=(const Colour4& c) const
		{
			return r != c.r || g != c.g || b != c.b || a != c.a;
		}

	private:
		uint8_t byteRep(T v) const
		{
			return convertColour<T, uint8_t>(v);
		}

		static T parseHex(String str)
		{
			std::stringstream ss(str);
			int value;
			ss << std::hex;
			ss >> value;
			return convertColour<uint8_t, T>(static_cast<uint8_t>(value));
		}

		template <typename U>
		void writeByte(U& os, T component) const
		{
			unsigned int value = byteRep(component);
			if (value < 16) os << '0';
			os << value;
		}
	};

	typedef Colour4<uint8_t> Colour4c;
	typedef Colour4<float> Colour4f;
	typedef Colour4f Colour;
}


/*
 * This file is part of the CN24 semantic segmentation software,
 * copyright (C) 2015 Clemens-Alexander Brust (ikosa dot de at gmail dot com).
 *
 * For licensing information, see the LICENSE file included with this project.
 */
/**
 * @file MKLHelper.h
 * @brief Contains macros to help with various BLAS implementations.
 *
 * @author Clemens-Alexander Brust (ikosa dot de at gmail dot com)
 */

#ifndef CONV_MKLHELPER_H
#define CONV_MKLHELPER_H
#ifdef BUILD_BLAS

/**
 * Here we include our favorite BLAS library and define important functions
 * so that they are independent of the type
 */

#ifdef BLAS_ACCELERATE
#include <Accelerate/Accelerate.h>
#define INNERGEMM cblas_sgemm
#define INNERGEMV cblas_sgemv

#endif

#ifdef BLAS_ATLAS
extern "C" {
#include <cblas.h>
}

#define INNERGEMM cblas_sgemm
#define INNERGEMV cblas_sgemv

#endif

#ifdef BLAS_ACML
extern "C" {
#include <cblas.h>
}

#define INNERGEMM cblas_sgemm
#define INNERGEMV cblas_sgemv

#endif

#ifdef BLAS_MKL

extern "C" {
#include <mkl_cblas.h>
}

#define INNERGEMM cblas_sgemm
#define INNERGEMV cblas_sgemv

#endif

#else

#include "Log.h"

#define INNERGEMM(...) { FATAL("GEMM function called without BLAS support!"); }
#define INNERGEMV(...) { FATAL("GEMV function called without BLAS support!"); }

#endif

#endif