// Package control owns the authoritative race state.
//
// For PoC, the physics model is intentionally simple (no tire slip, no
// collision): each car integrates position from (throttle, brake, steering).
// Realistic physics, anti-cheat checks, and CV-based ground truth will be
// added in MVP / production phases.
package control

import (
	"context"
	"fmt"
	"math"
	"sync"
	"sync/atomic"
	"time"

	"github.com/x0gp/server/internal/transport"
)

// RacePhase describes the lifecycle of a race.
type RacePhase int

const (
	PhaseLobby     RacePhase = 0
	PhaseCountdown RacePhase = 1
	PhaseRacing    RacePhase = 2
	PhaseFinished  RacePhase = 3
)

func (p RacePhase) String() string {
	switch p {
	case PhaseLobby:
		return "lobby"
	case PhaseCountdown:
		return "countdown"
	case PhaseRacing:
		return "racing"
	case PhaseFinished:
		return "finished"
	default:
		return "unknown"
	}
}

// Car is the authoritative state of a single car.
type Car struct {
	ID         string
	DriverID   string // session id
	DriverName string
	DeviceID   *int   // internal mapping to physical ESP32 device ID
	X, Y       float64
	Heading    float64 // radians
	Speed      float64 // m/s
	Lap        int
	Sector     int
	LastLapMs  int64
	BestLapMs  int64
	DNF        bool

	// Last applied input (echoed in snapshot for client reconciliation).
	AppliedSteering float64
	AppliedThrottle float64
	AppliedBrake    float64

	mu sync.Mutex // guards input application
}

// ApplyInput applies a control input with ramp limits (sanity-check).
func (c *Car) ApplyInput(in transport.InputState, dtSec float64) {
	c.mu.Lock()
	defer c.mu.Unlock()

	// Sanity clamps.
	s := clamp(in.Steering, -1, 1)
	t := clamp(in.Throttle, 0, 1)
	b := clamp(in.Brake, 0, 1)

	// Throttle / brake -> longitudinal acceleration.
	const maxAccel = 4.0  // m/s^2
	const maxBrake = 8.0  // m/s^2
	const maxSpeed = 6.0  // m/s (1/27 scale, ~21 km/h)
	const dragK = 0.5     // linear drag coefficient

	accel := maxAccel*t - maxBrake*b - dragK*c.Speed
	c.Speed += accel * dtSec
	if c.Speed < 0 {
		c.Speed = 0
	}
	if c.Speed > maxSpeed {
		c.Speed = maxSpeed
	}

	// Steering -> yaw rate, proportional to speed (no turn at standstill).
	const maxYawRate = 3.5 // rad/s
	yawRate := maxYawRate * s * (0.3 + 0.7*math.Min(c.Speed/maxSpeed, 1))
	c.Heading += yawRate * dtSec

	// Position integration.
	c.X += c.Speed * math.Cos(c.Heading) * dtSec
	c.Y += c.Speed * math.Sin(c.Heading) * dtSec

	c.AppliedSteering = s
	c.AppliedThrottle = t
	c.AppliedBrake = b
}

// Snapshot returns a copy of car state for the wire snapshot.
func (c *Car) Snapshot() transport.CarInfo {
	c.mu.Lock()
	defer c.mu.Unlock()
	info := transport.CarInfo{
		ID:         c.ID,
		DriverName: c.DriverName,
		X:          c.X,
		Y:          c.Y,
		Heading:    c.Heading,
		Speed:      c.Speed,
		Lap:        c.Lap,
		Sector:     c.Sector,
		LastLapMs:  c.LastLapMs,
		BestLapMs:  c.BestLapMs,
		DNF:        c.DNF,
	}
	info.InputApplied.Steering = c.AppliedSteering
	info.InputApplied.Throttle = c.AppliedThrottle
	info.InputApplied.Brake = c.AppliedBrake
	return info
}

// Engine is the authoritative race engine.
//
// Not safe to copy; pass by pointer.
type Engine struct {
	mu        sync.RWMutex
	phase     atomic.Int32 // RacePhase
	tick      atomic.Uint64
	cars      map[string]*Car // keyed by Car.ID
	byDriver  map[string]*Car // keyed by DriverID
	tickRate  int
	dtSec     float64

	// Listeners for snapshot fan-out.
	snapCh chan transport.RaceSnapshot

	// Lifecycle.
	stopCh chan struct{}
}

