// This is Free Software under GNU Affero General Public License v >= 3.0
// without warranty, see README.md and license for details.
//
// SPDX-License-Identifier: AGPL-3.0-or-later
// License-Filename: LICENSES/AGPL-3.0.txt
//
// Copyright (C) 2018 by via donau
//   – Österreichische Wasserstraßen-Gesellschaft mbH
// Software engineering by Intevation GmbH
//
// Author(s):
//  * Sascha L. Teichmann <sascha.teichmann@intevation.de>

package mesh

import (
	"bytes"
	"encoding/binary"
	"fmt"
	"io"
	"math"
	"sort"

	"gemma.intevation.de/gemma/pkg/log"
	"gemma.intevation.de/gemma/pkg/wkb"
)

type (
	// Point is a 2D point.
	Point struct {
		X float64
		Y float64
	}

	// Vertex is a 3D vertex.
	Vertex struct {
		X float64
		Y float64
		Z float64
	}

	// Triangle is a triangle consisting of three vertices.
	Triangle [3]Vertex

	// Line is a line defined by first vertex on that line
	// and the second being the direction.
	Line [2]Vertex

	// Box is a 3D box.
	Box [2]Vertex

	// MultiPointZ is a set of vertices.
	MultiPointZ []Vertex

	// LineStringZ is a line string formed of vertices.
	LineStringZ []Vertex

	// MultiLineStringZ is a set of line strings.
	MultiLineStringZ []LineStringZ

	// Box2D is 2D area from (X1, Y1) to (X2, Y2).
	Box2D struct {
		X1 float64
		Y1 float64
		X2 float64
		Y2 float64
	}

	// Plane2D is a 2D plane (a line in 2D space).
	Plane2D struct {
		A float64
		B float64
		C float64
	}

	// Plane3D is a 3D plane.
	Plane3D struct {
		A float64
		B float64
		C float64
		D float64
	}
)

// Plane3D returns the plane in which
// the three points of the triangles are in.
func (t *Triangle) Plane3D() Plane3D {

	v0 := t[1].Sub(t[0])
	v1 := t[2].Sub(t[0])
	n := v0.Cross(v1)

	// If the length of normal is near zero assume we have
	// a plane constant in z with an average z value of
	// the three vertices.
	// This should protect us from the effects of collinear
	// geometries.
	l := n.Length()
	if l < 1e-7 {
		sum := t[0].Z + t[1].Z + t[2].Z
		return Plane3D{
			A: 0,
			B: 0,
			C: 1,
			D: -sum / 3,
		}
	}
	n = n.Scale(1 / l)

	// x*nx+ y*ny+ z*nz + d = 0
	// d = - (x*nx+ y*ny + z*nz)
	d := -t[0].Dot(n)
	return Plane3D{
		A: n.X,
		B: n.Y,
		C: n.Z,
		D: d,
	}
}

// BBox calculates the 2D (in X/Y plane) bounding box
// of the triangle.
func (t *Triangle) BBox() Box2D {
	minX := math.Min(math.Min(t[0].X, t[1].X), t[2].X)
	maxX := math.Max(math.Max(t[0].X, t[1].X), t[2].X)
	minY := math.Min(math.Min(t[0].Y, t[1].Y), t[2].Y)
	maxY := math.Max(math.Max(t[0].Y, t[1].Y), t[2].Y)
	return Box2D{
		X1: minX, Y1: minY,
		X2: maxX, Y2: maxY,
	}
}

// Inside test if b is completely inside a.
func (a Box2D) Inside(b Box2D) bool {
	return a.X1 >= b.X1 && a.X2 <= b.X2 &&
		a.Y1 >= b.Y1 && a.Y2 <= b.Y2
}

// Size calculates the area of the box.
func (a Box2D) Size() (float64, float64) {
	return a.X2 - a.X1, a.Y2 - a.Y1
}

// Empty returns true if the box has no area.
func (a Box2D) Empty() bool {
	const eps = 0.0000001
	return math.Abs(a.X2-a.X1) < eps &&
		math.Abs(a.Y2-a.Y1) < eps
}

// Z calculates the Z value for a given X/Y value.
func (p Plane3D) Z(x, y float64) float64 {
	// p.A*x + p.B*y + p.C*z + p.D = 0
	return -(p.A*x + p.B*y + p.D) / p.C
}

// Eval evalutes the plane equation for a given vertex.
func (p Plane3D) Eval(v Vertex) float64 {
	return p.A*v.X + p.B*v.Y + p.C*v.Z + p.D
}

// Normalize constructs a new vertex with unit length for a given vertex.
func (v Vertex) Normalize() Vertex {
	s := 1 / v.Length()
	return Vertex{
		X: s * v.X,
		Y: s * v.Y,
		Z: s * v.Z,
	}
}

