package main

import (
	"io"
	"log"
	"math"
	"net/http"
	"os"
	"sort"
	"strconv"
	"time"
)

const (
	missionTick        = 200 * time.Millisecond
	missionTimeScale   = 10.0 // mock 加速倍数，便于在短时间内跑完航线
	defaultCruiseSpeed = 8.0  // m/s
	minLegDuration     = 2 * time.Second
	maxLegDuration     = 12 * time.Second
	maxHoldSec         = 8
	defaultTakeoffAlt  = 50.0
)

// missionWaypoint 与后台 workflow.start_task params.waypoints 字段对齐
type missionWaypoint struct {
	Seq       int
	Longitude float64
	Latitude  float64
	Altitude  float64
	Speed     float64
	Yaw       float64
	HoldSec   int
}

func (d *MockDock) beginMission(commandID string) bool {
	d.mu.Lock()
	defer d.mu.Unlock()
	if d.inMission {
		return false
	}
	d.inMission = true
	d.missionPaused = false
	d.missionCmdID = commandID
	d.missionCancel = make(chan struct{})
	return true
}

func (d *MockDock) endMission() {
	d.mu.Lock()
	defer d.mu.Unlock()
	d.inMission = false
	d.missionPaused = false
	d.missionCmdID = ""
	d.missionCancel = nil
}

func (d *MockDock) isRepeatCommand(commandID string) bool {
	d.mu.Lock()
	defer d.mu.Unlock()
	return commandID != "" && d.inMission && d.missionCmdID == commandID
}

// cancelMission 通知当前任务中止，返回是否有正在执行的任务
func (d *MockDock) cancelMission() bool {
	d.mu.Lock()
	defer d.mu.Unlock()
	if !d.inMission || d.missionCancel == nil {
		return false
	}
	select {
	case <-d.missionCancel:
	default:
		close(d.missionCancel)
	}
	d.missionPaused = false
	return true
}

func (d *MockDock) setMissionPaused(paused bool) {
	d.mu.Lock()
	defer d.mu.Unlock()
	if d.inMission {
		d.missionPaused = paused
	}
}

func (d *MockDock) currentCancel() <-chan struct{} {
	d.mu.Lock()
	defer d.mu.Unlock()
	return d.missionCancel
}

func (d *MockDock) currentLLA() (lat, lon, alt float64) {
	d.mu.Lock()
	defer d.mu.Unlock()
	return d.tele.latitude, d.tele.longitude, d.tele.altitude
}

func (d *MockDock) setPose(lat, lon, alt, speed, heading float64) {
	d.mu.Lock()
	d.tele.latitude = lat
	d.tele.longitude = lon
	d.tele.altitude = alt
	d.tele.groundSpeed = speed
	d.tele.heading = heading
	d.tele.yaw = heading
	if d.tele.batteryPct > 20 && speed > 0.5 {
		d.tele.batteryPctDrain++
		if d.tele.batteryPctDrain >= 20 {
			d.tele.batteryPctDrain = 0
			d.tele.batteryPct--
			d.tele.batteryV = 22.0 + float64(d.tele.batteryPct)*0.037
		}
	}
	d.mu.Unlock()
}

func (d *MockDock) setArmedFlying(alt float64) {
	d.mu.Lock()
	d.tele.armed = true
	d.tele.flightMode = "AUTO"
	d.tele.altitude = alt
	d.doorState = "open"
	d.mu.Unlock()
}

func (d *MockDock) snapHomeLanded() {
	d.mu.Lock()
	d.tele.armed = false
	d.tele.flightMode = "STANDBY"
	d.tele.groundSpeed = 0
	d.tele.altitude = 0
	d.tele.latitude = d.spec.DroneLat
	d.tele.longitude = d.spec.DroneLon
	d.tele.heading = 0
	d.tele.yaw = 0
	d.doorState = "closed"
	d.mu.Unlock()
}

