package hrend

// This is the linear algebra junk? Vectors, matrices, etc
import (
	//"log"
	"math"
)

type Vec3f struct {
	X, Y, Z float32
}

type Vec2i struct {
	X, Y int
}

type Vec2f struct {
	X, Y float32
}

// A ROW MAJOR matrix
type Mat44f [16]float32

func (m *Mat44f) Set(x int, y int, val float32) {
	m[x+y*4] = val
}
func (m *Mat44f) Get(x int, y int) float32 {
	return m[x+y*4]
}
func (m *Mat44f) ZeroFill() {
	for i := range m {
		m[i] = 0
	}
}

// Multiply the entire matrix by the given value
func (m *Mat44f) MultiplySelf(f float32) {
	for i := range m {
		m[i] *= f
	}
}

// Copied from https://github.com/go-gl/mathgl/blob/master/mgl32/matrix.go
func (m *Mat44f) Determinant() float32 {
	return m[0]*m[5]*m[10]*m[15] - m[0]*m[5]*m[11]*m[14] - m[0]*m[6]*m[9]*m[15] + m[0]*m[6]*m[11]*m[13] + m[0]*m[7]*m[9]*m[14] - m[0]*m[7]*m[10]*m[13] - m[1]*m[4]*m[10]*m[15] + m[1]*m[4]*m[11]*m[14] + m[1]*m[6]*m[8]*m[15] - m[1]*m[6]*m[11]*m[12] - m[1]*m[7]*m[8]*m[14] + m[1]*m[7]*m[10]*m[12] + m[2]*m[4]*m[9]*m[15] - m[2]*m[4]*m[11]*m[13] - m[2]*m[5]*m[8]*m[15] + m[2]*m[5]*m[11]*m[12] + m[2]*m[7]*m[8]*m[13] - m[2]*m[7]*m[9]*m[12] - m[3]*m[4]*m[9]*m[14] + m[3]*m[4]*m[10]*m[13] + m[3]*m[5]*m[8]*m[14] - m[3]*m[5]*m[10]*m[12] - m[3]*m[6]*m[8]*m[13] + m[3]*m[6]*m[9]*m[12]
}