// Dot returns the 3D dot product of the two given vertices.
func (v Vertex) Dot(w Vertex) float64 {
	return v.X*w.X + v.Y*w.Y + v.Z*w.Z
}

// Length return the length of the vertex.
func (v Vertex) Length() float64 {
	return math.Sqrt(v.Dot(v))
}

// Box2D constructs a Box2D of this vertex.
func (v Vertex) Box2D() Box2D {
	return Box2D{
		X1: v.X,
		Y1: v.Y,
		X2: v.X,
		Y2: v.Y,
	}
}

func area(a, b, c Vertex) float64 {
	return (b.Y-a.Y)*(c.X-b.X) - (b.X-a.X)*(c.Y-b.Y)
}

func inCircle(a, b, c, p Vertex) bool {
	dx := a.X - p.X
	dy := a.Y - p.Y
	ex := b.X - p.X
	ey := b.Y - p.Y
	fx := c.X - p.X
	fy := c.Y - p.Y

	ap := dx*dx + dy*dy
	bp := ex*ex + ey*ey
	cp := fx*fx + fy*fy

	return dx*(ey*cp-bp*fy)-dy*(ex*cp-bp*fx)+ap*(ex*fy-ey*fx) < 0
}

func circumradius(a, b, c Vertex) float64 {
	dx := b.X - a.X
	dy := b.Y - a.Y
	ex := c.X - a.X
	ey := c.Y - a.Y

	bl := dx*dx + dy*dy
	cl := ex*ex + ey*ey
	d := dx*ey - dy*ex

	x := (ey*bl - dy*cl) * 0.5 / d
	y := (dx*cl - ex*bl) * 0.5 / d

	r := x*x + y*y

	if bl == 0 || cl == 0 || d == 0 || r == 0 {
		return infinity
	}

	return r
}

func circumcenter(a, b, c Vertex) Vertex {
	dx := b.X - a.X
	dy := b.Y - a.Y
	ex := c.X - a.X
	ey := c.Y - a.Y

	bl := dx*dx + dy*dy
	cl := ex*ex + ey*ey
	d := dx*ey - dy*ex

	x := a.X + (ey*bl-dy*cl)*0.5/d
	y := a.Y + (dx*cl-ex*bl)*0.5/d

	return Vertex{X: x, Y: y}
}

func polygonArea(points []Vertex) float64 {
	var result float64
	for i, p := range points {
		q := points[(i+1)%len(points)]
		result += (p.X - q.X) * (p.Y + q.Y)
	}
	return result / 2
}

// Distance2D returns the distance of the two vertices in the X/Y plane.
func (v Vertex) Distance2D(w Vertex) float64 {
	return math.Hypot(v.X-w.X, v.Y-w.Y)
}

// Distance returns the distance of the two vertices.
func (v Vertex) Distance(w Vertex) float64 {
	v = v.Sub(w)
	return math.Sqrt(v.Dot(v))
}

// Minimize adjust this vertex v to hold the minimum
// values component-wise of v and w.
func (v *Vertex) Minimize(w Vertex) {
	if w.X < v.X {
		v.X = w.X
	}
	if w.Y < v.Y {
		v.Y = w.Y
	}
	if w.Z < v.Z {
		v.Z = w.Z
	}
}

// Maximize adjust this vertex v to hold the maximum
// values component-wise of v and w.
func (v *Vertex) Maximize(w Vertex) {
	if w.X > v.X {
		v.X = w.X
	}
	if w.Y > v.Y {
		v.Y = w.Y
	}
	if w.Z > v.Z {
		v.Z = w.Z
	}
}

// SquaredDistance2D returns the squared distances of the
// two given vertices in the X/Y plane.
func (v Vertex) SquaredDistance2D(w Vertex) float64 {
	dx := v.X - w.X
	dy := v.Y - w.Y
	return dx*dx + dy*dy
}

// Sub2D returns (v - w) component-wise.
func (v Vertex) Sub2D(w Vertex) Vertex {
	return Vertex{
		X: v.X - w.X,
		Y: v.Y - w.Y,
	}
}

// Sub returns (v - w) component-wise.
func (v Vertex) Sub(w Vertex) Vertex {
	return Vertex{
		v.X - w.X,
		v.Y - w.Y,
		v.Z - w.Z,
	}
}

// Add returns (v + w) component-wise.
func (v Vertex) Add(w Vertex) Vertex {
	return Vertex{
		v.X + w.X,
		v.Y + w.Y,
		v.Z + w.Z,
	}
}

// Scale returns s*v component-wise.
func (v Vertex) Scale(s float64) Vertex {
	return Vertex{
		s * v.X,
		s * v.Y,
		s * v.Z,
	}
}