// runStartTask 执行 workflow.start_task：按航点飞行并持续改写遥测坐标
func (d *MockDock) runStartTask(cmd commandMsg) {
	defer d.endMission()

	taskID, missionID := extractTaskIDs(cmd)
	waypoints := parseWaypoints(cmd.Params)
	recordOriginalVideo := asBool(cmd.Params["recordOriginalVideo"])
	originalVideo := parseOriginalVideo(cmd.Params["originalVideo"])
	d.mu.Lock()
	d.uploadedRoute = waypoints
	d.mu.Unlock()

	log.Printf("[%s] 开始执行任务 taskId=%s 航点数=%d", d.spec.DockID, taskID, len(waypoints))
	for _, wp := range waypoints {
		log.Printf("[%s]   航点 seq=%d lon=%.6f lat=%.6f alt=%.1f speed=%.1f hold=%ds",
			d.spec.DockID, wp.Seq, wp.Longitude, wp.Latitude, wp.Altitude, wp.Speed, wp.HoldSec)
	}

	d.setDoor("open")
	d.publishStateDock()

	prep := []string{
		"preparing_dock_takeoff",
		"waiting_drone_online",
		"checking_drone",
		"route_uploading",
		"route_ready",
	}
	for _, step := range prep {
		d.publishWorkflow(cmd, taskID, missionID, "running", step)
		if !d.sleepOrAbort(400 * time.Millisecond) {
			d.finishWorkflow(cmd, taskID, missionID, "cancelled")
			return
		}
	}

	takeoffAlt := defaultTakeoffAlt
	if len(waypoints) > 0 && waypoints[0].Altitude > 0 {
		takeoffAlt = waypoints[0].Altitude
	}

	d.publishWorkflow(cmd, taskID, missionID, "running", "taking_off")
	d.setArmedFlying(0)
	d.publishStateDrone()
	if !d.climbTo(takeoffAlt) {
		d.returnHomeAndLand(true)
		d.finishWorkflow(cmd, taskID, missionID, "cancelled")
		return
	}
	d.publishStateDrone()

	d.publishWorkflow(cmd, taskID, missionID, "running", "finishing_takeoff")
	if !d.sleepOrAbort(400 * time.Millisecond) {
		d.returnHomeAndLand(true)
		d.finishWorkflow(cmd, taskID, missionID, "cancelled")
		return
	}

	d.publishWorkflow(cmd, taskID, missionID, "running", "starting_mission")
	if !d.sleepOrAbort(300 * time.Millisecond) {
		d.returnHomeAndLand(true)
		d.finishWorkflow(cmd, taskID, missionID, "cancelled")
		return
	}

	d.publishWorkflow(cmd, taskID, missionID, "running", "mission_running")
	if !d.flyWaypoints(waypoints) {
		d.publishWorkflow(cmd, taskID, missionID, "running", "returning_to_home")
		d.returnHomeAndLand(true)
		d.finishWorkflow(cmd, taskID, missionID, "cancelled")
		return
	}

	d.publishWorkflow(cmd, taskID, missionID, "running", "returning_to_home")
	if !d.returnHomeAndLand(false) {
		d.returnHomeAndLand(true)
		d.finishWorkflow(cmd, taskID, missionID, "cancelled")
		return
	}

	d.finishWorkflow(cmd, taskID, missionID, "succeeded")
	if recordOriginalVideo {
		d.uploadOriginalVideo(cmd, taskID, originalVideo)
	}
	log.Printf("[%s] 任务执行完成 taskId=%s", d.spec.DockID, taskID)
}

func (d *MockDock) runOneKeyTakeoff(cmd commandMsg) {
	defer d.endMission()
	taskID, missionID := extractTaskIDs(cmd)
	d.setDoor("open")
	d.publishStateDock()
	d.publishWorkflow(cmd, taskID, missionID, "running", "taking_off")
	d.setArmedFlying(0)
	if !d.climbTo(defaultTakeoffAlt) {
		d.finishWorkflow(cmd, taskID, missionID, "cancelled")
		return
	}
	d.publishStateDrone()
	d.publishWorkflow(cmd, taskID, missionID, "succeeded", "idle")
}

func (d *MockDock) runGenericWorkflow(cmd commandMsg) {
	defer d.endMission()
	taskID, missionID := extractTaskIDs(cmd)

	d.setDoor("open")
	d.publishStateDock()

	steps := []string{
		"preparing_dock_takeoff",
		"waiting_drone_online",
		"checking_drone",
		"taking_off",
		"finishing_takeoff",
		"returning_to_home",
		"preparing_dock_landing",
		"returning",
		"finishing_landing",
	}
	for _, step := range steps {
		d.publishWorkflow(cmd, taskID, missionID, "running", step)
		if step == "taking_off" {
			d.setArmedFlying(0)
			if !d.climbTo(defaultTakeoffAlt) {
				d.returnHomeAndLand(true)
				d.finishWorkflow(cmd, taskID, missionID, "cancelled")
				return
			}
		}
		if !d.sleepOrAbort(time.Second) {
			d.returnHomeAndLand(true)
			d.finishWorkflow(cmd, taskID, missionID, "cancelled")
			return
		}
	}

	d.returnHomeAndLand(true)
	d.finishWorkflow(cmd, taskID, missionID, "succeeded")
}

