eq2go/internal/zone/position.go

package zone

import (
	"math"
)

// Position represents a 3D position with heading
type Position struct {
	X       float32
	Y       float32
	Z       float32
	Heading float32
}

// Position2D represents a 2D position
type Position2D struct {
	X float32
	Y float32
}

// BoundingBox represents an axis-aligned bounding box
type BoundingBox struct {
	MinX float32
	MinY float32
	MinZ float32
	MaxX float32
	MaxY float32
	MaxZ float32
}

// Cylinder represents a cylindrical collision shape
type Cylinder struct {
	X      float32
	Y      float32
	Z      float32
	Radius float32
	Height float32
}

const (
	// EQ2HeadingMax represents the maximum heading value in EQ2 (512 = full circle)
	EQ2HeadingMax = 512.0

	// DefaultEpsilon for floating point comparisons
	DefaultEpsilon = 0.0001

	// DegreesToRadians conversion factor
	DegreesToRadians = math.Pi / 180.0

	// RadiansToDegrees conversion factor
	RadiansToDegrees = 180.0 / math.Pi
)

// NewPosition creates a new position with the given coordinates and heading
func NewPosition(x, y, z, heading float32) *Position {
	return &Position{
		X:       x,
		Y:       y,
		Z:       z,
		Heading: heading,
	}
}

// NewPosition2D creates a new 2D position
func NewPosition2D(x, y float32) *Position2D {
	return &Position2D{
		X: x,
		Y: y,
	}
}

// NewBoundingBox creates a new bounding box with the given bounds
func NewBoundingBox(minX, minY, minZ, maxX, maxY, maxZ float32) *BoundingBox {
	return &BoundingBox{
		MinX: minX,
		MinY: minY,
		MinZ: minZ,
		MaxX: maxX,
		MaxY: maxY,
		MaxZ: maxZ,
	}
}

// NewCylinder creates a new cylinder with the given parameters
func NewCylinder(x, y, z, radius, height float32) *Cylinder {
	return &Cylinder{
		X:      x,
		Y:      y,
		Z:      z,
		Radius: radius,
		Height: height,
	}
}

// Copy creates a copy of the position
func (p *Position) Copy() *Position {
	return &Position{
		X:       p.X,
		Y:       p.Y,
		Z:       p.Z,
		Heading: p.Heading,
	}
}

// Set updates the position with new values
func (p *Position) Set(x, y, z, heading float32) {
	p.X = x
	p.Y = y
	p.Z = z
	p.Heading = heading
}

// SetXYZ updates only the coordinates, leaving heading unchanged
func (p *Position) SetXYZ(x, y, z float32) {
	p.X = x
	p.Y = y
	p.Z = z
}

// SetHeading updates only the heading
func (p *Position) SetHeading(heading float32) {
	p.Heading = heading
}

// Distance2D calculates the 2D distance to another position (ignoring Z)
func Distance2D(x1, y1, x2, y2 float32) float32 {
	dx := x2 - x1
	dy := y2 - y1
	return float32(math.Sqrt(float64(dx*dx + dy*dy)))
}

// Distance2DSquared calculates the squared 2D distance (more efficient, avoids sqrt)
func Distance2DSquared(x1, y1, x2, y2 float32) float32 {
	dx := x2 - x1
	dy := y2 - y1
	return dx*dx + dy*dy
}

// Distance3D calculates the 3D distance between two points
func Distance3D(x1, y1, z1, x2, y2, z2 float32) float32 {
	dx := x2 - x1
	dy := y2 - y1
	dz := z2 - z1
	return float32(math.Sqrt(float64(dx*dx + dy*dy + dz*dz)))
}

// Distance3DSquared calculates the squared 3D distance (more efficient, avoids sqrt)
func Distance3DSquared(x1, y1, z1, x2, y2, z2 float32) float32 {
	dx := x2 - x1
	dy := y2 - y1
	dz := z2 - z1
	return dx*dx + dy*dy + dz*dz
}

