#include "point_tris_dist.h"

#include <vtkPoints.h>
#include <vtkTriangle.h>
#include <vtkMath.h>

double pointSegmentDistance(double *p, double *a, double *b, double* direction) {
    double segSqrLength = vtkMath::Distance2BetweenPoints(a, b);

    double ap[3]; vtkMath::Subtract(p, a, ap);
    double ab[3]; vtkMath::Subtract(b, a, ab);

	if(segSqrLength <= 0) {
        direction[0] = ap[0];
        direction[1] = ap[1];
        direction[2] = ap[2];
        return vtkMath::Normalize(direction);
    }

    double h = vtkMath::ClampValue(vtkMath::Dot(ap, ab) / segSqrLength, 0., 1.);

    vtkMath::MultiplyScalar(ab, h);
    double v[3]; vtkMath::Subtract(ap, ab, v);

    direction[0] = v[0];
    direction[1] = v[1];
    direction[2] = v[2];
    return vtkMath::Normalize(direction);
}

double pointTriangleDistance(double *p, vtkCell *triangle, double *direction) {
    double a[3]; triangle->GetPoints()->GetPoint(0, a);
    double b[3]; triangle->GetPoints()->GetPoint(1, b);
    double c[3]; triangle->GetPoints()->GetPoint(2, c);

    double ab[3]; vtkMath::Subtract(b, a, ab);
    double bc[3]; vtkMath::Subtract(c, b, bc);
    double ca[3]; vtkMath::Subtract(a, c, ca);

    double n[3]; vtkTriangle::ComputeNormal(a, b, c, n);
    double vecTA[3]; vtkMath::Cross(ab, n, vecTA);
    double vecTB[3]; vtkMath::Cross(bc, n, vecTB);
    double vecTC[3]; vtkMath::Cross(ca, n, vecTC);

    double ap[3]; vtkMath::Subtract(p, a, ap);
    double bp[3]; vtkMath::Subtract(p, b, bp);
    double cp[3]; vtkMath::Subtract(p, c, cp);

    double da = vtkMath::Dot(vecTA, ap);
	double db = vtkMath::Dot(vecTB, bp);
	double dc = vtkMath::Dot(vecTC, cp);

	if(da <= 0 && db <= 0 && dc <= 0) {
        double d = vtkMath::Dot(n, ap);
        double n2 = vtkMath::Dot(n, n);

        if(d >= 0) {
            direction[0] = n[0];
            direction[1] = n[1];
            direction[2] = n[2];
        } else {
            direction[0] = -n[0];
            direction[1] = -n[1];
            direction[2] = -n[2];
        }
        
        return sqrt(d * d / n2);
	} else {
        double normalA[3];
        double normalB[3];
        double normalC[3];

		double lab = pointSegmentDistance(p, a, b, normalA);
		double lbc = pointSegmentDistance(p, b, c, normalB);
		double lca = pointSegmentDistance(p, c, a, normalC);

		double min = vtkMath::Min(lab, vtkMath::Min(lbc, lca));

        if(min == lab) {
            direction[0] = normalA[0];
            direction[1] = normalA[1];
            direction[2] = normalA[2];
        } else if(min == lbc) {
            direction[0] = normalB[0];
            direction[1] = normalB[1];
            direction[2] = normalB[2];
        } else {
            direction[0] = normalC[0];
            direction[1] = normalC[1];
            direction[2] = normalC[2];
        }

        return min;
	}
}