// NewEngine creates a race engine. tickRateHz is the authoritative tick rate.
func NewEngine(tickRateHz int) *Engine {
	return &Engine{
		phase:    atomic.Int32{},
		cars:     make(map[string]*Car),
		byDriver: make(map[string]*Car),
		tickRate: tickRateHz,
		dtSec:    1.0 / float64(tickRateHz),
		snapCh:   make(chan transport.RaceSnapshot, 64),
		stopCh:   make(chan struct{}),
	}
}

// Snapshots returns the channel of authoritative race snapshots.
func (e *Engine) Snapshots() <-chan transport.RaceSnapshot { return e.snapCh }

// Phase returns the current race phase.
func (e *Engine) Phase() RacePhase { return RacePhase(e.phase.Load()) }

// SetPhase transitions the race phase.
func (e *Engine) SetPhase(p RacePhase) { e.phase.Store(int32(p)) }

// AddCar registers a car and returns it. Returns an error if the driver
// already has a car or the max car count is reached.
func (e *Engine) AddCar(driverID, driverName string, slot int, deviceID *int) (*Car, error) {
	e.mu.Lock()
	defer e.mu.Unlock()

	if _, exists := e.byDriver[driverID]; exists {
		return nil, fmt.Errorf("driver %s already has a car", driverID)
	}
	if len(e.cars) >= 4 {
		return nil, fmt.Errorf("race is full (max 4 cars for PoC)")
	}

	id := fmt.Sprintf("car-%d", slot)
	car := &Car{
		ID:         id,
		DriverID:   driverID,
		DriverName: driverName,
		DeviceID:   deviceID,
		// Spread cars along start grid (x = 0, y = slot * 0.4 m).
		Y: float64(slot) * 0.4,
	}
	e.cars[id] = car
	e.byDriver[driverID] = car
	return car, nil
}

// RemoveCar removes a car by driver id.
func (e *Engine) RemoveCar(driverID string) {
	e.mu.Lock()
	defer e.mu.Unlock()
	car, ok := e.byDriver[driverID]
	if !ok {
		return
	}
	delete(e.byDriver, driverID)
	delete(e.cars, car.ID)
}

// GetCarByDriver returns the car for a driver (or nil).
func (e *Engine) GetCarByDriver(driverID string) *Car {
	e.mu.RLock()
	defer e.mu.RUnlock()
	return e.byDriver[driverID]
}

// Run drives the tick loop. Cancelled by ctx or Stop().
func (e *Engine) Run(ctx context.Context) {
	period := time.Duration(float64(time.Second) / float64(e.tickRate))
	t := time.NewTicker(period)
	defer t.Stop()

	// Snapshot cadence: 30 Hz for PoC (half tick rate is usually enough).
	const snapshotEveryTicks = 2
	var sinceSnap int

	for {
		select {
		case <-ctx.Done():
			return
		case <-e.stopCh:
			return
		case <-t.C:
			e.advance()
			sinceSnap++
			if sinceSnap >= snapshotEveryTicks {
				sinceSnap = 0
				e.publishSnapshot()
			}
		}
	}
}

// Stop terminates the tick loop.
func (e *Engine) Stop() { close(e.stopCh) }

// advance integrates one tick of physics (no-op if no cars).
func (e *Engine) advance() {
	e.tick.Add(1)
	e.mu.RLock()
	defer e.mu.RUnlock()
	for _, car := range e.cars {
		if car.DNF {
			continue
		}
		// PoC: keep applying last input without changes.
		// In real life, the input pipeline feeds this.
		in := transport.InputState{
			Steering: car.AppliedSteering,
			Throttle: car.AppliedThrottle,
			Brake:    car.AppliedBrake,
		}
		car.ApplyInput(in, e.dtSec)
	}
}

// publishSnapshot sends a snapshot to subscribers (non-blocking).
func (e *Engine) publishSnapshot() {
	e.mu.RLock()
	cars := make([]transport.CarInfo, 0, len(e.cars))
	for _, c := range e.cars {
		cars = append(cars, c.Snapshot())
	}
	e.mu.RUnlock()

	snap := transport.RaceSnapshot{
		Tick:    e.tick.Load(),
		TSMs:    time.Now().UnixMilli(),
		Elapsed: float64(e.tick.Load()) * e.dtSec,
		Cars:    cars,
	}

	select {
	case e.snapCh <- snap:
	default:
		// Drop if subscriber is slow; log in prod.
	}
}

func clamp(v, lo, hi float64) float64 {
	if v < lo {
		return lo
	}
	if v > hi {
		return hi
	}
	return v
}