// Interpolate returns a function that return s*b[1] + b[0]
// component-wise.
func (b Box) Interpolate() func(Vertex) Vertex {
	v1, v2 := b[0], b[1]
	v2 = v2.Sub(v1)
	return func(s Vertex) Vertex {
		return Vertex{
			v2.X*s.X + v1.X,
			v2.Y*s.Y + v1.Y,
			v2.Z*s.Z + v1.Z,
		}
	}
}

// Less returns if one of v component is less than the
// corresponing component in w.
func (v Vertex) Less(w Vertex) bool {
	return v.X < w.X || v.Y < w.Y || v.Z < w.Z
}

// NewLine return a line of point/direction.
func NewLine(p1, p2 Vertex) Line {
	return Line{
		p2.Sub(p1),
		p1,
	}
}

// Eval returns the vertex for t*l[0] + l[1].
func (l Line) Eval(t float64) Vertex {
	return l[0].Scale(t).Add(l[1])
}

// IntersectHorizontal returns the intersection point
// for a given z value.
func (l Line) IntersectHorizontal(h float64) Vertex {
	t := (h - l[1].Z) / l[0].Z
	return l.Eval(t)
}

func side(z, h float64) int {
	switch {
	case z < h:
		return -1
	case z > h:
		return +1
	}
	return 0
}

// Dot2 calculates the 2D dot product of the vertices.
func (v Vertex) Dot2(w Vertex) float64 {
	return v.X*w.X + v.Y*w.Y
}

// Contains returns true if the given point is inside the triangle.
func (t *Triangle) Contains(x, y float64) bool {
	v0 := t[2].Sub2D(t[0])
	v1 := t[1].Sub2D(t[0])
	v2 := Vertex{X: x, Y: y}.Sub2D(t[0])

	dot00 := v0.Dot2(v0)
	dot01 := v0.Dot2(v1)
	dot02 := v0.Dot2(v2)
	dot11 := v1.Dot2(v1)
	dot12 := v1.Dot2(v2)

	// Compute barycentric coordinates
	invDenom := 1 / (dot00*dot11 - dot01*dot01)
	u := (dot11*dot02 - dot01*dot12) * invDenom
	v := (dot00*dot12 - dot01*dot02) * invDenom

	// Check if point is in triangle
	return u >= 0 && v >= 0 && u+v < 1
}

// IntersectHorizontal calculates the line string that
// results when cutting a triangle a a certain height.
// Can be empty (on intersection),
// one vertex (only touching an vertex) or
// two vertices (real intersection).
func (t *Triangle) IntersectHorizontal(h float64) LineStringZ {
	sides := [3]int{
		side(t[0].Z, h),
		side(t[1].Z, h),
		side(t[2].Z, h),
	}

	var points LineStringZ

	for i := 0; i < 3; i++ {
		j := (i + 1) % 3
		si := sides[i]
		sj := sides[j]

		switch {
		case si == 0 && sj == 0:
			// both on plane
			points = append(points, t[i], t[j])
		case si == 0 && sj != 0:
			// first on plane
			points = append(points, t[i])
		case si != 0 && sj == 0:
			// second on plane
			points = append(points, t[j])
		case si == sj:
			// both on same side
		default:
			// real intersection
			v := NewLine(t[i], t[j]).IntersectHorizontal(h)
			points = append(points, v)
		}
	}

	return points
}

func linearScale(x1, y1, x2, y2 float64) func(Vertex) float64 {
	dx := x2 - x1
	dy := y2 - y1

	switch {
	case dx != 0:
		return func(v Vertex) float64 {
			return (v.X - x1) / dx
		}
	case dy != 0:
		return func(v Vertex) float64 {
			return (v.Y - y1) / dy
		}
	default:
		return func(Vertex) float64 {
			return 0
		}
	}
}

// BBox calcultes the 2D bounding box in the X/Y plane
// of the given line string.
func (ls LineStringZ) BBox() Box2D {

	min := Vertex{math.MaxFloat64, math.MaxFloat64, math.MaxFloat64}
	max := Vertex{-math.MaxFloat64, -math.MaxFloat64, -math.MaxFloat64}

	for _, v := range ls {
		min.Minimize(v)
		max.Maximize(v)
	}

	return Box2D{
		X1: min.X,
		Y1: min.Y,
		X2: max.X,
		Y2: max.Y,
	}
}

// Area calculated the area of the line string.
func (ls LineStringZ) Area() float64 {
	return polygonArea(ls)
}

// Reverse reverses the the vertices of this line string in place.
func (ls LineStringZ) Reverse() {
	for i, j := 0, len(ls)-1; i < j; i, j = i+1, j-1 {
		ls[i], ls[j] = ls[j], ls[i]
	}
}

