package govector

import (
	"math"
	"testing"
)

func TestSubMag(t *testing.T) {
	p1 := Point2d{10, 10}
	p2 := Point2d{7, 6}

	v := p1.Sub(p2)

	if v != (Vector2d{3, 4}) {
		t.Errorf("Sub Error")
	}

	m := v.Mag()

	if m != 5 {
		t.Errorf("Mag Error")
	}
}

func TestScale(t *testing.T) {
	v := Vector2d{3, 4}
	m := v.Mag()

	if m != 5 {
		t.Errorf("Mag Error")
	}

	m2 := v.Scale(2)

	if m2.Mag() != 10 {
		t.Errorf("Mag Error")
	}
}

func TestAdd(t *testing.T) {
	p := Point2d{10, 10}
	v := Vector2d{3, 4}
	m := p.Add(v)

	if m.X != 13 && m.Y != 14 {
		t.Errorf("Add Error")
	}
}

func TestNormalize(t *testing.T) {
	v := Vector2d{3, 4}
	vn := v.Normalize()

	if vn.Mag() != 1 {
		t.Errorf("Normalize Error")
	}

	v0 := Vector2d{0, 0}
	vn0 := v0.Normalize()

	if vn0.Mag() != 0 {
		t.Errorf("Normalize Error")
	}

}

func TestDot(t *testing.T) {
	v1 := Vector2d{0, 5}
	v2 := Vector2d{5, 0}
	d := v1.Dot(v2)

	if d != 0 {
		t.Errorf("Dot Error")
	}

	v1 = Vector2d{1, 5}
	v2 = Vector2d{5, 0}
	d = v1.Dot(v2)

	if d < 0 {
		t.Errorf("Dot Error")
	}

	v1 = Vector2d{-1, 5}
	v2 = Vector2d{5, 0}
	d = v1.Dot(v2)

	if d > 0 {
		t.Errorf("Dot Error")
	}

	v1 = Vector2d{5, 5}
	v2 = Vector2d{5, 0}
	v1 = v1.Normalize()
	v2 = v2.Normalize()
	d = v1.Dot(v2)

	if math.Abs(float64(d)-math.Acos(math.Pi/4)) < 0.0001 {
		t.Errorf("Dot Error")
	}
}

func TestIntersection(t *testing.T) {
	p1 := Point2d{0, 5}
	p2 := Point2d{5, 0}

	v1 := Vector2d{10, 0}
	v2 := Vector2d{0, 10}

	i, _ := Intersection(p1, p2, v1, v2)
	if !i {
		t.Errorf("Intersection Error")
	}

	p1 = Point2d{20, 5}
	p2 = Point2d{5, 0}

	v1 = Vector2d{10, 0}
	v2 = Vector2d{0, 10}

	i, _ = Intersection(p1, p2, v1, v2)
	if i {
		t.Errorf("Intersection Error")
	}
}

func TestAngle(t *testing.T) {
	cases := []struct {
		v1       Vector2d
		v2       Vector2d
		expected float32
	}{
		{
			v1:       Vector2d{10, 0},
			v2:       Vector2d{0, 10},
			expected: -math.Pi / 2,
		},
		{
			v1:       Vector2d{5, 0},
			v2:       Vector2d{10, 10},
			expected: -math.Pi / 4,
		},
		{
			v1:       Vector2d{0, 10},
			v2:       Vector2d{0, -10},
			expected: math.Pi,
		},
		// nan angles:
		{
			v1:       Vector2d{0, 0},
			v2:       Vector2d{0, 0},
			expected: 0,
		},
		// nan angles
		{
			v1:       Vector2d{0.0001, 0},
			v2:       Vector2d{0, 0},
			expected: math.Pi * Rad2deg,
		},
	}
	var a float32
	for _, c := range cases {
		a = Angle(c.v2, c.v1)

		if math.Abs(float64(a-c.expected)) > Epsilon {
			t.Errorf("Angle Error; expected: %f, actual: %f", c.expected, a)
		}
	}
}