// Distance4D calculates the 4D distance including heading difference
func Distance4D(x1, y1, z1, h1, x2, y2, z2, h2 float32) float32 {
	dx := x2 - x1
	dy := y2 - y1
	dz := z2 - z1
	dh := HeadingDifference(h1, h2)
	return float32(math.Sqrt(float64(dx*dx + dy*dy + dz*dz + dh*dh)))
}

// DistanceTo2D calculates the 2D distance to another position
func (p *Position) DistanceTo2D(other *Position) float32 {
	return Distance2D(p.X, p.Y, other.X, other.Y)
}

// DistanceTo3D calculates the 3D distance to another position
func (p *Position) DistanceTo3D(other *Position) float32 {
	return Distance3D(p.X, p.Y, p.Z, other.X, other.Y, other.Z)
}

// DistanceTo4D calculates the 4D distance to another position including heading
func (p *Position) DistanceTo4D(other *Position) float32 {
	return Distance4D(p.X, p.Y, p.Z, p.Heading, other.X, other.Y, other.Z, other.Heading)
}

// CalculateHeading calculates the heading from current position to target position
func (p *Position) CalculateHeading(targetX, targetY float32) float32 {
	return CalculateHeading(p.X, p.Y, targetX, targetY)
}

// CalculateHeading calculates the EQ2 heading from one point to another
func CalculateHeading(fromX, fromY, toX, toY float32) float32 {
	dx := toX - fromX
	dy := toY - fromY

	if dx == 0 && dy == 0 {
		return 0.0
	}

	// Calculate angle in radians
	angle := math.Atan2(float64(dx), float64(dy))

	// Convert to EQ2 heading (0-512 scale, 0 = north)
	heading := angle * (EQ2HeadingMax / (2 * math.Pi))

	// Ensure positive value
	if heading < 0 {
		heading += EQ2HeadingMax
	}

	return float32(heading)
}

// HeadingToRadians converts an EQ2 heading to radians
func HeadingToRadians(heading float32) float32 {
	return heading * (2 * math.Pi / EQ2HeadingMax)
}

// RadiansToHeading converts radians to an EQ2 heading
func RadiansToHeading(radians float32) float32 {
	heading := radians * (EQ2HeadingMax / (2 * math.Pi))
	if heading < 0 {
		heading += EQ2HeadingMax
	}
	return heading
}

// HeadingToRadiansDegrees converts an EQ2 heading to degrees
func HeadingToDegrees(heading float32) float32 {
	return heading * (360.0 / EQ2HeadingMax)
}

// DegreesToHeading converts degrees to an EQ2 heading
func DegreesToHeading(degrees float32) float32 {
	heading := degrees * (EQ2HeadingMax / 360.0)
	if heading < 0 {
		heading += EQ2HeadingMax
	}
	return heading
}

// NormalizeHeading ensures a heading is in the valid range [0, 512)
func NormalizeHeading(heading float32) float32 {
	for heading < 0 {
		heading += EQ2HeadingMax
	}
	for heading >= EQ2HeadingMax {
		heading -= EQ2HeadingMax
	}
	return heading
}

// HeadingDifference calculates the shortest angular difference between two headings
func HeadingDifference(heading1, heading2 float32) float32 {
	diff := heading2 - heading1

	// Normalize to [-256, 256] range (half circle)
	for diff > EQ2HeadingMax/2 {
		diff -= EQ2HeadingMax
	}
	for diff < -EQ2HeadingMax/2 {
		diff += EQ2HeadingMax
	}

	return diff
}

// GetReciprocalHeading calculates the opposite heading (180 degree turn)
func GetReciprocalHeading(heading float32) float32 {
	reciprocal := heading + EQ2HeadingMax/2
	if reciprocal >= EQ2HeadingMax {
		reciprocal -= EQ2HeadingMax
	}
	return reciprocal
}

