package raycast

import (
	"fmt"
	"math"
	"sync"
)

// RaycastMesh provides high-speed raycasting against triangle meshes using AABB trees
// This is a Go implementation based on the C++ raycast mesh system
type RaycastMesh struct {
	vertices    []float32         // Vertex positions (x,y,z,x,y,z,...)
	indices     []uint32          // Triangle indices (i1,i2,i3,i4,i5,i6,...)
	grids       []uint32          // Grid IDs for each triangle
	widgets     []uint32          // Widget IDs for each triangle
	triangles   []*Triangle       // Processed triangles
	root        *AABBNode         // Root of the AABB tree
	boundMin    [3]float32        // Minimum bounding box
	boundMax    [3]float32        // Maximum bounding box
	maxDepth    uint32            // Maximum tree depth
	minLeafSize uint32            // Minimum triangles per leaf
	minAxisSize float32           // Minimum axis size for subdivision
	mutex       sync.RWMutex
}

// Triangle represents a single triangle in the mesh
type Triangle struct {
	Vertices   [9]float32 // 3 vertices * 3 components (x,y,z)
	Normal     [3]float32 // Face normal
	GridID     uint32     // Associated grid ID
	WidgetID   uint32     // Associated widget ID
	BoundMin   [3]float32 // Triangle bounding box minimum
	BoundMax   [3]float32 // Triangle bounding box maximum
}

// AABBNode represents a node in the Axis-Aligned Bounding Box tree
type AABBNode struct {
	BoundMin  [3]float32   // Node bounding box minimum
	BoundMax  [3]float32   // Node bounding box maximum
	Left      *AABBNode    // Left child (nil for leaf nodes)
	Right     *AABBNode    // Right child (nil for leaf nodes)
	Triangles []*Triangle  // Triangles (only for leaf nodes)
	Depth     uint32       // Tree depth
}

// RaycastResult contains the results of a raycast operation
type RaycastResult struct {
	Hit         bool       // Whether the ray hit something
	HitLocation [3]float32 // World coordinates of hit point
	HitNormal   [3]float32 // Surface normal at hit point
	HitDistance float32    // Distance from ray origin to hit
	GridID      uint32     // Grid ID of hit triangle
	WidgetID    uint32     // Widget ID of hit triangle
}

// RaycastOptions configures raycast behavior
type RaycastOptions struct {
	IgnoredWidgets map[uint32]bool // Widget IDs to ignore during raycast
	MaxDistance    float32         // Maximum ray distance (0 = unlimited)
	BothSides      bool            // Check both sides of triangles
}

// NewRaycastMesh creates a new raycast mesh from triangle data
func NewRaycastMesh(vertices []float32, indices []uint32, grids []uint32, widgets []uint32, 
	maxDepth uint32, minLeafSize uint32, minAxisSize float32) (*RaycastMesh, error) {
	
	if len(vertices)%3 != 0 {
		return nil, fmt.Errorf("vertex count must be divisible by 3")
	}
	if len(indices)%3 != 0 {
		return nil, fmt.Errorf("index count must be divisible by 3")
	}
	
	triangleCount := len(indices) / 3
	if len(grids) != triangleCount {
		return nil, fmt.Errorf("grid count must match triangle count")
	}
	if len(widgets) != triangleCount {
		return nil, fmt.Errorf("widget count must match triangle count")
	}
	
	rm := &RaycastMesh{
		vertices:    make([]float32, len(vertices)),
		indices:     make([]uint32, len(indices)),
		grids:       make([]uint32, len(grids)),
		widgets:     make([]uint32, len(widgets)),
		triangles:   make([]*Triangle, triangleCount),
		maxDepth:    maxDepth,
		minLeafSize: minLeafSize,
		minAxisSize: minAxisSize,
	}
	
	// Copy input data
	copy(rm.vertices, vertices)
	copy(rm.indices, indices)
	copy(rm.grids, grids)
	copy(rm.widgets, widgets)
	
	// Process triangles
	if err := rm.processTriangles(); err != nil {
		return nil, fmt.Errorf("failed to process triangles: %v", err)
	}
	
	// Build AABB tree
	if err := rm.buildAABBTree(); err != nil {
		return nil, fmt.Errorf("failed to build AABB tree: %v", err)
	}
	
	return rm, nil
}