// Copied from https://github.com/go-gl/mathgl/blob/master/mgl32/matrix.go
func (m *Mat44f) Inverse() *Mat44f {
	det := m.Determinant()

	if det == float32(0.0) {
		return &Mat44f{}
	}

	// How the hell am I supposed to know if this is correct? Oh well...
	result := Mat44f{
		-m[7]*m[10]*m[13] + m[6]*m[11]*m[13] + m[7]*m[9]*m[14] - m[5]*m[11]*m[14] - m[6]*m[9]*m[15] + m[5]*m[10]*m[15],
		m[3]*m[10]*m[13] - m[2]*m[11]*m[13] - m[3]*m[9]*m[14] + m[1]*m[11]*m[14] + m[2]*m[9]*m[15] - m[1]*m[10]*m[15],
		-m[3]*m[6]*m[13] + m[2]*m[7]*m[13] + m[3]*m[5]*m[14] - m[1]*m[7]*m[14] - m[2]*m[5]*m[15] + m[1]*m[6]*m[15],
		m[3]*m[6]*m[9] - m[2]*m[7]*m[9] - m[3]*m[5]*m[10] + m[1]*m[7]*m[10] + m[2]*m[5]*m[11] - m[1]*m[6]*m[11],
		m[7]*m[10]*m[12] - m[6]*m[11]*m[12] - m[7]*m[8]*m[14] + m[4]*m[11]*m[14] + m[6]*m[8]*m[15] - m[4]*m[10]*m[15],
		-m[3]*m[10]*m[12] + m[2]*m[11]*m[12] + m[3]*m[8]*m[14] - m[0]*m[11]*m[14] - m[2]*m[8]*m[15] + m[0]*m[10]*m[15],
		m[3]*m[6]*m[12] - m[2]*m[7]*m[12] - m[3]*m[4]*m[14] + m[0]*m[7]*m[14] + m[2]*m[4]*m[15] - m[0]*m[6]*m[15],
		-m[3]*m[6]*m[8] + m[2]*m[7]*m[8] + m[3]*m[4]*m[10] - m[0]*m[7]*m[10] - m[2]*m[4]*m[11] + m[0]*m[6]*m[11],
		-m[7]*m[9]*m[12] + m[5]*m[11]*m[12] + m[7]*m[8]*m[13] - m[4]*m[11]*m[13] - m[5]*m[8]*m[15] + m[4]*m[9]*m[15],
		m[3]*m[9]*m[12] - m[1]*m[11]*m[12] - m[3]*m[8]*m[13] + m[0]*m[11]*m[13] + m[1]*m[8]*m[15] - m[0]*m[9]*m[15],
		-m[3]*m[5]*m[12] + m[1]*m[7]*m[12] + m[3]*m[4]*m[13] - m[0]*m[7]*m[13] - m[1]*m[4]*m[15] + m[0]*m[5]*m[15],
		m[3]*m[5]*m[8] - m[1]*m[7]*m[8] - m[3]*m[4]*m[9] + m[0]*m[7]*m[9] + m[1]*m[4]*m[11] - m[0]*m[5]*m[11],
		m[6]*m[9]*m[12] - m[5]*m[10]*m[12] - m[6]*m[8]*m[13] + m[4]*m[10]*m[13] + m[5]*m[8]*m[14] - m[4]*m[9]*m[14],
		-m[2]*m[9]*m[12] + m[1]*m[10]*m[12] + m[2]*m[8]*m[13] - m[0]*m[10]*m[13] - m[1]*m[8]*m[14] + m[0]*m[9]*m[14],
		m[2]*m[5]*m[12] - m[1]*m[6]*m[12] - m[2]*m[4]*m[13] + m[0]*m[6]*m[13] + m[1]*m[4]*m[14] - m[0]*m[5]*m[14],
		-m[2]*m[5]*m[8] + m[1]*m[6]*m[8] + m[2]*m[4]*m[9] - m[0]*m[6]*m[9] - m[1]*m[4]*m[10] + m[0]*m[5]*m[10],
	}

	result.MultiplySelf(1 / det)
	//log.Print(m)
	//log.Print(result)

	return &result
}

func (m *Mat44f) SetIdentity() {
	m.ZeroFill()
	for i := range 4 {
		m.Set(i, i, 1)
	}
}

// NOTE: we use "Set" instead of "Create" for all these so we reuse the matrix
// instead of creating a new one all the time (garbage collection)

// Compute the projection matrix, filling the given matrix. FOV is in degrees
func (m *Mat44f) SetProjection(fov float32, aspect float32, near float32, far float32) {
	// Projection matrix is (ROW MAJOR!)
	// S    0    0    0
	// 0    S    0    0
	// 0    0 -f/(f-n) -1
	// 0    0 -fn/(f-n) 0
	// where S (scale) is 1 / tan(fov / 2) (assuming fov is radians)
	// // NOTE: -1 there is actually -1/c, where c is distance from viewer to
	// // projection plane. We fix it at 1 for now but...
	// m.ZeroFill()
	// scale := float32(1 / math.Tan(float64(fov)*0.5*math.Pi/180))
	// m.Set(0, 0, scale)
	// m.Set(1, 1, scale)
	// m.Set(2, 2, -far/(far-near))
	// m.Set(3, 2, -1)
	// m.Set(2, 3, -far*near/(far-near))
	// OK apparently I suck, let's use somebody else's projection matrix:
	m.ZeroFill()

	DEG2RAD := math.Acos(-1.0) / 180.0
	tangent := math.Tan(float64(fov/2.0) * DEG2RAD) // tangent of half fovY
	top := near * float32(tangent)                  // half height of near plane
	right := top * aspect                           // half width of near plane
	// Column major maybe???
	// n/r 0   0             0
	// 0   n/t 0             0
	// 0   0   -(f+n)/(f-n) -1
	// 0   0   -(2fn)/(f-n)  0

	m.Set(0, 0, near/right)
	m.Set(1, 1, near/top)
	m.Set(2, 2, -(far+near)/(far-near))
	m.Set(3, 2, -1)
	m.Set(2, 3, -(2*far*near)/(far-near))
}

