#include "angles_filter.h"

#include <vtkUnstructuredGrid.h>
#include <vtkPointData.h>
#include <vtkCellData.h>
#include <vtkDoubleArray.h>
#include <vtkCellIterator.h>


vtkStandardNewMacro(AnglesFilter);


vtkTypeBool AnglesFilter::RequestData(
	vtkInformation *request,
	vtkInformationVector **inputVector,
	vtkInformationVector *outputVector) {
	(void) request;
	vtkUnstructuredGrid* input =
		vtkUnstructuredGrid::GetData(inputVector[0]);
	vtkUnstructuredGrid* output =
		vtkUnstructuredGrid::GetData(outputVector);
	output->CopyStructure(input);
	output->GetPointData()->PassData(input->GetPointData());
	vtkCellData *cellData = output->GetCellData();
	cellData->PassData(input->GetCellData());
	vtkNew<vtkDoubleArray> anglesArray;
	anglesArray->SetName("angles");
	anglesArray->SetNumberOfComponents(12);
	anglesArray->SetNumberOfTuples(input->GetNumberOfCells());
	double *anglesBase = anglesArray->GetPointer(0);
	size_t i = 0;
	auto it = input->NewCellIterator();
	for (it->InitTraversal(); !it->IsDoneWithTraversal(); it->GoToNextCell()) {
		if (it->GetCellType() != VTK_TETRA) continue;
		cellAngles(input, it->GetPointIds(), anglesBase + i);
		i += 12;
	}
	cellData->AddArray((vtkAbstractArray *) anglesArray);
	return true;
}


void cellAngles(vtkDataSet *dataSet, vtkIdList *idList, double *angles) {
	// std::cout << "nb points: " << idList->GetNumberOfIds() << std::endl;
	double a[3], b[3], c[3], d[3];
	dataSet->GetPoint(idList->GetId(0), a);
	dataSet->GetPoint(idList->GetId(1), b);
	dataSet->GetPoint(idList->GetId(2), c);
	dataSet->GetPoint(idList->GetId(3), d);
	// std::cout << "ids " << idList->GetId(0)
	// 		  << " " << idList->GetId(1)
	// 		  << " " << idList->GetId(2)
	// 		  << " " << idList->GetId(3) << std::endl;
	// std::cout << "coords" << std::endl
	// 		  << a[0] << ", " << a[1] << ", " << a[2] << std::endl
	// 		  << b[0] << ", " << b[1] << ", " << b[2] << std::endl
	// 		  << c[0] << ", " << c[1] << ", " << c[2] << std::endl
	// 		  << d[0] << ", " << d[1] << ", " << d[2] << std::endl;
	double ab[3], ac[3], ad[3],
		ba[3], bc[3], bd[3],
		ca[3], cb[3], cd[3],
		da[3], db[3], dc[3];
	vtkMath::Subtract(b, a, ab);
	vtkMath::Subtract(c, a, ac);
	vtkMath::Subtract(d, a, ad);
	vtkMath::Subtract(a, b, ba);
	vtkMath::Subtract(c, b, bc);
	vtkMath::Subtract(d, b, bd);
	vtkMath::Subtract(a, c, ca);
	vtkMath::Subtract(b, c, cb);
	vtkMath::Subtract(d, c, cd);
	vtkMath::Subtract(a, d, da);
	vtkMath::Subtract(b, d, db);
	vtkMath::Subtract(c, d, dc);
	angles[0] = vtkMath::AngleBetweenVectors(ab, ac);
	angles[1] = vtkMath::AngleBetweenVectors(ca, cb);
	angles[2] = vtkMath::AngleBetweenVectors(ba, bc);
	angles[3] = vtkMath::AngleBetweenVectors(ac, ad);
	angles[4] = vtkMath::AngleBetweenVectors(dc, da);
	angles[5] = vtkMath::AngleBetweenVectors(cd, ca);
	angles[6] = vtkMath::AngleBetweenVectors(ad, ab);
	angles[7] = vtkMath::AngleBetweenVectors(bd, ba);
	angles[8] = vtkMath::AngleBetweenVectors(da, db);
	angles[9] = vtkMath::AngleBetweenVectors(bc, bd);
	angles[10] = vtkMath::AngleBetweenVectors(cb, cd);
	angles[11] = vtkMath::AngleBetweenVectors(db, dc);
}