func (d *MockDock) flyUploadedMission(waypoints []missionWaypoint) {
	defer d.endMission()
	log.Printf("[%s] 执行已上传航线，航点数=%d", d.spec.DockID, len(waypoints))
	d.setArmedFlying(math.Max(d.currentAlt(), defaultTakeoffAlt))
	if !d.flyWaypoints(waypoints) {
		d.returnHomeAndLand(true)
		return
	}
	d.returnHomeAndLand(true)
}

func (d *MockDock) flyWaypoints(waypoints []missionWaypoint) bool {
	for i, wp := range waypoints {
		alt := wp.Altitude
		if alt <= 0 {
			_, _, alt = d.currentLLA()
			if alt <= 0 {
				alt = defaultTakeoffAlt
			}
		}
		speed := wp.Speed
		if speed <= 0 {
			speed = defaultCruiseSpeed
		}
		log.Printf("[%s] 飞向航点 %d/%d (seq=%d)", d.spec.DockID, i+1, len(waypoints), wp.Seq)
		if !d.flyTo(wp.Latitude, wp.Longitude, alt, speed, false) {
			return false
		}
		if wp.HoldSec > 0 {
			if !d.holdAt(wp.HoldSec, wp.Yaw) {
				return false
			}
		} else if wp.Yaw != 0 {
			d.mu.Lock()
			d.tele.heading = wp.Yaw
			d.tele.yaw = wp.Yaw
			d.mu.Unlock()
		}
		d.publishStateDrone()
	}
	return true
}

func (d *MockDock) climbTo(alt float64) bool {
	lat, lon, cur := d.currentLLA()
	if cur < 1 {
		d.setPose(lat, lon, 1, 0, d.currentHeading())
	}
	return d.flyTo(lat, lon, alt, 5, false)
}

func (d *MockDock) currentAlt() float64 {
	_, _, alt := d.currentLLA()
	return alt
}

func (d *MockDock) currentHeading() float64 {
	d.mu.Lock()
	defer d.mu.Unlock()
	return d.tele.heading
}

func (d *MockDock) returnHomeAndLand(forced bool) bool {
	homeAlt := math.Max(d.currentAlt(), 20)
	if !d.flyTo(d.spec.DroneLat, d.spec.DroneLon, homeAlt, defaultCruiseSpeed, forced) && !forced {
		return false
	}
	if !d.flyTo(d.spec.DroneLat, d.spec.DroneLon, 0, 3, forced) && !forced {
		return false
	}
	d.snapHomeLanded()
	d.publishStateDrone()
	d.publishStateDock()
	return true
}

func (d *MockDock) rtlAndLand() {
	if !d.beginMission("rtl") {
		return
	}
	defer d.endMission()
	d.returnHomeAndLand(true)
}

func (d *MockDock) finishWorkflow(cmd commandMsg, taskID, missionID, state string) {
	if state == "cancelled" {
		d.setDoor("closed")
		d.publishStateDock()
	}
	d.publishWorkflow(cmd, taskID, missionID, state, "idle")
}

// flyTo 按球面插值飞向目标点；forced 时忽略暂停/取消（返航落地用）
func (d *MockDock) uploadOriginalVideo(cmd commandMsg, taskID string, video originalVideoUpload) {
	if video.VideoID == 0 || video.ExecutionID == 0 || video.UploadURL == "" {
		d.publishOriginalVideo(cmd, taskID, video, "failed", "UPLOAD_CONFIG_INVALID", 0)
		return
	}
	if video.UploadExpireAt > 0 && time.Now().UnixMilli() >= video.UploadExpireAt {
		d.publishOriginalVideo(cmd, taskID, video, "failed", "UPLOAD_URL_EXPIRED", 0)
		return
	}

	file, err := os.Open(d.videoFile)
	if err != nil {
		d.publishOriginalVideo(cmd, taskID, video, "failed", "VIDEO_FILE_NOT_FOUND", 0)
		return
	}
	defer file.Close()
	info, err := file.Stat()
	if err != nil || info.Size() <= 0 {
		d.publishOriginalVideo(cmd, taskID, video, "failed", "VIDEO_FILE_INVALID", 0)
		return
	}
	req, err := http.NewRequest(http.MethodPut, video.UploadURL, file)
	if err != nil {
		d.publishOriginalVideo(cmd, taskID, video, "failed", "OSS_PUT_FAILED", 0)
		return
	}
	req.ContentLength = info.Size()
	req.Header.Set("Content-Type", "video/mp4")
	resp, err := (&http.Client{Timeout: 20 * time.Second}).Do(req)
	if err != nil {
		d.publishOriginalVideo(cmd, taskID, video, "failed", "OSS_PUT_FAILED", 0)
		return
	}
	_, _ = io.Copy(io.Discard, resp.Body)
	resp.Body.Close()
	if resp.StatusCode < http.StatusOK || resp.StatusCode >= http.StatusMultipleChoices {
		d.publishOriginalVideo(cmd, taskID, video, "failed", "OSS_PUT_FAILED", 0)
		return
	}
	d.publishOriginalVideo(cmd, taskID, video, "completed", "", info.Size())
}