func (m *Mat44f) SetTranslation(x, y, z float32) {
	m.SetIdentity()
	m.Set(0, 3, x) // Let user decide how to offset x
	m.Set(1, 3, y) // Let user decide how to offset x
	m.Set(2, 3, z) // Get farther away from the face (user)
}

func (m *Mat44f) SetRotationX(radang float32) {
	m.SetIdentity()
	m[5] = float32(math.Cos(float64(radang)))
	m[10] = m[5]
	m[6] = float32(math.Sin(float64(radang)))
	m[9] = -m[6]
}

func (m *Mat44f) SetRotationY(radang float32) {
	m.SetIdentity()
	m[0] = float32(math.Cos(float64(radang)))
	m[10] = m[0]
	m[8] = float32(math.Sin(float64(radang)))
	m[2] = -m[8]
}

func (m *Mat44f) SetRotationZ(radang float32) {
	m.SetIdentity()
	m[0] = float32(math.Cos(float64(radang)))
	m[5] = m[0]
	m[4] = float32(math.Sin(float64(radang)))
	m[2] = -m[4]
}

// Camera is easier to deal with using yaw and pitch, since we're not supporting roll
func (m *Mat44f) SetCamera(loc *Vec3f, yaw float32, pitch float32, up *Vec3f) Vec3f {
	// Use sphere equation to compute lookat vector through the two
	// player-controled angles (pitch and yaw)
	lookvec := Vec3f{
		Z: float32(-math.Sin(float64(pitch)) * math.Cos(float64(yaw))),
		X: float32(math.Sin(float64(pitch)) * math.Sin(float64(yaw))),
		Y: float32(math.Cos(float64(pitch))),
	}
	m.SetLookAt(loc, loc.Add(&lookvec), up)
	//m.Set(0, 0, m.Get(0, 0)*scale)
	//m.Set(1, 1, m.Get(1, 1)*scale)
	//m.Set(2, 2, m.Get(2, 2)*scale)
	return lookvec
}

// Note: use {0,1,0} for up for normal use
func (m *Mat44f) SetLookAt(from *Vec3f, to *Vec3f, up *Vec3f) {
	forward := from.Sub(to).Normalize()
	// IDK if you have to normalize but whatever
	right := up.CrossProduct(forward).Normalize()
	realup := forward.CrossProduct(right)
	m.SetIdentity()
	m.Set(0, 0, right.X)
	m.Set(1, 0, right.Y)
	m.Set(2, 0, right.Z)
	m.Set(0, 1, realup.X)
	m.Set(1, 1, realup.Y)
	m.Set(2, 1, realup.Z)
	m.Set(0, 2, forward.X)
	m.Set(1, 2, forward.Y)
	m.Set(2, 2, forward.Z)
	m.Set(0, 3, from.X)
	m.Set(1, 3, from.Y)
	m.Set(2, 3, from.Z)
}

func (m *Mat44f) SetViewport(tl Vec3f, br Vec3f) { //width, height, depth int) {
	m.ZeroFill()
	m.Set(0, 0, (br.X-tl.X)/2)
	m.Set(1, 1, (tl.Y-br.Y)/2) // Inverted because screen funny
	m.Set(2, 2, (br.Z-tl.Z)/2)
	m.Set(3, 3, 1)
	m.Set(0, 3, (br.X+tl.X)/2)
	m.Set(1, 3, (br.Y+tl.Y)/2)
	m.Set(2, 3, (br.Z+tl.Z)/2)
}

