#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];
        vtkMath::Normalize(direction);
        return vtkMath::Norm(ap, 3);
    }
	
    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];
    vtkMath::Normalize(direction);
	return vtkMath::Norm(v);
}

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 na[3] = {
            n[0] * a[0],
            n[1] * a[1],
            n[2] * a[2],
        };

        double np[3] = {
            n[0] * p[0],
            n[1] * p[1],
            n[2] * p[2],
        };

        double t[3]; vtkMath::Subtract(np, na, t);

        double nt[3] = {
            n[0] * t[0],
            n[1] * t[1],
            n[2] * t[2],
        };


        direction[0] = nt[0];
        direction[1] = nt[1];
        direction[2] = nt[2];
        vtkMath::Normalize(direction);
        return vtkMath::Norm(nt);

	} 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;
	}
}