func (d *MockDock) publishOriginalVideo(cmd commandMsg, taskID string, video originalVideoUpload, eventType, errorCode string, fileSize int64) {
	d.publishDrone(d.topic("internal/original-video"), 1, false, d.spec.DroneSN, map[string]any{
		"extension":   "laic.mock.original-video.v1",
		"eventId":     "mock-original-video-" + strconv.FormatInt(video.VideoID, 10),
		"eventType":   eventType,
		"videoId":     video.VideoID,
		"executionId": video.ExecutionID,
		"taskId":      taskID,
		"commandId":   cmd.CommandID,
		"fileSize":    fileSize,
		"duration":    1,
		"errorCode":   nilIfEmpty(errorCode),
		"uploadedAt":  time.Now().UnixMilli(),
	})
}

type originalVideoUpload struct {
	VideoID        int64
	ExecutionID    int64
	UploadURL      string
	UploadExpireAt int64
}

func parseOriginalVideo(value any) originalVideoUpload {
	data, ok := value.(map[string]any)
	if !ok {
		return originalVideoUpload{}
	}
	return originalVideoUpload{
		VideoID:        asInt64(data["videoId"]),
		ExecutionID:    asInt64(data["executionId"]),
		UploadURL:      asString(data["uploadUrl"]),
		UploadExpireAt: asInt64(data["uploadExpireAt"]),
	}
}

func asString(value any) string {
	text, _ := value.(string)
	return text
}

func asBool(value any) bool {
	valueBool, _ := value.(bool)
	return valueBool
}

func asInt64(value any) int64 {
	switch v := value.(type) {
	case float64:
		return int64(v)
	case string:
		parsed, _ := strconv.ParseInt(v, 10, 64)
		return parsed
	default:
		return 0
	}
}

func (d *MockDock) flyTo(lat, lon, alt, speed float64, forced bool) bool {
	if speed <= 0 {
		speed = defaultCruiseSpeed
	}
	fromLat, fromLon, fromAlt := d.currentLLA()
	dist := haversineM(fromLat, fromLon, lat, lon)
	dAlt := math.Abs(alt - fromAlt)
	heading := bearingDeg(fromLat, fromLon, lat, lon)
	if dist < 0.5 {
		heading = d.currentHeading()
	}

	horizSec := dist / speed / missionTimeScale
	vertSec := dAlt / 5.0 / missionTimeScale
	sec := math.Max(horizSec, vertSec)
	dur := time.Duration(sec * float64(time.Second))
	if (dist > 1 || dAlt > 1) && dur < minLegDuration {
		dur = minLegDuration
	}
	if dur > maxLegDuration {
		dur = maxLegDuration
	}
	if dur < missionTick {
		dur = missionTick
	}

	steps := int(dur / missionTick)
	if steps < 1 {
		steps = 1
	}
	for i := 1; i <= steps; i++ {
		if !forced && !d.waitMissionTick() {
			return false
		}
		if forced {
			time.Sleep(missionTick)
		}
		t := float64(i) / float64(steps)
		d.setPose(
			lerp(fromLat, lat, t),
			lerp(fromLon, lon, t),
			lerp(fromAlt, alt, t),
			speed,
			heading,
		)
	}
	d.setPose(lat, lon, alt, 0, heading)
	return true
}

func (d *MockDock) holdAt(seconds int, yaw float64) bool {
	if seconds > maxHoldSec {
		seconds = maxHoldSec
	}
	if yaw != 0 {
		d.mu.Lock()
		d.tele.heading = yaw
		d.tele.yaw = yaw
		d.tele.groundSpeed = 0
		d.mu.Unlock()
	} else {
		d.setPoseKeepLLA(0)
	}
	ticks := seconds * int(time.Second/missionTick)
	for i := 0; i < ticks; i++ {
		if !d.waitMissionTick() {
			return false
		}
	}
	return true
}