func (m *Mat44f) SetViewportSimple(width, height, depth int) {
	var tl Vec3f // All zero
	br := Vec3f{
		X: float32(width),
		Y: float32(height),
		Z: float32(depth),
	}
	m.SetViewport(tl, br)
}

// Multiply the given point by our vector. Remember this is row-major order
func (m *Mat44f) MultiplyPoint3(p Vec3f) Vec3f {
	var out Vec3f
	// We hope very much that Go will optimize the function calls for us,
	// along with computing the constants.
	out.X = p.X*m.Get(0, 0) + p.Y*m.Get(0, 1) + p.Z*m.Get(0, 2) + m.Get(0, 3)
	out.Y = p.X*m.Get(1, 0) + p.Y*m.Get(1, 1) + p.Z*m.Get(1, 2) + m.Get(1, 3)
	out.Z = p.X*m.Get(2, 0) + p.Y*m.Get(2, 1) + p.Z*m.Get(2, 2) + m.Get(2, 3)
	w := p.X*m.Get(3, 0) + p.Y*m.Get(3, 1) + p.Z*m.Get(3, 2) + m.Get(3, 3)
	if w != 1 {
		out.X /= w
		out.Y /= w
		out.Z /= w
	}
	return out
}

// Multiply two 4x4 matrices together (not optimized). May
// mess with garbage collector?? IDK
func (m *Mat44f) Multiply(m2 *Mat44f) *Mat44f {
	var result Mat44f
	// This is the x and y of our resulting matrix
	for y := 0; y < 4; y++ {
		for x := 0; x < 4; x++ {
			for i := 0; i < 4; i++ {
				result[x+y*4] += m[i+y*4] * m2[x+i*4]
			}
		}
	}
	return &result
}

// Multiply two matrices, storing the result in the first one
// func (m *Mat44f) MultiplyInto(m2 *Mat44f) {
// 	var orig Mat44f
// 	for i := 0; i < 16; i++ {
//     orig[i] = m[i]
//   }
// 	// This is the x and y of our resulting matrix
// 	for y := 0; y < 4; y++ {
// 		for x := 0; x < 4; x++ {
//       m[x+y*4] = 0
// 			for i := 0; i < 4; i++ {
// 				m[x+y*4] += orig[i+y*4] * m2[x+i*4]
// 			}
// 		}
// 	}
// 	return &result
// }

func (vi *Vec2i) ToF() Vec2f {
	return Vec2f{float32(vi.X), float32(vi.Y)}
}

func (vi *Vec3f) ToVec2i() Vec2i {
	return Vec2i{int(vi.X), int(vi.Y)}
}

func (v0 *Vec3f) Add(v1 *Vec3f) *Vec3f {
	return &Vec3f{
		X: v0.X + v1.X,
		Y: v0.Y + v1.Y,
		Z: v0.Z + v1.Z,
	}
}

func (v0 *Vec3f) Sub(v1 *Vec3f) *Vec3f {
	return &Vec3f{
		X: v0.X - v1.X,
		Y: v0.Y - v1.Y,
		Z: v0.Z - v1.Z,
	}
}

func (v0 *Vec3f) CrossProduct(v1 *Vec3f) *Vec3f {
	return &Vec3f{
		X: v0.Y*v1.Z - v0.Z*v1.Y,
		Y: v0.Z*v1.X - v0.X*v1.Z,
		Z: v0.X*v1.Y - v0.Y*v1.X,
	}
}

//func (v

func (v *Vec3f) Normalize() *Vec3f {
	l := float32(math.Sqrt(float64(v.MultSimp(v))))
	return &Vec3f{
		X: v.X / l,
		Y: v.Y / l,
		Z: v.Z / l,
	}
}

func (v0 *Vec3f) MultSimp(v1 *Vec3f) float32 {
	return v0.X*v1.X + v0.Y*v1.Y + v0.Z*v1.Z
}