/*
 * mexFunction(): entry point for the mex function
 */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  // interface to deal with input arguments from Matlab
  enum InputIndexType {IN_TRI, IN_X, IN_RES, IN_SIZE, IN_ORIGIN, InputIndexType_MAX};
  MatlabImportFilter::Pointer matlabImport = MatlabImportFilter::New();
  matlabImport->ConnectToMatlabFunctionInput(nrhs, prhs);

  // check the number of input arguments
  matlabImport->CheckNumberOfArguments(2, InputIndexType_MAX);

  // register the inputs for this function at the import filter
  typedef MatlabImportFilter::MatlabInputPointer MatlabInputPointer;
  MatlabInputPointer inTRI = matlabImport->RegisterInput(IN_TRI, "TRI");
  MatlabInputPointer inX = matlabImport->RegisterInput(IN_X, "X"); // (x, y, z)
  MatlabInputPointer inRES = matlabImport->RegisterInput(IN_RES, "RES"); // (r, c, s)
  MatlabInputPointer inSIZE = matlabImport->RegisterInput(IN_SIZE, "SIZE"); // (r, c, s)
  MatlabInputPointer inORIGIN = matlabImport->RegisterInput(IN_ORIGIN, "ORIGIN"); // (x, y, z)

  // interface to deal with outputs to Matlab
  enum OutputIndexType {OUT_IM, OutputIndexType_MAX};
  MatlabExportFilter::Pointer matlabExport = MatlabExportFilter::New();
  matlabExport->ConnectToMatlabFunctionOutput(nlhs, plhs);
  
  // check that the number of outputs the user is asking for is valid
  matlabExport->CheckNumberOfArguments(0, OutputIndexType_MAX);

  // register the outputs for this function at the export filter
  typedef MatlabExportFilter::MatlabOutputPointer MatlabOutputPointer;
  MatlabOutputPointer outIM = matlabExport->RegisterOutput(OUT_IM, "IM");

  // if any input point set is empty, the outputs are empty too
  if (mxIsEmpty(inTRI->pm) || mxIsEmpty(inX->pm)) {
    matlabExport->CopyEmptyArrayToMatlab(outIM);
    return;
  }

  // get number of rows in inputs X and TRI
  mwSize nrowsX = mxGetM(inX->pm);
  mwSize nrowsTRI = mxGetM(inTRI->pm);

  // instantiate mesh
  MeshType::Pointer mesh = MeshType::New();

  // read vertices
  PointSetType::Pointer xDef = PointSetType::New(); // default: empty point set
  PointSetType::Pointer x = PointSetType::New();
  x->GetPoints()->CastToSTLContainer()
    = matlabImport->ReadVectorOfVectorsFromMatlab<PointType::CoordRepType, PointType>
    (inX, xDef->GetPoints()->CastToSTLContainer());

#ifdef DEBUG
  std::cout << "Number of X points read = " << x->GetNumberOfPoints() << std::endl;
#endif

  // assertion check
  if (nrowsX != x->GetNumberOfPoints()) {
    mexErrMsgTxt(("Input " + inX->name 
		  + ": Number of points read different from number of points provided by user").c_str()); 
  }

  // swap XY coordinates to make them compliant with ITK convention
  // (see important programming note at the help header above)
  matlabImport->SwapXYInVectorOfVectors<PointType::CoordRepType, std::vector<PointType> >
    (x->GetPoints()->CastToSTLContainer(), x->GetNumberOfPoints());

  // populate mesh with vertices
  mesh->SetPoints(x->GetPoints());

  // read triangles
  PointType triDef;
  triDef.Fill(mxGetNaN());
  for (mwIndex i = 0; i < nrowsTRI; ++i) {

    PointType triangle = matlabImport->ReadRowVectorFromMatlab<CoordType, PointType>(inTRI, i, triDef);

    // create a triangle cell to read the vertex indices of the current input triangle
    CellAutoPointer cell;
    cell.TakeOwnership(new TriangleType);

    // assign to the 0, 1, 2 elements in the triangle cell the vertex
    // indices that we have just read. Note that we have to substract
    // 1 to convert Matlab's index convention 1, 2, 3, ... to C++
    // convention 0, 1, 2, ...
    cell->SetPointId(0, triangle[0] - 1);
    cell->SetPointId(1, triangle[1] - 1);
    cell->SetPointId(2, triangle[2] - 1);

    // insert cell into the mesh
    mesh->SetCell(i, cell);
  }

#ifdef DEBUG
  std::cout << "Number of triangles read = " << mesh->GetNumberOfCells() << std::endl;