// Raycast performs optimized raycasting using the AABB tree
func (rm *RaycastMesh) Raycast(from, to [3]float32, options *RaycastOptions) *RaycastResult {
	rm.mutex.RLock()
	defer rm.mutex.RUnlock()
	
	if options == nil {
		options = &RaycastOptions{}
	}
	
	result := &RaycastResult{
		Hit: false,
		HitDistance: math.MaxFloat32,
	}
	
	// Calculate ray direction and length
	rayDir := [3]float32{
		to[0] - from[0],
		to[1] - from[1],
		to[2] - from[2],
	}
	
	rayLength := vectorLength(rayDir)
	if rayLength < 1e-6 {
		return result // Zero-length ray
	}
	
	// Normalize ray direction
	rayDir[0] /= rayLength
	rayDir[1] /= rayLength
	rayDir[2] /= rayLength
	
	// Use max distance if specified
	maxDist := rayLength
	if options.MaxDistance > 0 && options.MaxDistance < rayLength {
		maxDist = options.MaxDistance
	}
	
	// Traverse AABB tree
	rm.raycastNode(rm.root, from, rayDir, maxDist, options, result)
	
	return result
}

// BruteForceRaycast performs raycast without spatial optimization (for testing/comparison)
func (rm *RaycastMesh) BruteForceRaycast(from, to [3]float32, options *RaycastOptions) *RaycastResult {
	rm.mutex.RLock()
	defer rm.mutex.RUnlock()
	
	if options == nil {
		options = &RaycastOptions{}
	}
	
	result := &RaycastResult{
		Hit: false,
		HitDistance: math.MaxFloat32,
	}
	
	rayDir := [3]float32{
		to[0] - from[0],
		to[1] - from[1],
		to[2] - from[2],
	}
	
	rayLength := vectorLength(rayDir)
	if rayLength < 1e-6 {
		return result
	}
	
	rayDir[0] /= rayLength
	rayDir[1] /= rayLength
	rayDir[2] /= rayLength
	
	maxDist := rayLength
	if options.MaxDistance > 0 && options.MaxDistance < rayLength {
		maxDist = options.MaxDistance
	}
	
	// Test all triangles
	for _, triangle := range rm.triangles {
		if options.IgnoredWidgets != nil && options.IgnoredWidgets[triangle.WidgetID] {
			continue
		}
		
		if hitDist, hitPoint, hitNormal := rm.rayTriangleIntersect(from, rayDir, triangle, options.BothSides); hitDist >= 0 && hitDist <= maxDist && hitDist < result.HitDistance {
			result.Hit = true
			result.HitDistance = hitDist
			result.HitLocation = hitPoint
			result.HitNormal = hitNormal
			result.GridID = triangle.GridID
			result.WidgetID = triangle.WidgetID
		}
	}
	
	return result
}

// GetBoundMin returns the minimum bounding box coordinates
func (rm *RaycastMesh) GetBoundMin() [3]float32 {
	rm.mutex.RLock()
	defer rm.mutex.RUnlock()
	return rm.boundMin
}

// GetBoundMax returns the maximum bounding box coordinates
func (rm *RaycastMesh) GetBoundMax() [3]float32 {
	rm.mutex.RLock()
	defer rm.mutex.RUnlock()
	return rm.boundMax
}

// GetTriangleCount returns the number of triangles in the mesh
func (rm *RaycastMesh) GetTriangleCount() int {
	rm.mutex.RLock()
	defer rm.mutex.RUnlock()
	return len(rm.triangles)
}

// GetTreeDepth returns the actual depth of the AABB tree
func (rm *RaycastMesh) GetTreeDepth() uint32 {
	rm.mutex.RLock()
	defer rm.mutex.RUnlock()
	if rm.root == nil {
		return 0
	}
	return rm.getNodeDepth(rm.root)
}

// Private methods

