package pathfinder

import "C"
import (
	"math"
	"unsafe"

	"gitee.com/night-tc/gobige/linmath"
	"gitee.com/night-tc/gobige/logger"
	"gitee.com/night-tc/gobige/msgdef/protomsg"
)

/*
c++没有动态阻挡的寻路
*/
type NavMesh struct {
	finder *cgo_dtNavfinder
}

func newNavMesh(navmesh *Cgo_dtNavMesh) IPathFinder {
	result := new(NavMesh)
	result.finder = GetNavFinder(navmesh)
	return result
}

// FindPath 提供外部寻路算法
//
// 参数：
//
//	params 起点终点
//
// 返回值：
//
//	path 路点，注意第一个点就是输入的起始点
//	result 是否寻路成功
func (this *NavMesh) FindPath(navmesh NavmeshParams) (path []protomsg.Vector3, result bool) {
	spos := linmath.PosSrv2Nav(navmesh.Start)
	epos := linmath.PosSrv2Nav(navmesh.End)

	count := int32(0)
	var floats *C.float

	if floats = FindNavPath(this.finder, &spos, &epos, &count); count == 0 {
		return nil, false
	}

	path = make([]protomsg.Vector3, count, count)
	retPtr := unsafe.Pointer((*C.float)(floats))
	size := unsafe.Sizeof(protomsg.Vector3{})
	for i := 0; i < int(count); i++ {
		linVec := (*protomsg.Vector3)(unsafe.Pointer(uintptr(retPtr) + uintptr(i)*size))
		path[i] = linmath.PosNav2Srv(*linVec)
	}
	logger.Debug(logger.LogKey_PathFinder, "[MavMesh] FindPath 寻路成功 路点个数 ", count)
	return path, true
}

// GetNear 获取某个点附近的合法位置
// 参数：
//
//	params 检查点 *protomsg.Vector3 类型
//
// 返回值：
//
//	pos 路点，注意第一个点就是输入的起始点
//	result 是否能成功
func (this *NavMesh) GetNear(srcPos *protomsg.Vector3) (pos *protomsg.Vector3, result bool) {
	if srcPos == nil {
		logger.Error("GetNear params not match")
		return nil, false
	}
	spos := linmath.PosSrv2Nav(*srcPos)
	epos := protomsg.Vector3_Invalid()

	var floats *C.float
	var count int32

	if floats = CalcNavNear(this.finder, &spos, &count); count == 0 {
		return nil, false
	}

	retPtr := unsafe.Pointer((*C.float)(floats))
	size := unsafe.Sizeof(protomsg.Vector3{})
	for i := 0; i < int(count); i++ {
		linVec := (*protomsg.Vector3)(unsafe.Pointer(uintptr(retPtr) + uintptr(i)*size))
		epos = linmath.PosNav2Srv(*linVec)
	}
	// logger.Debug(logger.LogKey_PathFinder, "[FindNavmeshCpp] GetNear spos ", srcPos, "epos ", epos)
	return &epos, true
}

// CheckWall 按照100单位步长检查墙
// 墙的定义点位高程超过500单位
func (this *NavMesh) CheckWall(srcPos, dstPos protomsg.Vector3) (path []protomsg.Vector3, bHaveWall bool) {
	bHaveWall = false

	srcPos.Y = dstPos.Y
	logger.Debug(logger.LogKey_PathFinder, "CheckWall srcPos ", srcPos, " dstPos ", dstPos)

	dir := dstPos.Sub(srcPos)
	dst := float64(dir.Len())
	dir.Normalize()

	minDst := 100.0

	stepCount := int(math.Ceil(dst / minDst))

	tmpPos := protomsg.Vector3_Invalid()
	for i := 0; i < stepCount; i++ {
		curDst := float64(i+1) * minDst
		tmpPos = srcPos.Add(dir.Mul(curDst))
		if nearPos, ok := this.GetNear(&tmpPos); ok {
			if !nearPos.IsEqualXZ(tmpPos) {
				path = append(path, *nearPos)
				logger.Debug(logger.LogKey_PathFinder, "nearPos ", *nearPos)
				bHaveWall = true
				return
			}
		} else {
			logger.Debug(logger.LogKey_PathFinder, "nearPos not find near tmpPos ", tmpPos)
			bHaveWall = true
			return
		}
	}
	return
}