// Equals compares two positions with epsilon tolerance
func (p *Position) Equals(other *Position) bool {
	return EqualsWithEpsilon(p.X, other.X, DefaultEpsilon) &&
		EqualsWithEpsilon(p.Y, other.Y, DefaultEpsilon) &&
		EqualsWithEpsilon(p.Z, other.Z, DefaultEpsilon) &&
		EqualsWithEpsilon(p.Heading, other.Heading, DefaultEpsilon)
}

// EqualsXYZ compares only the coordinates (ignoring heading)
func (p *Position) EqualsXYZ(other *Position) bool {
	return EqualsWithEpsilon(p.X, other.X, DefaultEpsilon) &&
		EqualsWithEpsilon(p.Y, other.Y, DefaultEpsilon) &&
		EqualsWithEpsilon(p.Z, other.Z, DefaultEpsilon)
}

// EqualsWithEpsilon compares two float values with the given epsilon tolerance
func EqualsWithEpsilon(a, b, epsilon float32) bool {
	diff := a - b
	if diff < 0 {
		diff = -diff
	}
	return diff < epsilon
}

// Contains checks if a point is inside the bounding box
func (bb *BoundingBox) Contains(x, y, z float32) bool {
	return x >= bb.MinX && x <= bb.MaxX &&
		y >= bb.MinY && y <= bb.MaxY &&
		z >= bb.MinZ && z <= bb.MaxZ
}

// ContainsPosition checks if a position is inside the bounding box
func (bb *BoundingBox) ContainsPosition(pos *Position) bool {
	return bb.Contains(pos.X, pos.Y, pos.Z)
}

// Intersects checks if this bounding box intersects with another
func (bb *BoundingBox) Intersects(other *BoundingBox) bool {
	return bb.MinX <= other.MaxX && bb.MaxX >= other.MinX &&
		bb.MinY <= other.MaxY && bb.MaxY >= other.MinY &&
		bb.MinZ <= other.MaxZ && bb.MaxZ >= other.MinZ
}

// Expand expands the bounding box by the given amount in all directions
func (bb *BoundingBox) Expand(amount float32) {
	bb.MinX -= amount
	bb.MinY -= amount
	bb.MinZ -= amount
	bb.MaxX += amount
	bb.MaxY += amount
	bb.MaxZ += amount
}

// GetCenter returns the center point of the bounding box
func (bb *BoundingBox) GetCenter() *Position {
	return &Position{
		X: (bb.MinX + bb.MaxX) / 2,
		Y: (bb.MinY + bb.MaxY) / 2,
		Z: (bb.MinZ + bb.MaxZ) / 2,
	}
}

// GetSize returns the size of the bounding box in each dimension
func (bb *BoundingBox) GetSize() (width, height, depth float32) {
	return bb.MaxX - bb.MinX, bb.MaxY - bb.MinY, bb.MaxZ - bb.MinZ
}

// Contains2D checks if a 2D point is inside the cylinder (ignoring height)
func (c *Cylinder) Contains2D(x, y float32) bool {
	return Distance2DSquared(c.X, c.Y, x, y) <= c.Radius*c.Radius
}

// Contains3D checks if a 3D point is inside the cylinder
func (c *Cylinder) Contains3D(x, y, z float32) bool {
	if z < c.Z || z > c.Z+c.Height {
		return false
	}
	return c.Contains2D(x, y)
}

// ContainsPosition checks if a position is inside the cylinder
func (c *Cylinder) ContainsPosition(pos *Position) bool {
	return c.Contains3D(pos.X, pos.Y, pos.Z)
}

// DistanceToEdge2D calculates the 2D distance from a point to the cylinder edge
func (c *Cylinder) DistanceToEdge2D(x, y float32) float32 {
	distance := Distance2D(c.X, c.Y, x, y)
	return distance - c.Radius
}

// GetBoundingBox returns a bounding box that encompasses the cylinder
func (c *Cylinder) GetBoundingBox() *BoundingBox {
	return &BoundingBox{
		MinX: c.X - c.Radius,
		MinY: c.Y - c.Radius,
		MinZ: c.Z,
		MaxX: c.X + c.Radius,
		MaxY: c.Y + c.Radius,
		MaxZ: c.Z + c.Height,
	}
}