func (rm *RaycastMesh) processTriangles() error {
	rm.boundMin = [3]float32{math.MaxFloat32, math.MaxFloat32, math.MaxFloat32}
	rm.boundMax = [3]float32{-math.MaxFloat32, -math.MaxFloat32, -math.MaxFloat32}
	
	for i := 0; i < len(rm.indices); i += 3 {
		triangle := &Triangle{
			GridID:   rm.grids[i/3],
			WidgetID: rm.widgets[i/3],
		}
		
		// Get vertex indices
		i1, i2, i3 := rm.indices[i], rm.indices[i+1], rm.indices[i+2]
		
		// Validate indices
		if i1*3+2 >= uint32(len(rm.vertices)) || i2*3+2 >= uint32(len(rm.vertices)) || i3*3+2 >= uint32(len(rm.vertices)) {
			return fmt.Errorf("invalid vertex index in triangle %d", i/3)
		}
		
		// Copy vertex positions
		copy(triangle.Vertices[0:3], rm.vertices[i1*3:i1*3+3])
		copy(triangle.Vertices[3:6], rm.vertices[i2*3:i2*3+3])
		copy(triangle.Vertices[6:9], rm.vertices[i3*3:i3*3+3])
		
		// Calculate triangle bounding box
		triangle.BoundMin = [3]float32{math.MaxFloat32, math.MaxFloat32, math.MaxFloat32}
		triangle.BoundMax = [3]float32{-math.MaxFloat32, -math.MaxFloat32, -math.MaxFloat32}
		
		for j := 0; j < 9; j += 3 {
			for k := 0; k < 3; k++ {
				if triangle.Vertices[j+k] < triangle.BoundMin[k] {
					triangle.BoundMin[k] = triangle.Vertices[j+k]
				}
				if triangle.Vertices[j+k] > triangle.BoundMax[k] {
					triangle.BoundMax[k] = triangle.Vertices[j+k]
				}
			}
		}
		
		// Update global bounding box
		for k := 0; k < 3; k++ {
			if triangle.BoundMin[k] < rm.boundMin[k] {
				rm.boundMin[k] = triangle.BoundMin[k]
			}
			if triangle.BoundMax[k] > rm.boundMax[k] {
				rm.boundMax[k] = triangle.BoundMax[k]
			}
		}
		
		// Calculate face normal
		rm.calculateTriangleNormal(triangle)
		
		rm.triangles[i/3] = triangle
	}
	
	return nil
}

func (rm *RaycastMesh) calculateTriangleNormal(triangle *Triangle) {
	// Calculate two edge vectors
	edge1 := [3]float32{
		triangle.Vertices[3] - triangle.Vertices[0],
		triangle.Vertices[4] - triangle.Vertices[1],
		triangle.Vertices[5] - triangle.Vertices[2],
	}
	
	edge2 := [3]float32{
		triangle.Vertices[6] - triangle.Vertices[0],
		triangle.Vertices[7] - triangle.Vertices[1],
		triangle.Vertices[8] - triangle.Vertices[2],
	}
	
	// Cross product
	triangle.Normal[0] = edge1[1]*edge2[2] - edge1[2]*edge2[1]
	triangle.Normal[1] = edge1[2]*edge2[0] - edge1[0]*edge2[2]
	triangle.Normal[2] = edge1[0]*edge2[1] - edge1[1]*edge2[0]
	
	// Normalize
	length := vectorLength(triangle.Normal)
	if length > 1e-6 {
		triangle.Normal[0] /= length
		triangle.Normal[1] /= length
		triangle.Normal[2] /= length
	}
}

func (rm *RaycastMesh) buildAABBTree() error {
	if len(rm.triangles) == 0 {
		return fmt.Errorf("no triangles to build tree from")
	}
	
	// Create root node with all triangles
	rm.root = &AABBNode{
		BoundMin:  rm.boundMin,
		BoundMax:  rm.boundMax,
		Triangles: rm.triangles,
		Depth:     0,
	}
	
	// Recursively subdivide
	rm.subdivideNode(rm.root)
	
	return nil
}

