package govector

import (
	"fmt"
	"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)

	fmt.Printf("%v\n", m2)

	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)

	fmt.Printf("%v\n", m)

	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)
	fmt.Printf("%v\n", d)

	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, p := Intersection(p1, p2, v1, v2)
	fmt.Printf("%v %v\n", i, p)
	if !i {
		t.Errorf("Intersection Error")
	}

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

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

	i, p = Intersection(p1, p2, v1, v2)
	fmt.Printf("%v %v\n", i, p)
	if i {
		t.Errorf("Intersection Error")
	}
}

func TestAngle(t *testing.T) {
	v1 := Vector2d{10, 0}
	v2 := Vector2d{0, 10}

	a := Angle(v2, v1)

	fmt.Printf("%v %v %f\n", v1, v2, a*Rad2deg)
	if math.Abs(float64(a))-math.Pi/2 > Epsilon {
		t.Errorf("Angle Error")
	}

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

	a = Angle(v1, v2)

	fmt.Printf("%v %v %f\n", v1, v2, a*Rad2deg)

	if math.Abs(float64(a))-math.Pi/4 > Epsilon {
		t.Errorf("Angle Error")
	}

}

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

	fmt.Printf("%v %v\n", v1, v2)
	if v2.X > Epsilon || v2.Y-10 > Epsilon {
		t.Errorf("Rotate Error")
	}

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

	fmt.Printf("%v %v\n", v1, v2)
	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,
	}

	fmt.Printf("%v %v\n", p1, r)
	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) {
	p1 := Point2d{10, 0}
	v1 := Vector2d{0, 10}

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

	fmt.Printf("%v %v\n", p1, r)
	coll, inside, pos := RectIntersection(r, p1, v1)
	if coll != true {
		fmt.Printf("Coll Err %v %v %v\n", inside, coll, pos)
		t.Errorf("RectIntersection Error")
	}
	if inside != false {
		fmt.Printf("Inside Err %v %v %v\n", inside, coll, pos)
		t.Errorf("RectIntersection Error")
	}
	if pos.X != 10 && pos.Y != 5 {
		fmt.Printf("Pos Err %v %v %v\n", inside, coll, pos)
		t.Errorf("RectIntersection Error")
	}
}

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, p := PolygonIntersection(p1, Point2d{0, 0}, Vector2d{10, 10})
	if i {
		fmt.Printf("%v", p)
		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, p := PolygonIntersection(p1, Point2d{0, 0}, Vector2d{-10, -10})
	if i {
		fmt.Printf("%v", p)
		t.Errorf("PolyIntersect Error")
	}
}