func TestRotate(t *testing.T) {
	v1 := Vector2d{10, 0}
	v2 := v1.Rotate(math.Pi / 2)

	if v2.X > Epsilon || v2.Y-10 > Epsilon {
		t.Errorf("Rotate Error")
	}

	v1 = Vector2d{1, 0}
	v2 = v1.Rotate(-30 * Deg2rad)

	if v2.X-0.866025403784438 > Epsilon || v2.Y+0.5 > Epsilon {
		t.Errorf("Rotate Error")
	}
}

func TestPtInRect(t *testing.T) {
	p1 := Point2d{10, 0}
	p2 := Point2d{10, 10}
	p3 := Point2d{11, 10000}
	p4 := Point2d{0, 0}

	pA := Point2d{5, 5}
	pB := Point2d{15, 15}
	r := AABB2d{
		A: pA,
		B: pB,
	}

	out := PointInRect(p1, r)
	if out != false {
		t.Errorf("PointInRect Error")
	}
	out = PointInRect(p2, r)
	if out != true {
		t.Errorf("PointInRect Error")
	}
	out = PointInRect(p3, r)
	if out != false {
		t.Errorf("PointInRect Error")
	}
	out = PointInRect(p4, r)
	if out != false {
		t.Errorf("PointInRect Error")
	}
}

func TestRectIntersect(t *testing.T) {
	type result struct {
		collided bool
		inside   bool
		pos      Point2d
	}
	tests := []struct {
		rect     AABB2d
		p        Point2d
		v        Vector2d
		expected result
	}{
		// inside
		{
			rect: AABB2d{
				A: Point2d{0, 0},
				B: Point2d{10, 10},
			},
			p: Point2d{1, 1},
			v: Vector2d{2, 2},
			expected: result{
				collided: false,
				inside:   true,
				pos:      Point2d{1, 1},
			},
		},
		// bottom
		{
			rect: AABB2d{
				A: Point2d{5, 5},
				B: Point2d{15, 15},
			},
			p: Point2d{10, 0},
			v: Vector2d{0, 10},
			expected: result{
				collided: true,
				inside:   false,
				pos:      Point2d{10, 5},
			},
		},
		// wall left
		{
			rect: AABB2d{
				A: Point2d{0, 0},
				B: Point2d{10, 10},
			},
			p: Point2d{-1, 5},
			v: Vector2d{2, 0},
			expected: result{
				collided: true,
				inside:   false,
				pos:      Point2d{0, 5},
			},
		},
		// wall right
		{
			rect: AABB2d{
				A: Point2d{0, 0},
				B: Point2d{10, 10},
			},
			p: Point2d{11, 5},
			v: Vector2d{-2, 0},
			expected: result{
				collided: true,
				inside:   false,
				pos:      Point2d{10, 5},
			},
		},
		// wall top
		{
			rect: AABB2d{
				A: Point2d{0, 0},
				B: Point2d{10, 10},
			},
			p: Point2d{5, 11},
			v: Vector2d{0, -2},
			expected: result{
				collided: true,
				inside:   false,
				pos:      Point2d{5, 10},
			},
		},
		// outside
		{
			rect: AABB2d{
				A: Point2d{0, 0},
				B: Point2d{10, 10},
			},
			p: Point2d{5, -1},
			v: Vector2d{0, -1},
			expected: result{
				collided: false,
				inside:   false,
				pos:      Point2d{5, -1},
			},
		},
	}
	for _, test := range tests {
		coll, inside, pos := RectIntersection(test.rect, test.p, test.v)
		if coll != test.expected.collided {
			t.Errorf("unexpected collision: expected: %t, actual: %t", test.expected.collided, coll)
		}
		if inside != test.expected.inside {
			t.Errorf("RectIntersection Error")
			t.Errorf("unexpected inside: expected: %t, actual: %t", test.expected.inside, inside)
		}
		if pos != test.expected.pos {
			t.Errorf("unexpected collision point: expected: %+v, actual: %+v", test.expected.pos, pos)
		}
	}

}