func (rm *RaycastMesh) subdivideNode(node *AABBNode) {
	// Stop subdivision if we've reached limits
	if node.Depth >= rm.maxDepth || uint32(len(node.Triangles)) <= rm.minLeafSize {
		return
	}
	
	// Find longest axis
	size := [3]float32{
		node.BoundMax[0] - node.BoundMin[0],
		node.BoundMax[1] - node.BoundMin[1],
		node.BoundMax[2] - node.BoundMin[2],
	}
	
	axis := 0
	if size[1] > size[axis] {
		axis = 1
	}
	if size[2] > size[axis] {
		axis = 2
	}
	
	// Stop if axis is too small
	if size[axis] < rm.minAxisSize {
		return
	}
	
	// Split at midpoint
	split := node.BoundMin[axis] + size[axis]*0.5
	
	// Partition triangles
	var leftTriangles, rightTriangles []*Triangle
	for _, triangle := range node.Triangles {
		center := (triangle.BoundMin[axis] + triangle.BoundMax[axis]) * 0.5
		if center < split {
			leftTriangles = append(leftTriangles, triangle)
		} else {
			rightTriangles = append(rightTriangles, triangle)
		}
	}
	
	// Make sure both sides have triangles
	if len(leftTriangles) == 0 || len(rightTriangles) == 0 {
		return
	}
	
	// Create child nodes
	node.Left = &AABBNode{
		Triangles: leftTriangles,
		Depth:     node.Depth + 1,
	}
	node.Right = &AABBNode{
		Triangles: rightTriangles,
		Depth:     node.Depth + 1,
	}
	
	// Calculate child bounding boxes
	rm.calculateNodeBounds(node.Left)
	rm.calculateNodeBounds(node.Right)
	
	// Clear triangles from internal node
	node.Triangles = nil
	
	// Recursively subdivide children
	rm.subdivideNode(node.Left)
	rm.subdivideNode(node.Right)
}

func (rm *RaycastMesh) calculateNodeBounds(node *AABBNode) {
	if len(node.Triangles) == 0 {
		return
	}
	
	node.BoundMin = [3]float32{math.MaxFloat32, math.MaxFloat32, math.MaxFloat32}
	node.BoundMax = [3]float32{-math.MaxFloat32, -math.MaxFloat32, -math.MaxFloat32}
	
	for _, triangle := range node.Triangles {
		for k := 0; k < 3; k++ {
			if triangle.BoundMin[k] < node.BoundMin[k] {
				node.BoundMin[k] = triangle.BoundMin[k]
			}
			if triangle.BoundMax[k] > node.BoundMax[k] {
				node.BoundMax[k] = triangle.BoundMax[k]
			}
		}
	}
}

func (rm *RaycastMesh) raycastNode(node *AABBNode, rayOrigin, rayDir [3]float32, maxDist float32, 
	options *RaycastOptions, result *RaycastResult) {
	
	if node == nil {
		return
	}
	
	// Test ray against node bounding box
	if !rm.rayAABBIntersect(rayOrigin, rayDir, node.BoundMin, node.BoundMax, maxDist) {
		return
	}
	
	// Leaf node - test triangles
	if node.Left == nil && node.Right == nil {
		for _, triangle := range node.Triangles {
			if options.IgnoredWidgets != nil && options.IgnoredWidgets[triangle.WidgetID] {
				continue
			}
			
			if hitDist, hitPoint, hitNormal := rm.rayTriangleIntersect(rayOrigin, rayDir, triangle, options.BothSides); hitDist >= 0 && hitDist <= maxDist && hitDist < result.HitDistance {
				result.Hit = true
				result.HitDistance = hitDist
				result.HitLocation = hitPoint
				result.HitNormal = hitNormal
				result.GridID = triangle.GridID
				result.WidgetID = triangle.WidgetID
			}
		}
	} else {
		// Internal node - recurse to children
		rm.raycastNode(node.Left, rayOrigin, rayDir, maxDist, options, result)
		rm.raycastNode(node.Right, rayOrigin, rayDir, maxDist, options, result)
	}
}

func (rm *RaycastMesh) rayAABBIntersect(rayOrigin, rayDir [3]float32, boundMin, boundMax [3]float32, maxDist float32) bool {
	var tMin, tMax float32 = 0, maxDist
	
	for i := 0; i < 3; i++ {
		if math.Abs(float64(rayDir[i])) < 1e-6 {
			// Ray is parallel to axis
			if rayOrigin[i] < boundMin[i] || rayOrigin[i] > boundMax[i] {
				return false
			}
		} else {
			// Calculate intersection distances
			invDir := 1.0 / rayDir[i]
			t1 := (boundMin[i] - rayOrigin[i]) * invDir
			t2 := (boundMax[i] - rayOrigin[i]) * invDir
			
			if t1 > t2 {
				t1, t2 = t2, t1
			}
			
			tMin = float32(math.Max(float64(tMin), float64(t1)))
			tMax = float32(math.Min(float64(tMax), float64(t2)))
			
			if tMin > tMax {
				return false
			}
		}
	}
	
	return tMin <= maxDist
}