// Raycast 射线检测，是否撞到墙了
// 参数：
//
//	params 起点终点
//
// 返回值：
//
//	pos 如果撞到墙了将会返回墙体位置，否则为空
//	result 是否碰到墙了，true 碰到墙 false 畅通无阻
func (this *NavMesh) Raycast(navmesh NavmeshParams) (*protomsg.Vector3, bool) {
	spos := linmath.PosSrv2Nav(navmesh.Start)
	epos := linmath.PosSrv2Nav(navmesh.End)

	var floats *C.float
	var count int32

	if floats = CalcRaycast(this.finder, &spos, &epos, &count); count == 0 {
		return nil, false
	}

	path := make([]protomsg.Vector3, count, count)
	retPtr := unsafe.Pointer((*C.float)(floats))
	size := unsafe.Sizeof(protomsg.Vector3{})
	for i := 0; i < int(count); i++ {
		linVec := (*protomsg.Vector3)(unsafe.Pointer(uintptr(retPtr) + uintptr(i)*size))
		path[i] = linmath.PosNav2Srv(*linVec)
	}
	return &(path[0]), true
}

// Raycast 射线检测，是否撞到墙了
// 参数：
//
//	srcPos 起点
//	dstPos 终点
//
// 返回值：
//
//	pos 如果撞到墙了将会返回墙体位置，否则为空
//	result 是否碰到墙了，true 碰到墙 false 没有撞到墙
func (this *NavMesh) RaycastEx(srcPos, dstPos protomsg.Vector3) (*protomsg.Vector3, bool) {
	params := NavmeshParams{srcPos, dstPos}
	return this.Raycast(params)
}

// 动态阻挡相关
// 增加可旋转的Box阻挡
func (this *NavMesh) CreateObstacleOBBBox(doorName string, centPos, halfPos protomsg.Vector3, YawDegree int32) {
	return
}

// 增加圆柱体阻挡
func (this *NavMesh) CreateObstacleCylinder(doorName string, centPos protomsg.Vector3, radius, height int32) {
	return
}

// 清理掉某个门
func (this *NavMesh) RemoveObstacle(doorName string) {
	return
}

// 清理全部的门
func (this *NavMesh) RemoveAllObstacle() {
	return
}

// 更新阻挡信息
func (this *NavMesh) UpdateObstacle() {
	return
}

// 走直线，找落点；如果我在墙里面，就从墙里出来；
func (this *NavMesh) MeshCheckRushPath(srcPos, stpos protomsg.Vector3) protomsg.Vector3 {
	if tmpPos, ok := this.RaycastEx(srcPos, stpos); !ok {
		logger.Debug(logger.LogKey_PathFinder, " [MavMesh] MeshCheckRushPath 未能检测到墙直接通过 ", stpos)
		return stpos
	} else {
		if tmpPos.IsEqualXZ(srcPos) {
			if path, ok := this.FindPath(PackageFindParams(srcPos, stpos)); ok {
				if len(path) == 2 {
					*tmpPos = path[1]
					logger.Debug(logger.LogKey_PathFinder, " [MavMesh] MeshCheckRushPath 检测到墙 再次尝试走到墙根 ", path[1])
				}
			}
		} else {
			logger.Debug(logger.LogKey_PathFinder, " [MavMesh] MeshCheckRushPath 检测到墙 ", tmpPos)
		}
		stpos = *tmpPos
		return stpos
	}
}

func (this *NavMesh) Destroy() {
	if this.finder != nil {
		Free(unsafe.Pointer(this.finder))
		this.finder = nil
	}
}