func (d *MockDock) setPoseKeepLLA(speed float64) {
	d.mu.Lock()
	d.tele.groundSpeed = speed
	d.mu.Unlock()
}

func (d *MockDock) waitMissionTick() bool {
	cancel := d.currentCancel()
	for {
		if isClosed(cancel) {
			return false
		}
		d.mu.Lock()
		paused := d.missionPaused
		d.mu.Unlock()
		if !paused {
			if cancel == nil {
				time.Sleep(missionTick)
				return true
			}
			select {
			case <-cancel:
				return false
			case <-time.After(missionTick):
				return true
			}
		}
		if cancel == nil {
			time.Sleep(100 * time.Millisecond)
			continue
		}
		select {
		case <-cancel:
			return false
		case <-time.After(100 * time.Millisecond):
		}
	}
}

func (d *MockDock) sleepOrAbort(wait time.Duration) bool {
	cancel := d.currentCancel()
	if cancel == nil {
		time.Sleep(wait)
		return true
	}
	t := time.NewTimer(wait)
	defer t.Stop()
	select {
	case <-cancel:
		return false
	case <-t.C:
		return true
	}
}

func extractTaskIDs(cmd commandMsg) (taskID, missionID string) {
	if cmd.Params == nil {
		return "", ""
	}
	if v, ok := cmd.Params["taskId"].(string); ok {
		taskID = v
	}
	if v, ok := cmd.Params["missionId"].(string); ok {
		missionID = v
	}
	return taskID, missionID
}

func parseWaypoints(params map[string]any) []missionWaypoint {
	if params == nil {
		return nil
	}
	raw, ok := params["waypoints"]
	if !ok || raw == nil {
		return nil
	}
	arr, ok := raw.([]any)
	if !ok {
		return nil
	}
	out := make([]missionWaypoint, 0, len(arr))
	for i, item := range arr {
		m, ok := item.(map[string]any)
		if !ok {
			continue
		}
		wp := missionWaypoint{
			Seq:       asInt(m["seq"]),
			Longitude: asFloat(m["longitude"]),
			Latitude:  asFloat(m["latitude"]),
			Altitude:  asFloat(m["altitude"]),
			Speed:     asFloat(m["speed"]),
			Yaw:       asFloat(m["yaw"]),
			HoldSec:   asInt(m["holdSec"]),
		}
		if wp.Seq == 0 {
			wp.Seq = i
		}
		if wp.Longitude == 0 && wp.Latitude == 0 {
			continue
		}
		out = append(out, wp)
	}
	sort.SliceStable(out, func(i, j int) bool { return out[i].Seq < out[j].Seq })
	return out
}

func asFloat(v any) float64 {
	switch n := v.(type) {
	case float64:
		return n
	case float32:
		return float64(n)
	case int:
		return float64(n)
	case int32:
		return float64(n)
	case int64:
		return float64(n)
	case uint64:
		return float64(n)
	default:
		return 0
	}
}

func asInt(v any) int {
	return int(asFloat(v))
}

func isClosed(ch <-chan struct{}) bool {
	if ch == nil {
		return false
	}
	select {
	case <-ch:
		return true
	default:
		return false
	}
}

func lerp(a, b, t float64) float64 {
	return a + (b-a)*t
}

func haversineM(lat1, lon1, lat2, lon2 float64) float64 {
	const earthRadiusM = 6371000.0
	phi1 := lat1 * math.Pi / 180
	phi2 := lat2 * math.Pi / 180
	dPhi := (lat2 - lat1) * math.Pi / 180
	dLambda := (lon2 - lon1) * math.Pi / 180
	a := math.Sin(dPhi/2)*math.Sin(dPhi/2) +
		math.Cos(phi1)*math.Cos(phi2)*math.Sin(dLambda/2)*math.Sin(dLambda/2)
	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
	return earthRadiusM * c
}

func bearingDeg(lat1, lon1, lat2, lon2 float64) float64 {
	phi1 := lat1 * math.Pi / 180
	phi2 := lat2 * math.Pi / 180
	dLambda := (lon2 - lon1) * math.Pi / 180
	y := math.Sin(dLambda) * math.Cos(phi2)
	x := math.Cos(phi1)*math.Sin(phi2) - math.Sin(phi1)*math.Cos(phi2)*math.Cos(dLambda)
	deg := math.Atan2(y, x) * 180 / math.Pi
	if deg < 0 {
		deg += 360
	}
	return deg
}