// Copy creates a copy of the 2D position
func (p *Position2D) Copy() *Position2D {
	return &Position2D{
		X: p.X,
		Y: p.Y,
	}
}

// DistanceTo calculates the distance to another 2D position
func (p *Position2D) DistanceTo(other *Position2D) float32 {
	return Distance2D(p.X, p.Y, other.X, other.Y)
}

// DistanceToSquared calculates the squared distance to another 2D position
func (p *Position2D) DistanceToSquared(other *Position2D) float32 {
	return Distance2DSquared(p.X, p.Y, other.X, other.Y)
}

// Equals compares two 2D positions with epsilon tolerance
func (p *Position2D) Equals(other *Position2D) bool {
	return EqualsWithEpsilon(p.X, other.X, DefaultEpsilon) &&
		EqualsWithEpsilon(p.Y, other.Y, DefaultEpsilon)
}

// InterpolateLinear performs linear interpolation between two positions
func InterpolateLinear(from, to *Position, t float32) *Position {
	if t <= 0 {
		return from.Copy()
	}
	if t >= 1 {
		return to.Copy()
	}

	return &Position{
		X:       from.X + (to.X-from.X)*t,
		Y:       from.Y + (to.Y-from.Y)*t,
		Z:       from.Z + (to.Z-from.Z)*t,
		Heading: InterpolateHeading(from.Heading, to.Heading, t),
	}
}

// InterpolateHeading performs interpolation between two headings, taking the shortest path
func InterpolateHeading(from, to, t float32) float32 {
	diff := HeadingDifference(from, to)
	result := from + diff*t
	return NormalizeHeading(result)
}

// GetRandomPositionInRadius generates a random position within the given radius
func GetRandomPositionInRadius(centerX, centerY, centerZ float32, radius float32) *Position {
	// Generate random angle
	angle := float32(math.Random() * 2 * math.Pi)

	// Generate random distance (uniform distribution in circle)
	distance := float32(math.Sqrt(math.Random())) * radius

	// Calculate new position
	x := centerX + distance*float32(math.Cos(float64(angle)))
	y := centerY + distance*float32(math.Sin(float64(angle)))

	return &Position{
		X:       x,
		Y:       y,
		Z:       centerZ,
		Heading: 0,
	}
}

// IsWithinRange checks if two positions are within the specified range
func IsWithinRange(pos1, pos2 *Position, maxRange float32) bool {
	return Distance3DSquared(pos1.X, pos1.Y, pos1.Z, pos2.X, pos2.Y, pos2.Z) <= maxRange*maxRange
}

// IsWithinRange2D checks if two positions are within the specified 2D range
func IsWithinRange2D(pos1, pos2 *Position, maxRange float32) bool {
	return Distance2DSquared(pos1.X, pos1.Y, pos2.X, pos2.Y) <= maxRange*maxRange
}

// ClampToRange clamps a distance to be within the specified range
func ClampToRange(distance, minRange, maxRange float32) float32 {
	if distance < minRange {
		return minRange
	}
	if distance > maxRange {
		return maxRange
	}
	return distance
}

// GetDirectionVector calculates a normalized direction vector from one position to another
func GetDirectionVector(from, to *Position) (dx, dy, dz float32) {
	dx = to.X - from.X
	dy = to.Y - from.Y
	dz = to.Z - from.Z

	// Normalize
	length := float32(math.Sqrt(float64(dx*dx + dy*dy + dz*dz)))
	if length > 0 {
		dx /= length
		dy /= length
		dz /= length
	}

	return dx, dy, dz
}

// MoveTowards moves a position towards a target by the specified distance
func MoveTowards(from, to *Position, distance float32) *Position {
	dx, dy, dz := GetDirectionVector(from, to)

	return &Position{
		X:       from.X + dx*distance,
		Y:       from.Y + dy*distance,
		Z:       from.Z + dz*distance,
		Heading: from.Heading,
	}
}