package fixed

import "math/bits"

// Vec is a 3D fixed-point vector.
type Vec struct{ X, Y, Z F }

func (a Vec) Add(b Vec) Vec { return Vec{a.X + b.X, a.Y + b.Y, a.Z + b.Z} }
func (a Vec) Sub(b Vec) Vec { return Vec{a.X - b.X, a.Y - b.Y, a.Z - b.Z} }
func (a Vec) Scale(k F) Vec { return Vec{a.X.Mul(k), a.Y.Mul(k), a.Z.Mul(k)} }

// Len returns the vector magnitude. The squares are accumulated in 128 bits
// (three squares of 63-bit values cannot overflow that), so it is exact to the
// last bit over the whole F range.
func (a Vec) Len() F { return Norm3(a.X, a.Y, a.Z) }

// Hypot returns sqrt(x*x + y*y) without intermediate overflow.
func Hypot(x, y F) F { return Norm3(x, y, 0) }

// Norm3 returns sqrt(x*x + y*y + z*z) without intermediate overflow.
func Norm3(x, y, z F) F {
	h1, l1 := bits.Mul64(uabs(x), uabs(x))
	h2, l2 := bits.Mul64(uabs(y), uabs(y))
	h3, l3 := bits.Mul64(uabs(z), uabs(z))
	lo, c := bits.Add64(l1, l2, 0)
	hi, _ := bits.Add64(h1, h2, c)
	lo, c = bits.Add64(lo, l3, 0)
	hi, _ = bits.Add64(hi, h3, c)
	return F(isqrt128(hi, lo))
}

// Unit returns the unit vector, or zero for the zero vector.
func (a Vec) Unit() Vec {
	l := a.Len()
	if l == 0 {
		return Vec{}
	}
	return Vec{a.X.Div(l), a.Y.Div(l), a.Z.Div(l)}
}

// FromSpherical returns the unit vector with the given azimuth (angle from +X
// in the XY plane) and elevation (angle above the XY plane).
func FromSpherical(azimuth, elevation F) Vec {
	sa, ca := SinCos(azimuth)
	se, ce := SinCos(elevation)
	return Vec{ce.Mul(ca), ce.Mul(sa), se}
}

// RotateZ rotates counter-clockwise about the Z axis.
func (a Vec) RotateZ(ang F) Vec {
	s, c := SinCos(ang)
	return Vec{a.X.Mul(c) - a.Y.Mul(s), a.X.Mul(s) + a.Y.Mul(c), a.Z}
}

// RotateX rotates counter-clockwise about the X axis.
func (a Vec) RotateX(ang F) Vec {
	s, c := SinCos(ang)
	return Vec{a.X, a.Y.Mul(c) - a.Z.Mul(s), a.Y.Mul(s) + a.Z.Mul(c)}
}

// Dot returns the dot product, saturating on overflow.
func (a Vec) Dot(b Vec) F { return a.X.Mul(b.X) + a.Y.Mul(b.Y) + a.Z.Mul(b.Z) }