func (ls LineStringZ) order(position func(Vertex) float64) {
	type posVertex struct {
		pos float64
		v   Vertex
	}
	positions := make([]posVertex, len(ls))
	for i, v := range ls {
		positions[i] = posVertex{position(v), v}
	}
	sort.Slice(positions, func(i, j int) bool {
		return positions[i].pos < positions[j].pos
	})
	for i := range positions {
		ls[i] = positions[i].v
	}
}

// EpsEquals returns true if v and w are equal component-wise
// with the values within a epsilon range.
func (v Vertex) EpsEquals(w Vertex) bool {
	const eps = 1e-5
	return math.Abs(v.X-w.X) < eps &&
		math.Abs(v.Y-w.Y) < eps && math.Abs(v.Z-w.Z) < eps
}

// EpsEquals2D returns true if v and w are equal component-wise
// in the X/Y plane with the values within a epsilon range.
func (v Vertex) EpsEquals2D(w Vertex) bool {
	const eps = 1e-5
	return math.Abs(v.X-w.X) < eps &&
		math.Abs(v.Y-w.Y) < eps
}

// JoinOnLine joins the the elements of a given multi line string
// under the assumption that the segments are all on the line segment
// from (x1, y1) to (x2, y2).
func (mls MultiLineStringZ) JoinOnLine(x1, y1, x2, y2 float64) MultiLineStringZ {

	position := linearScale(x1, y1, x2, y2)

	type posLineString struct {
		pos  float64
		line LineStringZ
	}

	positions := make([]posLineString, 0, len(mls))

	for _, ls := range mls {
		if len(ls) == 0 {
			continue
		}
		// order the atoms first
		ls.order(position)
		positions = append(positions, posLineString{position(ls[0]), ls})
	}

	sort.Slice(positions, func(i, j int) bool {
		return positions[i].pos < positions[j].pos
	})

	out := make(MultiLineStringZ, 0, len(positions))

	var ignored int

	for i := range positions {
		curr := positions[i].line
		if l := len(out); l > 0 {
			last := out[l-1]

			if last[len(last)-1].EpsEquals(curr[0]) {
				out[l-1] = append(last[:len(last)-1], curr...)
				continue
			}
			if position(last[len(last)-1]) > position(curr[0]) {
				ignored++
				continue
			}
		}
		out = append(out, curr)
	}

	log.Infof("ignored parts: %d\n", ignored)

	return out
}

// Write writes a Vertex as three 64 bit values in little endian order
// to the given writer.
func (v *Vertex) Write(w io.Writer) error {
	if err := binary.Write(
		w, binary.LittleEndian, math.Float64bits(v.X)); err != nil {
		return err
	}
	if err := binary.Write(
		w, binary.LittleEndian, math.Float64bits(v.Y)); err != nil {
		return err
	}
	return binary.Write(
		w, binary.LittleEndian, math.Float64bits(v.Z))
}

// Read fills this vertex with three 64 bit values stored as
// little endian from the given reader.
func (v *Vertex) Read(r io.Reader) error {
	var buf [8]byte
	b := buf[:]
	if _, err := io.ReadFull(r, b); err != nil {
		return nil
	}
	v.X = math.Float64frombits(binary.LittleEndian.Uint64(b))
	if _, err := io.ReadFull(r, b); err != nil {
		return nil
	}
	v.Y = math.Float64frombits(binary.LittleEndian.Uint64(b))
	if _, err := io.ReadFull(r, b); err != nil {
		return nil
	}
	v.Z = math.Float64frombits(binary.LittleEndian.Uint64(b))
	return nil
}

// AsWKB returns the WKB representation of the given multi line string.
func (mls MultiLineStringZ) AsWKB() []byte {

	// pre-calculate size to avoid reallocations.
	size := 1 + 4 + 4
	for _, ml := range mls {
		size += 1 + 4 + 4 + len(ml)*3*8
	}

	buf := bytes.NewBuffer(make([]byte, 0, size))

	binary.Write(buf, binary.LittleEndian, wkb.NDR)
	binary.Write(buf, binary.LittleEndian, wkb.MultiLineStringZ)
	binary.Write(buf, binary.LittleEndian, uint32(len(mls)))

	for _, ml := range mls {
		binary.Write(buf, binary.LittleEndian, wkb.NDR)
		binary.Write(buf, binary.LittleEndian, wkb.LineStringZ)
		binary.Write(buf, binary.LittleEndian, uint32(len(ml)))
		for _, p := range ml {
			binary.Write(buf, binary.LittleEndian, math.Float64bits(p.X))
			binary.Write(buf, binary.LittleEndian, math.Float64bits(p.Y))
			binary.Write(buf, binary.LittleEndian, math.Float64bits(p.Z))
		}
	}

	return buf.Bytes()
}