#endif

  // assertion check
  if (nrowsTRI != mesh->GetNumberOfCells()) {
    mexErrMsgTxt(("Input " + inTRI->name 
		  + ": Number of triangles read different from number of triangles provided by user").c_str()); 
  }

  // get user input parameters for the output rasterization
  ImageType::SpacingType spacingDef;
  spacingDef.Fill(1.0);
  ImageType::SpacingType spacing = matlabImport->
    ReadRowVectorFromMatlab<ImageType::SpacingValueType, ImageType::SpacingType>(inRES, spacingDef);

  ImageType::SizeType sizeDef;
  sizeDef.Fill(10);
  ImageType::SizeType size = matlabImport->
    ReadRowVectorFromMatlab<ImageType::SizeValueType, ImageType::SizeType>(inSIZE, sizeDef);

  ImageType::PointType originDef;
  originDef.Fill(0.0);
  ImageType::PointType origin = matlabImport->
    ReadRowVectorFromMatlab<ImageType::PointType::ValueType, ImageType::PointType>(inORIGIN, originDef);
  // (see important programming note at the help header above)
  matlabImport->SwapXYInVector<ImageType::PointType::ValueType, ImageType::PointType>(origin);

  // instantiate rasterization filter
  MeshFilterType::Pointer meshFilter = MeshFilterType::New();

  // smallest voxel side length
  ImageType::SpacingValueType minSpacing = spacing[0];
  for (mwIndex i = 1; i < Dimension; ++i) {
    minSpacing = std::min(minSpacing, spacing[i]);
  }

  // pass input parameters to the filter
  meshFilter->SetInput(mesh);
  meshFilter->SetSpacing(spacing);
  meshFilter->SetSize(size);
  meshFilter->SetOrigin(origin);
  meshFilter->SetTolerance(minSpacing / 10.0);
  meshFilter->SetInsideValue(1);
  meshFilter->SetOutsideValue(0);

  ImageType::IndexType start;
  start.Fill(0);
  meshFilter->SetIndex(start);

  // convert image size from itk::Size format to std::vector<mwSize>
  // so that we can use it in GraftItkImageOntoMatlab
  std::vector<mwSize> sizeStdVector(Dimension);
  for (unsigned int i = 0; i < Dimension; ++i) {
    sizeStdVector[i] = size[i];
  }

  // graft ITK filter output onto Matlab output
  matlabExport->GraftItkImageOntoMatlab<PixelType, Dimension>
    (outIM, meshFilter->GetOutput(), sizeStdVector);

#ifdef DEBUG
  std::cout << "Resolution (spacing) = " << meshFilter->GetSpacing() << std::endl;
  std::cout << "Size = " << meshFilter->GetSize() << std::endl;
  std::cout << "Origin = " << meshFilter->GetOrigin() << std::endl;