func (rm *RaycastMesh) rayTriangleIntersect(rayOrigin, rayDir [3]float32, triangle *Triangle, bothSides bool) (float32, [3]float32, [3]float32) {
	// Möller-Trumbore ray-triangle intersection algorithm
	
	// Get triangle vertices
	v0 := [3]float32{triangle.Vertices[0], triangle.Vertices[1], triangle.Vertices[2]}
	v1 := [3]float32{triangle.Vertices[3], triangle.Vertices[4], triangle.Vertices[5]}
	v2 := [3]float32{triangle.Vertices[6], triangle.Vertices[7], triangle.Vertices[8]}
	
	// Edge vectors
	edge1 := [3]float32{v1[0] - v0[0], v1[1] - v0[1], v1[2] - v0[2]}
	edge2 := [3]float32{v2[0] - v0[0], v2[1] - v0[1], v2[2] - v0[2]}
	
	// Cross product of ray direction and edge2
	h := [3]float32{
		rayDir[1]*edge2[2] - rayDir[2]*edge2[1],
		rayDir[2]*edge2[0] - rayDir[0]*edge2[2],
		rayDir[0]*edge2[1] - rayDir[1]*edge2[0],
	}
	
	// Dot product of edge1 and h
	a := edge1[0]*h[0] + edge1[1]*h[1] + edge1[2]*h[2]
	
	if math.Abs(float64(a)) < 1e-6 {
		return -1, [3]float32{}, [3]float32{} // Ray is parallel to triangle
	}
	
	f := 1.0 / a
	s := [3]float32{rayOrigin[0] - v0[0], rayOrigin[1] - v0[1], rayOrigin[2] - v0[2]}
	u := f * (s[0]*h[0] + s[1]*h[1] + s[2]*h[2])
	
	if u < 0.0 || u > 1.0 {
		return -1, [3]float32{}, [3]float32{}
	}
	
	q := [3]float32{
		s[1]*edge1[2] - s[2]*edge1[1],
		s[2]*edge1[0] - s[0]*edge1[2],
		s[0]*edge1[1] - s[1]*edge1[0],
	}
	
	v := f * (rayDir[0]*q[0] + rayDir[1]*q[1] + rayDir[2]*q[2])
	
	if v < 0.0 || u+v > 1.0 {
		return -1, [3]float32{}, [3]float32{}
	}
	
	t := f * (edge2[0]*q[0] + edge2[1]*q[1] + edge2[2]*q[2])
	
	if t < 1e-6 { // Ray intersection behind origin
		return -1, [3]float32{}, [3]float32{}
	}
	
	// Check backface culling
	if !bothSides && a > 0 {
		return -1, [3]float32{}, [3]float32{}
	}
	
	// Calculate hit point
	hitPoint := [3]float32{
		rayOrigin[0] + rayDir[0]*t,
		rayOrigin[1] + rayDir[1]*t,
		rayOrigin[2] + rayDir[2]*t,
	}
	
	// Use precomputed normal
	hitNormal := triangle.Normal
	
	return t, hitPoint, hitNormal
}

func (rm *RaycastMesh) getNodeDepth(node *AABBNode) uint32 {
	if node == nil {
		return 0
	}
	
	if node.Left == nil && node.Right == nil {
		return node.Depth
	}
	
	leftDepth := rm.getNodeDepth(node.Left)
	rightDepth := rm.getNodeDepth(node.Right)
	
	if leftDepth > rightDepth {
		return leftDepth
	}
	return rightDepth
}

// Utility functions

func vectorLength(v [3]float32) float32 {
	return float32(math.Sqrt(float64(v[0]*v[0] + v[1]*v[1] + v[2]*v[2])))
}