// AsWKB2D returns the WKB representation of the given multi line string
// leaving the z component out.
func (mls MultiLineStringZ) AsWKB2D() []byte {

	// pre-calculate size to avoid reallocations.
	size := 1 + 4 + 4
	for _, ml := range mls {
		size += 1 + 4 + 4 + len(ml)*2*8
	}

	buf := bytes.NewBuffer(make([]byte, 0, size))

	binary.Write(buf, binary.LittleEndian, wkb.NDR)
	binary.Write(buf, binary.LittleEndian, wkb.MultiLineString)
	binary.Write(buf, binary.LittleEndian, uint32(len(mls)))

	for _, ml := range mls {
		binary.Write(buf, binary.LittleEndian, wkb.NDR)
		binary.Write(buf, binary.LittleEndian, wkb.LineString)
		binary.Write(buf, binary.LittleEndian, uint32(len(ml)))
		for _, p := range ml {
			binary.Write(buf, binary.LittleEndian, math.Float64bits(p.X))
			binary.Write(buf, binary.LittleEndian, math.Float64bits(p.Y))
		}
	}

	return buf.Bytes()
}

// CCW returns true if this line string is oriented counter clockwise.
func (ls LineStringZ) CCW() bool {
	var sum float64
	for i, v1 := range ls {
		v2 := ls[(i+1)%len(ls)]
		sum += (v2.X - v1.X) * (v2.Y + v1.Y)
	}
	return sum > 0
}

// Join joins two lines leaving the first of the second out.
func (ls LineStringZ) Join(other LineStringZ) LineStringZ {
	nline := make(LineStringZ, len(ls)+len(other)-1)
	copy(nline, ls)
	copy(nline[len(ls):], other[1:])
	return nline
}

// Merge merges line segments of a given multi line string
// by finding common start and end vertices.
func (mls MultiLineStringZ) Merge() MultiLineStringZ {

	var out MultiLineStringZ

	min := Vertex{math.MaxFloat64, math.MaxFloat64, math.MaxFloat64}

	for _, line := range mls {
		for _, v := range line {
			min.Minimize(v)
		}
	}

	type point struct {
		x int64
		y int64
	}

	const precision = 1e7

	quant := func(v Vertex) point {
		return point{
			x: int64(math.Round((v.X - min.X) * precision)),
			y: int64(math.Round((v.Y - min.Y) * precision)),
		}
	}

	heads := make(map[point]*[]LineStringZ)

	for _, line := range mls {
		if len(line) < 2 {
			out = append(out, line)
			continue
		}
		head := quant(line[0])
		tail := quant(line[len(line)-1])
		if head == tail { // its already a ring
			out = append(out, line)
			continue
		}

		if hs := heads[tail]; hs != nil {
			l := len(*hs)
			last := (*hs)[l-1]
			if l == 1 {
				delete(heads, tail)
			} else {
				(*hs)[l-1] = nil
				*hs = (*hs)[:l-1]
			}
			line = line.Join(last)

			if head == quant(line[len(line)-1]) { // its a ring
				out = append(out, line)
				continue
			}
		}

		if hs := heads[head]; hs != nil {
			*hs = append(*hs, line)
		} else {
			heads[head] = &[]LineStringZ{line}
		}
	}

again:
	for head, lines := range heads {
		for i, line := range *lines {
			tail := quant(line[len(line)-1])
			for hs := heads[tail]; hs != nil && len(*hs) > 0; hs = heads[tail] {
				l := len(*hs)
				last := (*hs)[l-1]
				(*hs)[l-1] = nil
				*hs = (*hs)[:l-1]
				line = line.Join(last)

				if tail = quant(line[len(line)-1]); head == tail { // its a ring
					out = append(out, line)
					// remove from current lines
					copy((*lines)[i:], (*lines)[i+1:])
					(*lines)[len(*lines)-1] = nil
					*lines = (*lines)[:len(*lines)-1]
					goto again
				}
				// overwrite in current lines
				(*lines)[i] = line
			}
		}
	}

	// rings := len(out)

	// The rest are open line strings.
	for _, lines := range heads {
		for _, line := range *lines {
			out = append(out, line)
		}
	}

	// log.Debugf("segments before/after merge: %d/%d (%d rings)\n",
	// len(mls), len(out), rings)

	return out
}

// Rect returns the bounding box of this box as separated coordinates.
func (a Box2D) Rect() ([2]float64, [2]float64) {
	return [2]float64{a.X1, a.Y1}, [2]float64{a.X2, a.Y2}
}