func TestRectRectIntersectionTrue(t *testing.T) {
	r1 := AABB2d{Point2d{10, 10}, Point2d{20, 20}}
	r2 := AABB2d{Point2d{15, 15}, Point2d{25, 25}}

	col := RectRectIntersection(r1, r2)
	if !col {
		t.Errorf("RectRect Error")
	}

	r1 = AABB2d{Point2d{10, 10}, Point2d{20, 20}}
	r2 = AABB2d{Point2d{5, 15}, Point2d{30, 25}}

	col = RectRectIntersection(r1, r2)
	if !col {
		t.Errorf("RectRect Error")
	}
}

func TestRectRectIntersectionFalse(t *testing.T) {
	r1 := AABB2d{Point2d{10, 10}, Point2d{14, 14}}
	r2 := AABB2d{Point2d{15, 15}, Point2d{25, 25}}

	col := RectRectIntersection(r1, r2)
	if col {
		t.Errorf("RectRect Error")
	}
}

func TestPolyIntersect(t *testing.T) {
	p1 := Polygon2d{}
	p1.Points = append(p1.Points, Vector2d{3, 3})
	p1.Points = append(p1.Points, Vector2d{5, 5})
	p1.Points = append(p1.Points, Vector2d{5, 3})

	i, _ := PolygonIntersection(p1, Point2d{0, 0}, Vector2d{10, 10})
	if i {
		t.Errorf("PolyIntersect Error")
	}
}

func TestPolyIntersectFail(t *testing.T) {
	p1 := Polygon2d{}
	p1.Points = append(p1.Points, Vector2d{3, 3})
	p1.Points = append(p1.Points, Vector2d{5, 5})
	p1.Points = append(p1.Points, Vector2d{5, 3})

	i, _ := PolygonIntersection(p1, Point2d{0, 0}, Vector2d{-10, -10})
	if i {
		t.Errorf("PolyIntersect Error")
	}
}

func TestPolyIntersectTwoHits(t *testing.T) {
	p1 := Polygon2d{}
	p1.Points = append(p1.Points, Vector2d{0, 0})
	p1.Points = append(p1.Points, Vector2d{0, 10})
	p1.Points = append(p1.Points, Vector2d{10, 10})
	p1.Points = append(p1.Points, Vector2d{10, 0})

	i, p := PolygonIntersection(p1, Point2d{-1, 5}, Vector2d{20, 0})
	if !i || (p.X != 0 && p.Y != 5) {
		t.Errorf("PolyIntersect Error")
	}
}

func TestPolyPolyIntersect(t *testing.T) {
	p1 := Polygon2d{}
	p1.Points = append(p1.Points, Vector2d{0, 0})
	p1.Points = append(p1.Points, Vector2d{0, 10})
	p1.Points = append(p1.Points, Vector2d{10, 10})
	p1.Points = append(p1.Points, Vector2d{10, 0})

	p2 := Polygon2d{}
	p2.Origin = Point2d{5, 5}
	p2.Points = append(p2.Points, Vector2d{0, 0})
	p2.Points = append(p2.Points, Vector2d{0, 10})
	p2.Points = append(p2.Points, Vector2d{10, 10})
	p2.Points = append(p2.Points, Vector2d{10, 0})

	i, m, _ := PolyPolyIntersection(p1, Vector2d{0, 0}, p2)
	if !i || !m {
		t.Errorf("should not have intersected")
	}
}

