#!/usr/bin/env python

# Experiments with coefficients of a geometric plane

# Resources:
# http://www.wolframalpha.com/input/?i=plane+through+(1,-2,0),(4,-2,-2),(4,1,4)&lk=3
#    ==> 2 x - 6 y + 3 z + 14 == 0


# Translate a point relative to some origin
from __future__ import print_function


def translate(point, origin):
    return tuple([a - b for a, b in zip(point, origin)])


# Given two points in 3d space, define a vector
def vector(p1, p2):
    return tuple([b - a for a, b in zip(p1, p2)])


# Given two vectors in a plane, find the normal vector
def normal(u, v):
    # A normal vector is the cross-product of two coplanar vectors
    return tuple(
        [
            u[1] * v[2] - u[2] * v[1],
            u[2] * v[0] - u[0] * v[2],
            u[0] * v[1] - u[1] * v[0],
        ]
    )


def plane_from_three_points(P, Q, R):
    u = vector(P, Q)
    v = vector(P, R)
    n = normal(u, v)

    # Find the coefficients
    (A, B, C) = n

    # The equation of the plane is thus Ax+By+Cz+K=0.
    # Solve for K to get the final coefficient
    (x, y, z) = P
    K = -(A * x + B * y + C * z)

    return (A, B, C, K)


# find the Z offset for any x,y
# z = -(Ax + By + K) / C
def calcz(x, y, plane, translation=(0, 0, 0)):
    (A, B, C, K) = plane
    (tx, ty, tz) = translation
    return -(A * (x - tx) + B * (y - ty) + K) / C + tz


# Verify a point is on this plane
def validate(plane, point):
    (A, B, C, K) = plane
    (x, y, z) = point
    return z == calcz(x, y, plane)


def verify_plane(points):
    print("  ", "\n   ".join([str(p) for p in points]))

    plane = plane_from_three_points(*points)
    print("Plane coordinates: ", plane)

    if plane[2] == 0:
        print("   Error: points are colinear")
        return

    valid = True
    for p in points:
        if not validate(plane, p):
            print("Failed: sample point not on plane, ", p)
            valid = False
    print("Validation:", "Failed" if not valid else "Passed")


samples = [
    # canonical example
    [(1, -2, 0), (4, -2, -2), (4, 1, 4)],
    # three colinear points (infinite planes)
    [(2, 2, 2), (4, 4, 4), (10, 10, 10)],
    # Extreme tilt example in mm
    [(57, 123, -5), (200, 0, 35), (0, 207, 2)],
    # Some more examples in um
    [(0, 0, 1300), (200000, 200000, 3500), (0, 150000, -1000)],
    [(20000, 20000, -300), (220000, 120000, -1700), (120000, 220000, -700)],
    # some example in tenths of mm
    [(200, 200, -300), (2200, 1200, -1700), (1200, 2200, -700)],
    [(20000, 20000, -300), (220000, 120000, -1700), (120000, 220000, -700)],
    [(200, 200, -300), (2200, 1200, -1700), (1200, 2200, -700)],
]

for points in samples:
    verify_plane(points)

    print("====[Translated]=========")
    # Translate plane to origin at P (simplifies by removing K coefficient)
    # A*x' + B*y' + C*z' = 0
    P = points[0]
    T = translate((0, 0, 0), P)
    xpoints = [translate(p, P) for p in points]
    verify_plane(xpoints)
    print("=========================\n")