// Intersects checks if two Box2Ds intersect.
func (a Box2D) Intersects(b Box2D) bool {
	return !(a.X2 < a.X1 || a.X2 < b.X1 ||
		a.Y2 < a.Y1 || a.Y2 < b.Y1)
}

// Contains returns true if the given point is inside the box.
func (a Box2D) Contains(x, y float64) bool {
	return a.X1 <= x && x <= a.X2 &&
		a.Y1 <= y && y <= a.Y2
}

// Xi returns the i-th x component.
func (a Box2D) Xi(i int) float64 {
	if i == 0 {
		return a.X1
	}
	return a.X2
}

// Yi returns the i-th y component.
func (a Box2D) Yi(i int) float64 {
	if i == 0 {
		return a.Y1
	}
	return a.Y2
}

// Union calculates the united bounding box of the two given boxes.
func (a Box2D) Union(b Box2D) Box2D {
	return Box2D{
		X1: math.Min(a.X1, b.X1),
		Y1: math.Min(a.Y1, b.Y1),
		X2: math.Max(a.X2, b.X2),
		Y2: math.Max(a.Y2, b.Y2),
	}
}

// Area returns the area of the box.
func (a Box2D) Area() float64 {
	return (a.X2 - a.X1) * (a.Y2 - a.Y1)
}

// NewPlane2D creates a new Plane2D from two given points.
func NewPlane2D(x1, y1, x2, y2 float64) Plane2D {
	b := x2 - x1
	a := -(y2 - y1)

	l := math.Sqrt(a*a + b*b)
	a /= l
	b /= l

	// a*x1 + b*y1 + c = 0
	c := -(a*x1 + b*y1)
	return Plane2D{a, b, c}
}

// Eval determines the distance of a given point
// from the plane. The sign of the result indicates
// the sideness.
func (p Plane2D) Eval(x, y float64) float64 {
	return p.A*x + p.B*y + p.C
}

const epsPlane = 1e-5

func sides(s int, x float64) int {
	if math.Signbit(x) {
		return s | 2
	}
	return s | 1
}

// IntersectsPlane checks if a Box2D intersects with
// a given Plane2D.
func (a Box2D) IntersectsPlane(p Plane2D) bool {
	var s int
	for i := 0; i < 2; i++ {
		x := a.Xi(i)
		for j := 0; j < 2; j++ {
			y := a.Yi(j)
			v := p.Eval(x, y)
			if math.Abs(v) < epsPlane {
				//log.Printf("on line")
				return true
			}
			if s = sides(s, v); s == 3 {
				//log.Printf("... on both sides (djt)")
				return true
			}
		}
	}
	//log.Printf("side: %d\n", s)
	return false
}

// Cross calculates the cross product of two vertices.
func (v Vertex) Cross(w Vertex) Vertex {
	return Vertex{
		v.Y*w.Z - v.Z*w.Y,
		v.Z*w.X - v.X*w.Z,
		v.X*w.Y - v.Y*w.X,
	}
}

// Intersection calcultes the 2D intersection point of
// two Plane2Ds. If they do not intersect the returned
// bool flags is set to false.
func (p Plane2D) Intersection(o Plane2D) (float64, float64, bool) {

	u1 := Vertex{p.A, p.B, p.C}
	u2 := Vertex{o.A, o.B, o.C}

	plane := u1.Cross(u2)

	if plane.Z == 0 {
		return 0, 0, false
	}

	return plane.X / plane.Z, plane.Y / plane.Z, true
}

// VerticalLine is a 2D line segment.
type VerticalLine struct {
	x1 float64
	y1 float64
	x2 float64
	y2 float64

	line Plane2D
}

// NewVerticalLine creates a new 2D line segment.
func NewVerticalLine(x1, y1, x2, y2 float64) *VerticalLine {
	return &VerticalLine{
		x1:   x1,
		y1:   y1,
		x2:   x2,
		y2:   y2,
		line: NewPlane2D(x1, y1, x2, y2),
	}
}

func onPlane(x float64) bool { return math.Abs(x) < epsPlane }

func relative(v1, v2 Vertex) func(x, y float64) float64 {
	ls := linearScale(v1.X, v1.Y, v2.X, v2.Y)
	return func(x, y float64) float64 {
		return ls(Vertex{x, y, 0})
	}
}

func interpolate(a, b float64) func(float64) float64 {
	return func(x float64) float64 {
		return (b-a)*x + a
	}
}

func linear(v1, v2 Vertex) func(t float64) Vertex {
	return func(t float64) Vertex {
		return Vertex{
			(v2.X-v1.X)*t + v1.X,
			(v2.Y-v1.Y)*t + v1.Y,
			(v2.Z-v1.Z)*t + v1.Z,
		}
	}
}