#endif
  
  // run rasterization
  meshFilter->Update();

}
int main(int, char ** argv)
{
	OptionsData options;
	readXMLValues(argv[2], options);
	
	exit(1);


	// parse the dicom directory
	gdcm::Directory dir;
	dir.Load(argv[1], true);
	gdcm::Directory::FilenamesType filenames = dir.GetFilenames();

	
	gdcm::Tag seriesDescription = gdcm::Tag(0x0008,0x103e);
	gdcm::Tag seriesNumber = gdcm::Tag(0x0020,0x0011);
	gdcm::Tag instanceNumber = gdcm::Tag(0x0020,0x0013);
	gdcm::Tag sliceThickness = gdcm::Tag(0x0018,0x0050);
	gdcm::Tag triggerTime = gdcm::Tag(0x0018,0x1060);
	gdcm::Tag numberOfImages = gdcm::Tag(0x0018,0x1090);
	gdcm::Tag slicePosition = gdcm::Tag(0x0019,0x1015);
	gdcm::Tag imagePosition = gdcm::Tag(0x0020,0x0032);
	gdcm::Tag imageOrientation = gdcm::Tag(0x0020,0x0037);
	gdcm::Tag sliceLocation = gdcm::Tag(0x0020,0x1041);
	gdcm::Tag rows = gdcm::Tag(0x0028,0x0010);
	gdcm::Tag cols = gdcm::Tag(0x0028,0x0011);
	gdcm::Tag pixelSpacing = gdcm::Tag(0x0028,0x0030);

	gdcm::Scanner scanner;
	scanner.AddTag(seriesDescription);
	scanner.AddTag(seriesNumber);
	scanner.AddTag(instanceNumber);
	scanner.AddTag(sliceThickness);
	scanner.AddTag(triggerTime);
	scanner.AddTag(numberOfImages);
	scanner.AddTag(slicePosition);
	scanner.AddTag(imagePosition);
	scanner.AddTag(imageOrientation);
	scanner.AddTag(sliceLocation);
	scanner.AddTag(rows);
	scanner.AddTag(cols);
	scanner.AddTag(pixelSpacing);

	scanner.Scan(filenames);
	gdcm::Scanner::MappingType mapping = scanner.GetMappings();


	// extract all the images that are Short axis and are the first instance
	gdcm::Directory::FilenamesType targetFilenames;
	for(unsigned int i = 0; i < filenames.size(); i++)
	{
		const char * fname = filenames[i].c_str();
		if(mapping.count(fname) == 0) continue;


		// extract the image information
		std::string descriptionStr = mapping[fname][seriesDescription];
		std::string instanceNumberStr = mapping[fname][instanceNumber];
		QString description = QString::fromStdString(descriptionStr);
		unsigned int instanceNumber = QString::fromStdString(instanceNumberStr).toInt();

		// check that the description is a short axis one and is instance 1
		if(instanceNumber == 1 && description.contains("sa", Qt::CaseInsensitive))
		{
			targetFilenames.push_back(filenames[i]);
		}
	}


	// sort the images based on their slice position
	/*
	gdcm::Sorter sorter;
	sorter.SetSortFunction(position_sort);
	sorter.StableSort(targetFilenames);

	gdcm::Directory::FilenamesType sorted = sorter.GetFilenames();
	for(unsigned int i = 0; i < sorted.size(); i++)
	{
		const char * fname = sorted[i].c_str();
		std::string position = mapping[fname][imagePosition];
		std::cout << position << std::endl;
		std::cout << mapping[fname][sliceLocation] << std::endl;
	}
	*/

	std::cout << targetFilenames.size() << std::endl;


	// find the slice with the smallest slice position
	double smallest = 1000000.0;
	int smallestIndex;
	for(unsigned int i = 0; i < targetFilenames.size(); i++)
	{
		const char * fname = targetFilenames[i].c_str();
		std::string slicePosition = mapping[fname][sliceLocation];
		double pos = QString::fromStdString(slicePosition).toDouble();

		std::cout << pos << std::endl;
		if(pos < smallest)
		{	
			smallest = pos;
			smallestIndex = i;
		}
	}

	// load the image
	typedef itk::Image<unsigned short, 3> ImageType;
	typedef itk::ImageFileReader<ImageType> ReaderType;
	ReaderType::Pointer reader = ReaderType::New();
	reader->SetFileName(targetFilenames[smallestIndex]);
	reader->SetImageIO(itk::GDCMImageIO::New());
	reader->Update();

	
	// flip the x and y axis
	typedef itk::PermuteAxesImageFilter<ImageType> FlipperType;
	FlipperType::Pointer flipper = FlipperType::New();
	itk::FixedArray<unsigned int, 3> order;
	order[0] = 1;
	order[1] = 0;
	order[2] = 2;
	flipper->SetOrder(order);
	flipper->SetInput(reader->GetOutput());
	flipper->Update();


	ImageType::Pointer referenceImage = flipper->GetOutput();
	ImageType::DirectionType direction = referenceImage->GetDirection();
	direction.SetIdentity();
	referenceImage->SetDirection(direction);
	
	ImageType::PointType origin = referenceImage->GetOrigin();
	origin.Fill(20.0);
	referenceImage->SetOrigin(origin);

	ImageType::SpacingType spacing;
	spacing.Fill(1.0);

	referenceImage->SetSpacing(spacing);


	
	flipper->GetOutput()->Print(std::cout);

	typedef itk::ImageFileWriter<ImageType> WriterType;
	WriterType::Pointer writer = WriterType::New();
	writer->SetInput(referenceImage);
	writer->SetImageIO(itk::NrrdImageIO::New());
	writer->SetFileName("test.nrrd");
	writer->Update();

	std::cout << targetFilenames[smallestIndex] << std::endl;

	return 0;
}
// ------------------------------------------------------------------------
void NormaliseImage(const ImageType::Pointer &input, 
		const ImageType::Pointer &reference,	
		ImageType::Pointer &output,
		SeriesTransform &trans)
{
	// flip the x and y axis 
	typedef itk::PermuteAxesImageFilter<ImageType> FlipperType;
	FlipperType::Pointer flipper = FlipperType::New();
	itk::FixedArray<unsigned int, 3> order;
	order[0] = 1;
	order[1] = 0;
	order[2] = 2;

	flipper->SetOrder(order);
	flipper->SetInput(input);
	flipper->Update();

	output = flipper->GetOutput();

	// get the reference offset
	vnl_vector<double> refOrigin(3);
	refOrigin[0] = reference->GetOrigin()[0];// + trans.translation[0];
	refOrigin[1] = reference->GetOrigin()[1];// + trans.translation[1];
	refOrigin[2] = reference->GetOrigin()[2];// + trans.translation[2];

	vnl_matrix<double> refRot = reference->GetDirection().GetVnlMatrix();
	vnl_matrix<double> refRotInv = vnl_inverse(refRot);
	vnl_vector<double> refOffset = refRotInv * refOrigin;

	// get the image origin
	vnl_vector<double> origin(3);
	origin[0] = output->GetOrigin()[0];
	origin[1] = output->GetOrigin()[1];
	origin[2] = output->GetOrigin()[2];
	
	// apply the rotation to the origin
	origin = refRotInv * origin;

	// apply the offset to the origin
	origin = origin - refOffset;

	// apply the scaling 
	origin /= reference->GetSpacing()[0];

	// put the values into the output image
	ImageType::PointType itkOrigin;
	itkOrigin[0] = origin[0];
	itkOrigin[1] = origin[1];
	itkOrigin[2] = origin[2];

	ImageType::SpacingType spacing;
	spacing.Fill(output->GetSpacing()[0] / reference->GetSpacing()[0]);

	// get the new direction
	ImageType::DirectionType dirOut = output->GetDirection();
	dirOut = reference->GetDirection().GetInverse() * dirOut.GetVnlMatrix();
	
	output->SetSpacing(spacing);
	output->SetDirection(dirOut);
	output->SetOrigin(itkOrigin);
}