func TestPolyPolyIntersectMoving(t *testing.T) {
	p1 := Polygon2d{}
	p1.Points = append(p1.Points, Vector2d{0, 0})
	p1.Points = append(p1.Points, Vector2d{0, 10})
	p1.Points = append(p1.Points, Vector2d{10, 10})
	p1.Points = append(p1.Points, Vector2d{10, 0})

	p2 := Polygon2d{}
	p2.Origin = Point2d{15, 15}
	p2.Points = append(p2.Points, Vector2d{0, 0})
	p2.Points = append(p2.Points, Vector2d{0, 10})
	p2.Points = append(p2.Points, Vector2d{10, 10})
	p2.Points = append(p2.Points, Vector2d{10, 0})

	i, m, _ := PolyPolyIntersection(p1, Vector2d{10, 10}, p2)
	if i || !m {
		t.Errorf("should not have intersected")
	}
}

func TestPolyPolyIntersectFail(t *testing.T) {
	p1 := Polygon2d{}
	p1.Points = append(p1.Points, Vector2d{0, 0})
	p1.Points = append(p1.Points, Vector2d{0, 10})
	p1.Points = append(p1.Points, Vector2d{10, 10})
	p1.Points = append(p1.Points, Vector2d{10, 0})

	p2 := Polygon2d{}
	p2.Origin = Point2d{15, 15}
	p2.Points = append(p2.Points, Vector2d{0, 0})
	p2.Points = append(p2.Points, Vector2d{0, 10})
	p2.Points = append(p2.Points, Vector2d{10, 10})
	p2.Points = append(p2.Points, Vector2d{10, 0})

	i, m, _ := PolyPolyIntersection(p1, Vector2d{0, 0}, p2)
	if i || m {
		t.Errorf("should not have intersected")
	}
}

func TestOrientedSquare(t *testing.T) {
	p := OrientedSquare(Point2d{10, 10}, Vector2d{0.5, 0.5}, 5)
	expected := []Vector2d{
		{0, 7.071068},
		{-7.071068, 0},
		{0, -7.071068},
		{7.071068, 0},
	}
	for i, v := range expected {
		if p.Points[i] != v {
			t.Errorf("unexpected points: %+v, expected: %+v", p.Points, expected)
		}
	}
}

func TestPopPop(t *testing.T) {
	v1 := Vector2d{10, 10}
	if v1.PopPop() != float32(math.Sqrt(10*10+10*10)) {
		t.Errorf("Incorrect Magnitude!")
	}
}

func TestAASquareAtPoint(t *testing.T) {
	p1 := Point2d{10, 10}
	expected := AABB2d{
		A: Point2d{5, 5},
		B: Point2d{15, 15},
	}
	aabb := AASquareAtPoint(p1, 10)
	if aabb != expected {
		t.Errorf("incorrect AABB2d; expected: %+v, calculated: %+v ", expected, aabb)
	}

}

func TestDistance(t *testing.T) {
	if Distance(Point2d{0, 0}, Point2d{0, 0}) != 0.0 {
		t.Error("incorectly calculating distance between two points at origin")
	}
	if Distance(Point2d{0, 0}, Point2d{10, 0}) != 10.0 {
		t.Error("incorectly calculating distance between points on y axis")
	}
	if Distance(Point2d{0, 0}, Point2d{3, 4}) != 5.0 {
		t.Error("incorectly calculating distance for pythagorean triple")
	}
}

func TestAABBToRect(t *testing.T) {
	r := AABB2d{
		A: Point2d{5, 5},
		B: Point2d{15, 15},
	}
	p := r.ToPolygon()
	expected := Polygon2d{
		Points: []Vector2d{
			Vector2d{X: 0, Y: 0},
			Vector2d{X: 0, Y: 10},
			Vector2d{X: 10, Y: 10},
			Vector2d{X: 10, Y: 0},
		},
		Origin: Point2d{X: 5, Y: 5},
	}
	if len(p.Points) != 4 {
		t.Error("incorrect number of points")
	}
	// alright, this just feels cumbersome ... unfortunate that structs with
	// slices can't be compared this way ...
	if p.Origin != expected.Origin {
		t.Error("just wrong")
	}
	for i, pp := range expected.Points {
		if p.Points[i] != pp {
			t.Error("just wrong")
		}
	}
}