func inRange(a float64) bool { return 0 <= a && a <= 1 }

// Intersection intersects a line segment with a triangle.
func (vl *VerticalLine) Intersection(t *Triangle) LineStringZ {

	var out LineStringZ

	//defer func() { log.Printf("length out: %d\n", len(out)) }()

edges:
	for i := 0; i < 3 && len(out) < 2; i++ {
		j := (i + 1) % 3
		edge := NewPlane2D(t[i].X, t[i].Y, t[j].X, t[j].Y)

		s1 := edge.Eval(vl.x1, vl.y1)
		s2 := edge.Eval(vl.x2, vl.y2)

		o1 := onPlane(s1)
		o2 := onPlane(s2)

		// log.Printf("s1, s2: %t %t (%f %f)\n", o1, o2, s1, s2)

		switch {
		case o1 && o2:
			pos := relative(t[i], t[j])
			t1 := pos(vl.x1, vl.y1)
			t2 := pos(vl.x2, vl.y2)

			r1 := inRange(t1)
			r2 := inRange(t2)

			switch {
			case r1 && r2:
				lin := linear(t[i], t[j])
				out = append(out, lin(t1), lin(t2))
				return out

			case !r1 && !r2: // the hole edge
				out = append(out, t[i], t[j])
				return out
			case !r1:
				if t1 < 0 {
					// below first -> clip by first
					lin := linear(t[i], t[j])
					out = append(out, t[i], lin(t2))
				} else {
					// above second -> clip by second
					lin := linear(t[i], t[j])
					out = append(out, lin(t2), t[j])
				}
				return out
			case !r2:
				if t2 < 0 {
					// below first -> clip by first
					lin := linear(t[i], t[j])
					out = append(out, t[i], lin(t1))
				} else {
					// above second -> clip by second
					lin := linear(t[i], t[j])
					out = append(out, lin(t1), t[j])
				}
				return out
			}

		case o1:
			t1 := relative(t[i], t[j])(vl.x1, vl.y1)
			if !inRange(t1) {
				continue edges
			}
			out = append(out, linear(t[i], t[j])(t1))

		case o2:
			t2 := relative(t[i], t[j])(vl.x2, vl.y2)
			if !inRange(t2) {
				continue edges
			}
			out = append(out, linear(t[i], t[j])(t2))

		default:
			x, y, intersects := vl.line.Intersection(edge)
			if !intersects {
				continue edges
			}

			// log.Println("Intersection -----------------------------")
			t1 := relative(t[i], t[j])(x, y)
			// log.Printf("relative pos: %f\n", t1)
			if !inRange(t1) {
				continue edges
			}

			// log.Println("valid ***************")

			z := interpolate(t[j].Z, t[i].Z)(t1)
			n := Vertex{x, y, z}

			if math.Signbit(s1) != math.Signbit(s2) {
				// log.Println("\ton different sides")
				// different sides -> vertex on edge.
				out = append(out, n)
			} else { // same side -> inside. Find peer
				if len(out) > 0 { // we already have our peer
					out = append(out, n)
					return out
				}

				for k := 0; k < 3; k++ {
					if k == i {
						continue
					}
					l := (k + 1) % 3
					other := NewPlane2D(t[k].X, t[k].Y, t[l].X, t[l].Y)
					xo, yo, intersects := vl.line.Intersection(other)
					if !intersects {
						continue
					}
					t2 := relative(t[k], t[l])(xo, yo)
					if !inRange(t2) {
						continue
					}
					zo := interpolate(t[k].Z, t[l].Z)(t2)

					m := Vertex{xo, yo, zo}

					var xn, yn, xf, yf float64

					// Which is nearer to current edge?
					if math.Abs(s1) < math.Abs(s2) {
						xn, yn = vl.x1, vl.y1
						xf, yf = vl.x2, vl.y2
					} else {
						xn, yn = vl.x2, vl.y2
						xf, yf = vl.x1, vl.y1
					}

					if onPlane(other.Eval(xn, yn)) {
						// triangle intersect in only point
						// treat as no intersection
						break edges
					}

					pos := relative(n, m)

					tzn := pos(xn, yn)
					tzf := pos(xf, yf)

					if !inRange(tzn) {
						// if near is out of range far is, too.
						return out
					}

					lin := interpolate(n.Z, m.Z)

					if inRange(tzf) {
						m.X = xf
						m.Y = yf
						m.Z = lin(tzf)
					} // else m is clipping

					n.X = xn
					n.Y = yn
					n.Z = lin(tzn)

					out = append(out, n, m)
					return out
				}
			}
		}
	}

	// supress single point touches.
	if len(out) == 1 {
		out = out[:0]
	}

	return out
}

