package octree

import (
	"log"
	"math"
)

type Tree struct {
	EPSG uint32

	vertices  []Vertex
	triangles [][]int32
	index     []int32

	Min Vertex
	Max Vertex
}

type box2d struct {
	x1 float64
	y1 float64
	x2 float64
	y2 float64
}

func (a box2d) intersects(b box2d) bool {
	return !(a.x2 < a.x1 || a.x2 < b.x1 ||
		a.y2 < a.y1 || a.y2 < b.y1)
}

func (a box2d) mid() (float64, float64) {
	return (a.x2-a.x1)*0.5 + a.x1, (a.y2-a.y1)*0.5 + a.y1
}

func (a box2d) xi(i int) float64 {
	if i == 0 {
		return a.x1
	}
	return a.x2
}

func (a box2d) yi(i int) float64 {
	if i == 0 {
		return a.y1
	}
	return a.y2
}

var scale = [4][4]float64{
	{0.0, 0.0, 0.5, 0.5},
	{0.5, 0.0, 1.0, 0.5},
	{0.0, 0.5, 0.5, 1.0},
	{0.5, 0.5, 1.0, 1.0},
}

type plane2d struct {
	a float64
	b float64
	c float64
}

func newPlane2d(x1, y1, x2, y2 float64) plane2d {
	a := x2 - x1
	b := 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}
}

func (p plane2d) eval(x, y float64) float64 {
	return p.a*x + p.b*y + p.c
}

func (ot *Tree) Vertical(x1, y1, x2, y2 float64, fn func(*Triangle)) {

	box := box2d{
		x1: math.Min(x1, x2),
		y1: math.Min(y1, y2),
		x2: math.Max(x1, x2),
		y2: math.Max(y1, y2),
	}

	// out of bounding box
	if box.x2 < ot.Min.X || ot.Max.X < box.x1 ||
		box.y2 < ot.Min.Y || ot.Max.Y < box.y1 {
		return
	}

	line := newPlane2d(x1, y1, x2, y2)

	type frame struct {
		pos int32
		box2d
	}

	dupes := map[int32]struct{}{}

	all := box2d{ot.Min.X, ot.Min.Y, ot.Max.X, ot.Max.Y}
	log.Printf("area: %f\n", (box.x2-box.x1)*(box.y2-box.y1))
	log.Printf("all: %f\n", (all.x2-all.x1)*(all.y2-all.y1))

	stack := []frame{{1, all}}

	var lineRejected int
	var dupeRejected int

	for len(stack) > 0 {
		top := stack[len(stack)-1]
		stack = stack[:len(stack)-1]

		if top.pos > 0 { // node
			for i := int32(0); i < 4; i++ {
				a := ot.index[top.pos+i]
				b := ot.index[top.pos+i+4]
				if a == 0 && b == 0 {
					continue
				}
				dx := top.x2 - top.x1
				dy := top.y2 - top.y1
				nbox := box2d{
					dx*scale[i][0] + top.x1,
					dy*scale[i][1] + top.y1,
					dx*scale[i][2] + top.x1,
					dy*scale[i][3] + top.y1,
				}
				if nbox.intersects(box) {
					if a != 0 {
						stack = append(stack, frame{a, nbox})
					}
					if b != 0 {
						stack = append(stack, frame{b, nbox})
					}
				}
			}
		} else { // leaf
			pos := -top.pos - 1
			n := ot.index[pos]
			indices := ot.index[pos+1 : pos+1+n]

			for _, idx := range indices {
				if _, found := dupes[idx]; found {
					dupeRejected++
					continue
				}
				tri := ot.triangles[idx]
				t := Triangle{
					ot.vertices[tri[0]],
					ot.vertices[tri[1]],
					ot.vertices[tri[2]],
				}

				v0 := line.eval(t[0].X, t[0].Y)
				v1 := line.eval(t[1].X, t[1].Y)
				v2 := line.eval(t[2].X, t[2].Y)

				sides := func(s int, b bool) int {
					if b {
						return s | 2
					}
					return s | 1
				}

				const eps = 1e-05

				if math.Abs(v0) < eps || math.Abs(v1) < eps ||
					math.Abs(v2) < eps ||
					sides(sides(sides(0,
						math.Signbit(v0)),
						math.Signbit(v1)),
						math.Signbit(v2)) == 3 {
					fn(&t)
				} else {
					lineRejected++
				}
				dupes[idx] = struct{}{}
			}
		}
	}

	// XXX: This value seems to high!
	log.Printf("rejected by line test: %d %d %d\n",
		lineRejected, len(dupes), dupeRejected)
}

func (ot *Tree) Horizontal(h float64, fn func(*Triangle)) {

	if h < ot.Min.Z || ot.Max.Z < h {
		return
	}

	type frame struct {
		pos int32
		min float64
		max float64
	}

	dupes := map[int32]struct{}{}

	stack := []frame{{1, ot.Min.Z, ot.Max.Z}}

	for len(stack) > 0 {
		top := stack[len(stack)-1]
		stack = stack[:len(stack)-1]

		pos := top.pos
		if pos == 0 {
			continue
		}
		min, max := top.min, top.max

		if pos > 0 { // node
			if mid := (max-min)*0.5 + min; h >= mid {
				pos += 4 // nodes with z-bit set
				min = mid
			} else {
				max = mid
			}
			stack = append(stack,
				frame{ot.index[pos+0], min, max},
				frame{ot.index[pos+1], min, max},
				frame{ot.index[pos+2], min, max},
				frame{ot.index[pos+3], min, max})
		} else { // leaf
			pos = -pos - 1
			n := ot.index[pos]
			//log.Printf("%d %d %d\n", pos, n, len(ot.index))
			indices := ot.index[pos+1 : pos+1+n]

			for _, idx := range indices {
				if _, found := dupes[idx]; found {
					continue
				}
				tri := ot.triangles[idx]
				t := Triangle{
					ot.vertices[tri[0]],
					ot.vertices[tri[1]],
					ot.vertices[tri[2]],
				}

				if !(math.Min(t[0].Z, math.Min(t[1].Z, t[2].Z)) > h ||
					math.Max(t[0].Z, math.Max(t[1].Z, t[2].Z)) < h) {
					dupes[idx] = struct{}{}
					fn(&t)
				}
			}
		}
	}
}