// QuantizeXY quantize the X/Y values to scale and
// removes duplicates in place. The z value of
// duplicates will be averaged pairwise.
func (mpz MultiPointZ) QuantizeXY(scale float64) MultiPointZ {
	type qpoint struct {
		x int64
		y int64
	}
	m := make(map[qpoint]float64, len(mpz))
	for _, p := range mpz {
		k := qpoint{
			x: int64(math.Round(p.X * scale)),
			y: int64(math.Round(p.Y * scale)),
		}
		if z, ok := m[k]; ok {
			m[k] = (z + p.Z) * 0.5
		} else {
			m[k] = p.Z
		}
	}
	i := 0
	invScale := 1 / scale
	for k, z := range m {
		mpz[i] = Vertex{
			X: float64(k.x) * invScale,
			Y: float64(k.y) * invScale,
			Z: z,
		}
		i++
	}
	return mpz[:i]
}

// Filter returns a copy removed the vertices which
// don't pass the filter test.
func (mpz MultiPointZ) Filter(filter func(Vertex) bool) MultiPointZ {
	n := make(MultiPointZ, 0, len(mpz))
	for _, v := range mpz {
		if filter(v) {
			n = append(n, v)
		}
	}
	return n
}

// MinMax returns the extend of the point set.
func (mpz MultiPointZ) MinMax() (Vertex, Vertex) {
	min := Vertex{math.MaxFloat64, math.MaxFloat64, math.MaxFloat64}
	max := Vertex{-math.MaxFloat64, -math.MaxFloat64, -math.MaxFloat64}
	for _, v := range mpz {
		min.Minimize(v)
		max.Maximize(v)
	}
	return min, max
}

// AsWKB returns a WKB representation of the given point cloud.
func (mpz MultiPointZ) AsWKB() []byte {
	size := 1 + 4 + 4 + len(mpz)*(1+4+3*8)

	buf := bytes.NewBuffer(make([]byte, 0, size))

	binary.Write(buf, binary.LittleEndian, wkb.NDR)
	binary.Write(buf, binary.LittleEndian, wkb.MultiPointZ)
	binary.Write(buf, binary.LittleEndian, uint32(len(mpz)))

	perPoint := bytes.NewBuffer(make([]byte, 0, 1+4))
	binary.Write(perPoint, binary.LittleEndian, wkb.NDR)
	binary.Write(perPoint, binary.LittleEndian, wkb.PointZ)
	hdr := perPoint.Bytes()

	for _, p := range mpz {
		buf.Write(hdr)
		binary.Write(buf, binary.LittleEndian, math.Float64bits(p.X))
		binary.Write(buf, binary.LittleEndian, math.Float64bits(p.Y))
		binary.Write(buf, binary.LittleEndian, math.Float64bits(p.Z))
	}

	return buf.Bytes()
}

// FromWKB de-serializes this multi point z geometry from a WKB representation.
func (mpz *MultiPointZ) FromWKB(data []byte) error {

	r := bytes.NewReader(data)

	endian, err := r.ReadByte()

	var order binary.ByteOrder

	switch {
	case err != nil:
		return err
	case endian == wkb.NDR:
		order = binary.LittleEndian
	case endian == wkb.XDR:
		order = binary.BigEndian
	default:
		return fmt.Errorf("unknown byte order %x", endian)
	}

	var geomType uint32
	err = binary.Read(r, order, &geomType)

	switch {
	case err != nil:
		return err
	case geomType != wkb.MultiPointZ:
		return fmt.Errorf("unknown geometry type %x", geomType)
	}

	var numPoints uint32
	if err := binary.Read(r, order, &numPoints); err != nil {
		return err
	}

	points := make(MultiPointZ, numPoints)

	for i := range points {
		endian, err = r.ReadByte()

		switch {
		case err != nil:
			return err
		case endian == wkb.NDR:
			order = binary.LittleEndian
		case endian == wkb.XDR:
			order = binary.BigEndian
		default:
			return fmt.Errorf("unknown byte order %x", endian)
		}

		err = binary.Read(r, order, &geomType)

		switch {
		case err != nil:
			return err
		case geomType != wkb.PointZ:
			return fmt.Errorf("unknown geometry type %x", geomType)
		}

		var x, y, z uint64
		if err = binary.Read(r, order, &x); err != nil {
			return err
		}
		if err = binary.Read(r, order, &y); err != nil {
			return err
		}
		if err = binary.Read(r, order, &z); err != nil {
			return err
		}
		points[i] = Vertex{
			X: math.Float64frombits(x),
			Y: math.Float64frombits(y),
			Z: math.Float64frombits(z),
		}
	}

	*mpz = points

	return nil
}