package model

import (
    "math"
    "time"
)

// LocationUpdate adalah data GPS dari driver
type LocationUpdate struct {
    OrderID      string    `json:"order_id"`
    DriverID     string    `json:"driver_id"`
    Lat          float64   `json:"lat"`
    Lng          float64   `json:"lng"`
    Altitude     float64   `json:"altitude,omitempty"`
    Speed        float64   `json:"speed"`         // m/s
    Bearing      float64   `json:"bearing"`        // degrees
    Accuracy     float64   `json:"accuracy"`       // meters (GPS accuracy)
    Timestamp    time.Time `json:"timestamp"`       // device timestamp
    ReceivedAt   time.Time `json:"received_at"`     // server timestamp
    Provider     string    `json:"provider"`        // gps, network, fused
}

// Validate melakukan validasi dasar posisi
func (l *LocationUpdate) Validate() error {
    if l.OrderID == "" {
        return ErrMissingOrderID
    }
    if l.DriverID == "" {
        return ErrMissingDriverID
    }
    if math.Abs(l.Lat) > 90 || math.Abs(l.Lng) > 180 {
        return ErrInvalidCoordinates
    }
    if l.Speed < 0 || l.Speed > 55.6 { // >200 km/jam = invalid
        return ErrInvalidSpeed
    }
    if l.Accuracy < 0 || l.Accuracy > 500 { // >500m accuracy = useless
        return ErrInvalidAccuracy
    }
    if time.Since(l.Timestamp) > 30*time.Second {
        return ErrStaleTimestamp
    }
    if l.Timestamp.After(time.Now()) {
        return ErrFutureTimestamp
    }
    return nil
}

// Sentinels for validation errors
var (
    ErrMissingOrderID   = &ValidationError{"missing_order_id"}
    ErrMissingDriverID  = &ValidationError{"missing_driver_id"}
    ErrInvalidCoordinates = &ValidationError{"invalid_coordinates"}
    ErrInvalidSpeed     = &ValidationError{"invalid_speed"}
    ErrInvalidAccuracy  = &ValidationError{"invalid_accuracy"}
    ErrStaleTimestamp   = &ValidationError{"stale_timestamp"}
    ErrFutureTimestamp  = &ValidationError{"future_timestamp"}
    ErrThrottled        = &ValidationError{"throttled"}
    ErrDuplicate        = &ValidationError{"duplicate"}
)

type ValidationError struct {
    Code string
}

func (e *ValidationError) Error() string { return e.Code }

// SmoothedPosition adalah hasil Kalman filtering
type SmoothedPosition struct {
    OrderID     string    `json:"order_id"`
    DriverID    string    `json:"driver_id"`
    Lat         float64   `json:"lat"`
    Lng         float64   `json:"lng"`
    Speed       float64   `json:"speed"`
    Bearing     float64   `json:"bearing"`
    Estimated   bool      `json:"estimated"` // true jika hasil prediksi (bukan observasi)
    Timestamp   time.Time `json:"timestamp"`
}

package ingestion

import (
    "context"
    "encoding/json"
    "fmt"
    "log"
    "net/http"
    "sync"
    "time"

    "github.com/faisalaffan/qcommerce/internal/tracking/model"
    "github.com/gorilla/websocket"
)

var upgrader = websocket.Upgrader{
    ReadBufferSize:  1024,
    WriteBufferSize: 1024,
    CheckOrigin: func(r *http.Request) bool {
        return true // Production: validate origin
    },
}

// Config untuk WebSocket server
type Config struct {
    MaxConnections     int           `json:"max_connections"`      // 10000
    ReadTimeout        time.Duration `json:"read_timeout"`         // 60s
    WriteTimeout       time.Duration `json:"write_timeout"`        // 10s
    PingInterval       time.Duration `json:"ping_interval"`        // 30s
    MaxMessageSize     int64         `json:"max_message_size"`     // 4096 bytes
    MinUpdateInterval  time.Duration `json:"min_update_interval"`  // 2s throttle
}

// DefaultConfig returns a sensible default
func DefaultConfig() Config {
    return Config{
        MaxConnections:    10000,
        ReadTimeout:       60 * time.Second,
        WriteTimeout:      10 * time.Second,
        PingInterval:      30 * time.Second,
        MaxMessageSize:    4096,
        MinUpdateInterval: 2 * time.Second,
    }
}

// DriverConnection mewakili satu koneksi WebSocket driver
type DriverConnection struct {
    Conn       *websocket.Conn
    DriverID   string
    OrderID    string
    ConnectedAt time.Time
    LastUpdate  time.Time
    mu         sync.Mutex
}

// IngestionServer menerima GPS location dari driver
type IngestionServer struct {
    config      Config
    validator   *LocationValidator
    publisher   LocationPublisher
    connections map[string]*DriverConnection // driverID -> conn
    mu          sync.RWMutex
    connCount   int32
    done        chan struct{}
}

// LocationPublisher interface untuk publish location setelah validasi
type LocationPublisher interface {
    PublishLocation(ctx context.Context, loc *model.LocationUpdate) error
    PublishSmoothed(ctx context.Context, pos *model.SmoothedPosition) error
}

func NewIngestionServer(config Config, validator *LocationValidator, publisher LocationPublisher) *IngestionServer {
    return &IngestionServer{
        config:      config,
        validator:   validator,
        publisher:   publisher,
        connections: make(map[string]*DriverConnection),
        done:        make(chan struct{}),
    }
}

// HandleWebSocket adalah HTTP handler untuk upgrade ke WebSocket
func (s *IngestionServer) HandleWebSocket(w http.ResponseWriter, r *http.Request) {
    // Check connection limit
    if int(s.connCount) >= s.config.MaxConnections {
        http.Error(w, "max connections reached", http.StatusServiceUnavailable)
        return
    }

    // Authenticate driver
    driverID := r.URL.Query().Get("driver_id")
    orderID := r.URL.Query().Get("order_id")
    token := r.URL.Query().Get("token")

    if driverID == "" || orderID == "" || token == "" {
        http.Error(w, "missing driver_id, order_id, or token", http.StatusBadRequest)
        return
    }

    // Verify token (production: validate JWT)
    // For now, basic sanity check
    if len(token) < 10 {
        http.Error(w, "invalid token", http.StatusUnauthorized)
        return
    }

    conn, err := upgrader.Upgrade(w, r, nil)
    if err != nil {
        log.Printf("websocket upgrade: %v", err)
        return
    }

    dc := &DriverConnection{
        Conn:        conn,
        DriverID:    driverID,
        OrderID:     orderID,
        ConnectedAt: time.Now(),
    }

    // Register connection
    s.mu.Lock()
    // Close existing connection if same driver reconnects
    if existing, ok := s.connections[driverID]; ok {
        existing.Conn.Close()
    }
    s.connections[driverID] = dc
    s.connCount++
    s.mu.Unlock()

    log.Printf("driver connected: %s order=%s total=%d", driverID, orderID, s.connCount)

    // Set read/write deadlines
    conn.SetReadLimit(s.config.MaxMessageSize)
    conn.SetReadDeadline(time.Now().Add(s.config.ReadTimeout))
    conn.SetPongHandler(func(string) error {
        conn.SetReadDeadline(time.Now().Add(s.config.ReadTimeout))
        return nil
    })

    // Start ping goroutine
    ctx, cancel := context.WithCancel(context.Background())
    defer cancel()

    go s.pingLoop(ctx, dc)

    // Read loop
    s.readLoop(dc, ctx)

    // Cleanup
    s.mu.Lock()
    if current, ok := s.connections[driverID]; ok && current == dc {
        delete(s.connections, driverID)
    }
    s.connCount--
    s.mu.Unlock()

    conn.Close()
    log.Printf("driver disconnected: %s total=%d", driverID, s.connCount)
}

// readLoop membaca messages dari WebSocket connection
func (s *IngestionServer) readLoop(dc *DriverConnection, ctx context.Context) {
    for {
        select {
        case <-ctx.Done():
            return
        default:
        }

        _, message, err := dc.Conn.ReadMessage()
        if err != nil {
            if websocket.IsUnexpectedCloseError(err, websocket.CloseGoingAway, websocket.CloseNormalClosure) {
                log.Printf("websocket read error driver=%s: %v", dc.DriverID, err)
            }
            return
        }

        // Parse location
        var loc model.LocationUpdate
        if err := json.Unmarshal(message, &loc); err != nil {
            log.Printf("invalid location json driver=%s: %v", dc.DriverID, err)
            continue
        }

        loc.DriverID = dc.DriverID
        loc.OrderID = dc.OrderID
        loc.ReceivedAt = time.Now().UTC()

        // Validate
        if err := s.validator.Validate(&loc, dc); err != nil {
            if _, ok := err.(*model.ValidationError); ok {
                log.Printf("validation error driver=%s: %v", dc.DriverID, err)
                // Send error back to driver
                s.sendError(dc, err.Error())
            }
            continue
        }

        // Publish to next stage
        if err := s.publisher.PublishLocation(ctx, &loc); err != nil {
            log.Printf("publish location driver=%s: %v", dc.DriverID, err)
            continue
        }

        dc.LastUpdate = time.Now()
    }
}

// pingLoop sends periodic pings to keep connection alive
func (s *IngestionServer) pingLoop(ctx context.Context, dc *DriverConnection) {
    ticker := time.NewTicker(s.config.PingInterval)
    defer ticker.Stop()

    for {
        select {
        case <-ctx.Done():
            return
        case <-ticker.C:
            dc.mu.Lock()
            err := dc.Conn.WriteControl(websocket.PingMessage, []byte{}, time.Now().Add(s.config.WriteTimeout))
            dc.mu.Unlock()
            if err != nil {
                log.Printf("ping error driver=%s: %v", dc.DriverID, err)
                return
            }
        }
    }
}

func (s *IngestionServer) sendError(dc *DriverConnection, msg string) {
    errMsg := map[string]string{"error": msg}
    data, _ := json.Marshal(errMsg)

    dc.mu.Lock()
    dc.Conn.SetWriteDeadline(time.Now().Add(s.config.WriteTimeout))
    dc.Conn.WriteMessage(websocket.TextMessage, data)
    dc.mu.Unlock()
}

// Shutdown gracefully stops the server
func (s *IngestionServer) Shutdown() {
    close(s.done)

    s.mu.RLock()
    defer s.mu.RUnlock()

    for _, dc := range s.connections {
        dc.Conn.WriteMessage(websocket.CloseMessage, websocket.FormatCloseMessage(
            websocket.CloseNormalClosure, "server shutdown"))
        dc.Conn.Close()
    }
}

package ingestion

import (
    "math"
    "sync"
    "time"

    "github.com/faisalaffan/qcommerce/internal/tracking/model"
)

// LocationValidator memvalidasi dan throttle location updates
type LocationValidator struct {
    config Config
    // lastUpdate: driverID -> last valid update time
    lastUpdate map[string]time.Time
    // lastPosition: driverID -> last valid position (for duplicate detection)
    lastPosition map[string]*model.LocationUpdate
    mu           sync.RWMutex
}

func NewLocationValidator(config Config) *LocationValidator {
    return &LocationValidator{
        config:       config,
        lastUpdate:   make(map[string]time.Time),
        lastPosition: make(map[string]*model.LocationUpdate),
    }
}

// Validate melakukan validasi lengkap + throttle
func (v *LocationValidator) Validate(loc *model.LocationUpdate, conn *DriverConnection) error {
    // 1. Format validation
    if err := loc.Validate(); err != nil {
        return err
    }

    v.mu.RLock()
    lastTime, hasLast := v.lastUpdate[loc.DriverID]
    lastPos, hasPos := v.lastPosition[loc.DriverID]
    v.mu.RUnlock()

    // 2. Throttle: minimum interval
    if hasLast {
        elapsed := time.Since(lastTime)
        if elapsed < v.config.MinUpdateInterval {
            return model.ErrThrottled
        }
    }

    // 3. Duplicate detection: skip if same coordinates
    if hasPos && hasLast {
        if haversineDistance(lastPos.Lat, lastPos.Lng, loc.Lat, loc.Lng) < 0.5 {
            // Less than 0.5 meters = basically same point
            return model.ErrDuplicate
        }
    }

    // 4. Speed sanity: max realistic speed between updates
    if hasPos && hasLast {
        distance := haversineDistance(lastPos.Lat, lastPos.Lng, loc.Lat, loc.Lng) // meters
        elapsed := loc.Timestamp.Sub(lastPos.Timestamp).Seconds()
        if elapsed > 0 {
            speed := distance / elapsed // m/s
            if speed > 55.6 { // >200 km/jam
                return model.ErrInvalidSpeed
            }
        }
    }

    // 5. Geofence: check if within operational area
    if err := v.checkGeofence(loc.Lat, loc.Lng); err != nil {
        return err
    }

    // Store last valid position
    v.mu.Lock()
    v.lastUpdate[loc.DriverID] = time.Now()
    v.lastPosition[loc.DriverID] = loc
    v.mu.Unlock()

    return nil
}

// checkGeofence: validasi apakah posisi dalam area operasional
func (v *LocationValidator) checkGeofence(lat, lng float64) error {
    // Production: load operational areas from config/DB
    // Simplified: Jakarta area
    if lat < -6.5 || lat > -6.0 || lng < 106.5 || lng > 107.0 {
        // Tidak dalam area Jakarta — log but don't reject
        // Driver might be in transit
    }
    return nil
}

// CleanupPeriodically: periodik cleanup map untuk mencegah memory leak
func (v *LocationValidator) CleanupPeriodically(ctx context.Context, interval time.Duration) {
    ticker := time.NewTicker(interval)
    defer ticker.Stop()

    for {
        select {
        case <-ctx.Done():
            return
        case <-ticker.C:
            v.mu.Lock()
            now := time.Now()
            for driverID, lastTime := range v.lastUpdate {
                if now.Sub(lastTime) > 10*time.Minute {
                    delete(v.lastUpdate, driverID)
                    delete(v.lastPosition, driverID)
                }
            }
            v.mu.Unlock()
        }
    }
}

// haversineDistance computes distance between two coordinates in meters
func haversineDistance(lat1, lng1, lat2, lng2 float64) float64 {
    R := 6371000.0 // Earth radius in meters

    dLat := (lat2 - lat1) * math.Pi / 180.0
    dLng := (lng2 - lng1) * math.Pi / 180.0

    a := math.Sin(dLat/2)*math.Sin(dLat/2) +
        math.Cos(lat1*math.Pi/180.0)*math.Cos(lat2*math.Pi/180.0)*
            math.Sin(dLng/2)*math.Sin(dLng/2)
    c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))

    return R * c
}

package smoother

import (
    "math"
    "time"

    "github.com/faisalaffan/qcommerce/internal/tracking/model"
)

// KalmanState adalah internal state Kalman filter
// State vector: [lat, lng, speed_lat, speed_lng]
// speed_lat/lng adalah velocity dalam degrees/second
type KalmanState struct {
    // State vector (4x1): [lat, lng, v_lat, v_lng]
    x [4]float64

    // Covariance matrix (4x4): uncertainty of estimate
    P [4][4]float64

    // Process noise covariance (how much we trust our motion model)
    Q [4][4]float64

    // Measurement noise covariance (how much we trust GPS)
    R [4][4]float64

    // Last update time
    lastUpdate time.Time

    // Is initialized
    initialized bool
}

// NewKalmanFilter creates a new Kalman filter for tracking
func NewKalmanFilter(processNoise, measurementNoise float64) *KalmanState {
    kf := &KalmanState{}

    // Initial state uncertainly: high (we don't know where we are)
    kf.P = [4][4]float64{
        {10, 0, 0, 0},
        {0, 10, 0, 0},
        {0, 0, 5, 0},
        {0, 0, 0, 5},
    }

    // Process noise: how much randomness in motion
    // Higher = trust GPS more, lower = trust prediction more
    kf.Q = [4][4]float64{
        {processNoise, 0, 0, 0},
        {0, processNoise, 0, 0},
        {0, 0, processNoise * 10, 0},
        {0, 0, 0, processNoise * 10},
    }

    // Measurement noise: GPS accuracy
    // Higher = trust GPS less, smooth more
    kf.R = [4][4]float64{
        {measurementNoise, 0, 0, 0},
        {0, measurementNoise, 0, 0},
        {0, 0, measurementNoise * 100, 0},
        {0, 0, 0, measurementNoise * 100},
    }

    return kf
}

// Initialize sets the initial state from first GPS reading
func (kf *KalmanState) Initialize(lat, lng float64, timestamp time.Time) {
    kf.x = [4]float64{lat, lng, 0, 0}
    kf.lastUpdate = timestamp
    kf.initialized = true
}

// Update processes a new GPS measurement through the Kalman filter
// Returns the estimated position after incorporating the measurement
func (kf *KalmanState) Update(lat, lng float64, accuracy float64, timestamp time.Time) model.SmoothedPosition {
    if !kf.initialized {
        kf.Initialize(lat, lng, timestamp)
        return model.SmoothedPosition{
            Lat:       lat,
            Lng:       lng,
            Speed:     0,
            Bearing:   0,
            Estimated: false,
            Timestamp: timestamp,
        }
    }

    // 1. PREDICT step: estimate position at current time using motion model
    dt := timestamp.Sub(kf.lastUpdate).Seconds()
    if dt <= 0 {
        dt = 0.1 // minimum delta
    }

    // State transition matrix F
    // [1, 0, dt, 0 ]
    // [0, 1, 0,  dt]
    // [0, 0, 1,  0 ]
    // [0, 0, 0,  1 ]
    F := [4][4]float64{
        {1, 0, dt, 0},
        {0, 1, 0, dt},
        {0, 0, 1, 0},
        {0, 0, 0, 1},
    }

    // Predict state: x = F * x
    x_pred := multiplyMatrixVector(F, kf.x)

    // Predict covariance: P = F * P * F^T + Q
    FP := multiplyMatrices(F, kf.P)
    FT := transposeMatrix(F)
    FPF := multiplyMatrices(FP, FT)
    P_pred := addMatrices(FPF, kf.Q)

    // 2. UPDATE step: incorporate measurement

    // Measurement matrix H (we measure lat and lng directly)
    // [1, 0, 0, 0]
    // [0, 1, 0, 0]
    H := [2][4]float64{
        {1, 0, 0, 0},
        {0, 1, 0, 0},
    }

    // Measurement noise from GPS accuracy
    // Adapt R based on reported accuracy
    adaptedR := kf.R
    accFactor := accuracy / 10.0
    if accFactor < 0.5 {
        accFactor = 0.5
    }
    if accFactor > 5.0 {
        accFactor = 5.0
    }
    adaptedR[0][0] = kf.R[0][0] * accFactor
    adaptedR[1][1] = kf.R[1][1] * accFactor

    // Innovation (measurement residual): y = z - H*x
    z := [2]float64{lat, lng}
    Hx := multiplyHMatrix(H, x_pred)
    y := subtractVectors(z, Hx)

    // Innovation covariance: S = H*P*H^T + R
    HP := multiplyHPMatrix(H, P_pred)
    HT := transposeHMatrix(H)
    HPH := multiplyHPHTMatrix(HP, HT)
    S := add2x2Matrices(HPH, adaptedR)

    // Kalman gain: K = P*H^T * S^-1
    PH := multiplyPHMatrix(P_pred, H)
    S_inv := invert2x2(S)
    K := multiplyKMatrix(PH, S_inv)

    // Update state: x = x_pred + K * y
    Ky := multiplyKVector(K, y)
    x_new := addVectors(x_pred, Ky)
    kf.x = x_new

    // Update covariance: P = (I - K*H) * P_pred
    KH := multiplyKMatrix(K, H)
    I := [4][4]float64{
        {1, 0, 0, 0},
        {0, 1, 0, 0},
        {0, 0, 1, 0},
        {0, 0, 0, 1},
    }
    IKH := subtractMatrices(I, KH)
    P_new := multiplyMatrices(IKH, P_pred)
    kf.P = P_new

    kf.lastUpdate = timestamp

    // Calculate speed and bearing from velocity state
    v_lat := x_new[2]
    v_lng := x_new[3]
    speed := math.Sqrt(v_lat*v_lat+v_lng*v_lng) * 111320.0 // convert deg/s to m/s
    bearing := math.Atan2(v_lng, v_lat) * 180.0 / math.Pi
    if bearing < 0 {
        bearing += 360
    }

    return model.SmoothedPosition{
        Lat:       x_new[0],
        Lng:       x_new[1],
        Speed:     speed,
        Bearing:   bearing,
        Estimated: false,
        Timestamp: timestamp,
    }
}

// PredictNext predicts the next position without measurement (for interpolation)
func (kf *KalmanState) PredictNext(lookAhead time.Duration) model.SmoothedPosition {
    if !kf.initialized {
        return model.SmoothedPosition{}
    }

    dt := lookAhead.Seconds()
    F := [4][4]float64{
        {1, 0, dt, 0},
        {0, 1, 0, dt},
        {0, 0, 1, 0},
        {0, 0, 0, 1},
    }

    pred := multiplyMatrixVector(F, kf.x)
    speed := math.Sqrt(pred[2]*pred[2]+pred[3]*pred[3]) * 111320.0
    bearing := math.Atan2(pred[3], pred[2]) * 180.0 / math.Pi
    if bearing < 0 {
        bearing += 360
    }

    return model.SmoothedPosition{
        Lat:       pred[0],
        Lng:       pred[1],
        Speed:     speed,
        Bearing:   bearing,
        Estimated: true,
        Timestamp: time.Now().Add(lookAhead),
    }
}

// --- Matrix operations (4x4, 2x4, 2x2, etc.) ---

func multiplyMatrixVector(A [4][4]float64, x [4]float64) [4]float64 {
    var result [4]float64
    for i := 0; i < 4; i++ {
        for j := 0; j < 4; j++ {
            result[i] += A[i][j] * x[j]
        }
    }
    return result
}

func multiplyMatrices(A, B [4][4]float64) [4][4]float64 {
    var result [4][4]float64
    for i := 0; i < 4; i++ {
        for j := 0; j < 4; j++ {
            for k := 0; k < 4; k++ {
                result[i][j] += A[i][k] * B[k][j]
            }
        }
    }
    return result
}

func transposeMatrix(A [4][4]float64) [4][4]float64 {
    var result [4][4]float64
    for i := 0; i < 4; i++ {
        for j := 0; j < 4; j++ {
            result[i][j] = A[j][i]
        }
    }
    return result
}

func addMatrices(A, B [4][4]float64) [4][4]float64 {
    var result [4][4]float64
    for i := 0; i < 4; i++ {
        for j := 0; j < 4; j++ {
            result[i][j] = A[i][j] + B[i][j]
        }
    }
    return result
}

func subtractMatrices(A, B [4][4]float64) [4][4]float64 {
    var result [4][4]float64
    for i := 0; i < 4; i++ {
        for j := 0; j < 4; j++ {
            result[i][j] = A[i][j] - B[i][j]
        }
    }
    return result
}

func multiplyHMatrix(H [2][4]float64, x [4]float64) [2]float64 {
    var result [2]float64
    for i := 0; i < 2; i++ {
        for j := 0; j < 4; j++ {
            result[i] += H[i][j] * x[j]
        }
    }
    return result
}

func subtractVectors(a, b [2]float64) [2]float64 {
    return [2]float64{a[0] - b[0], a[1] - b[1]}
}

func addVectors(a, b [4]float64) [4]float64 {
    var result [4]float64
    for i := 0; i < 4; i++ {
        result[i] = a[i] + b[i]
    }
    return result
}

func multiplyHPMatrix(H [2][4]float64, P [4][4]float64) [2][4]float64 {
    var result [2][4]float64
    for i := 0; i < 2; i++ {
        for j := 0; j < 4; j++ {
            for k := 0; k < 4; k++ {
                result[i][j] += H[i][k] * P[k][j]
            }
        }
    }
    return result
}

func transposeHMatrix(H [2][4]float64) [4][2]float64 {
    var result [4][2]float64
    for i := 0; i < 2; i++ {
        for j := 0; j < 4; j++ {
            result[j][i] = H[i][j]
        }
    }
    return result
}

func multiplyHPHTMatrix(A [2][4]float64, B [4][2]float64) [2][2]float64 {
    var result [2][2]float64
    for i := 0; i < 2; i++ {
        for j := 0; j < 2; j++ {
            for k := 0; k < 4; k++ {
                result[i][j] += A[i][k] * B[k][j]
            }
        }
    }
    return result
}

func add2x2Matrices(A, B [2][2]float64) [2][2]float64 {
    return [2][2]float64{
        {A[0][0] + B[0][0], A[0][1] + B[0][1]},
        {A[1][0] + B[1][0], A[1][1] + B[1][1]},
    }
}

func invert2x2(A [2][2]float64) [2][2]float64 {
    det := A[0][0]*A[1][1] - A[0][1]*A[1][0]
    if math.Abs(det) < 1e-10 {
        return A // singular matrix — return as-is
    }
    invDet := 1.0 / det
    return [2][2]float64{
        {A[1][1] * invDet, -A[0][1] * invDet},
        {-A[1][0] * invDet, A[0][0] * invDet},
    }
}

func multiplyKMatrix(K [4][2]float64, H [2][4]float64) [4][4]float64 {
    var result [4][4]float64
    for i := 0; i < 4; i++ {
        for j := 0; j < 4; j++ {
            for k := 0; k < 2; k++ {
                result[i][j] += K[i][k] * H[k][j]
            }
        }
    }
    return result
}

func multiplyKVector(K [4][2]float64, y [2]float64) [4]float64 {
    var result [4]float64
    for i := 0; i < 4; i++ {
        for j := 0; j < 2; j++ {
            result[i] += K[i][j] * y[j]
        }
    }
    return result
}

func multiplyPHMatrix(P [4][4]float64, H [2][4]float64) [4][2]float64 {
    var result [4][2]float64
    for i := 0; i < 4; i++ {
        for j := 0; j < 2; j++ {
            for k := 0; k < 4; k++ {
                result[i][j] += P[i][k] * H[j][k]
            }
        }
    }
    return result
}

package pubsub

import (
    "context"
    "encoding/json"
    "fmt"
    "log"
    "sync"
    "time"

    "github.com/redis/go-redis/v9"
    "github.com/faisalaffan/qcommerce/internal/tracking/model"
)

// RedisPubSub implements the LocationPublisher interface
// and provides subscription for user connections
type RedisPubSub struct {
    client      *redis.Client
    channelPrefix string
    lastPositions map[string]*model.SmoothedPosition // orderID -> last position
    mu           sync.RWMutex
}

func NewRedisPubSub(client *redis.Client, prefix string) *RedisPubSub {
    return &RedisPubSub{
        client:        client,
        channelPrefix: prefix,
        lastPositions: make(map[string]*model.SmoothedPosition),
    }
}

// orderChannel returns the Redis channel name for an order
func (ps *RedisPubSub) orderChannel(orderID string) string {
    return fmt.Sprintf("%s:tracking:%s", ps.channelPrefix, orderID)
}

// PublishLocation publishes a raw location update to Redis
func (ps *RedisPubSub) PublishLocation(ctx context.Context, loc *model.LocationUpdate) error {
    data, err := json.Marshal(loc)
    if err != nil {
        return fmt.Errorf("marshal location: %w", err)
    }

    channel := ps.orderChannel(loc.OrderID)

    // Publish to Redis — all subscribers of this order will get it
    result, err := ps.client.Publish(ctx, channel, string(data)).Result()
    if err != nil {
        return fmt.Errorf("redis publish: %w", err)
    }

    // result = number of subscribers that received the message
    if result == 0 {
        // No subscribers — position update is dropped (acceptable)
    }

    return nil
}

// PublishSmoothed publishes smoothed position to Redis
func (ps *RedisPubSub) PublishSmoothed(ctx context.Context, pos *model.SmoothedPosition) error {
    data, err := json.Marshal(pos)
    if err != nil {
        return fmt.Errorf("marshal smoothed position: %w", err)
    }

    channel := ps.orderChannel(pos.OrderID)

    _, err = ps.client.Publish(ctx, channel, string(data)).Result()
    if err != nil {
        return fmt.Errorf("redis publish smoothed: %w", err)
    }

    // Store last position for late subscribers
    ps.mu.Lock()
    ps.lastPositions[pos.OrderID] = pos
    ps.mu.Unlock()

    // Set expiry for last position (cleanup old entries)
    ps.client.Expire(ctx, channel, 30*time.Minute)

    return nil
}

// SubscribeOrder creates a subscription for a specific order
// Returns a channel of smoothed positions
func (ps *RedisPubSub) SubscribeOrder(ctx context.Context, orderID string) (<-chan *model.SmoothedPosition, error) {
    channel := ps.orderChannel(orderID)
    pubsub := ps.client.Subscribe(ctx, channel)

    // Wait for subscription to be active
    _, err := pubsub.Receive(ctx)
    if err != nil {
        return nil, fmt.Errorf("subscribe to %s: %w", channel, err)
    }

    ch := make(chan *model.SmoothedPosition, 100)
    go func() {
        defer pubsub.Close()

        // Send last known position immediately (for late subscribers)
        ps.mu.RLock()
        if last, ok := ps.lastPositions[orderID]; ok {
            ch <- last
        }
        ps.mu.RUnlock()

        ch <- &model.SmoothedPosition{
            OrderID:  orderID,
            Lat:      -6.2088, // Jakarta default
            Lng:      106.8456,
            Speed:    0,
            Estimated: true,
            Timestamp: time.Now(),
        }

        msgCh := pubsub.Channel()
        for {
            select {
            case <-ctx.Done():
                close(ch)
                return
            case msg, ok := <-msgCh:
                if !ok {
                    close(ch)
                    return
                }

                // Try to parse as SmoothedPosition first
                var pos model.SmoothedPosition
                if err := json.Unmarshal([]byte(msg.Payload), &pos); err == nil && pos.OrderID != "" {
                    select {
                    case ch <- &pos:
                    default:
                        // Channel full, drop message (position updates are high frequency)
                    }
                    continue
                }

                // Fallback: parse as LocationUpdate and convert
                var loc model.LocationUpdate
                if err := json.Unmarshal([]byte(msg.Payload), &loc); err == nil {
                    smoothed := &model.SmoothedPosition{
                        OrderID:   loc.OrderID,
                        DriverID:  loc.DriverID,
                        Lat:       loc.Lat,
                        Lng:       loc.Lng,
                        Speed:     loc.Speed,
                        Bearing:   loc.Bearing,
                        Estimated: false,
                        Timestamp: loc.Timestamp,
                    }
                    select {
                    case ch <- smoothed:
                    default:
                    }
                }
            }
        }
    }()

    return ch, nil
}

// UnsubscribeOrder removes a subscription
func (ps *RedisPubSub) UnsubscribeOrder(ctx context.Context, orderID string) error {
    channel := ps.orderChannel(orderID)
    return ps.client.Unsubscribe(ctx, channel).Err()
}

// CleanupOldPositions periodically removes stale last positions
func (ps *RedisPubSub) CleanupOldPositions(ctx context.Context, interval time.Duration) {
    ticker := time.NewTicker(interval)
    defer ticker.Stop()

    for {
        select {
        case <-ctx.Done():
            return
        case <-ticker.C:
            ps.mu.Lock()
            now := time.Now()
            for orderID, pos := range ps.lastPositions {
                if now.Sub(pos.Timestamp) > 15*time.Minute {
                    delete(ps.lastPositions, orderID)
                }
            }
            ps.mu.Unlock()
        }
    }
}

package push

import (
    "context"
    "encoding/json"
    "fmt"
    "log"
    "net/http"
    "sync"
    "time"

    "github.com/faisalaffan/qcommerce/internal/tracking/model"
)

// ConnectionManager manages user SSE connections
type ConnectionManager struct {
    // orderConnections maps orderID -> set of user connections
    orderConnections map[string]map[*UserConnection]struct{}
    mu               sync.RWMutex
}

// UserConnection represents a single SSE connection
type UserConnection struct {
    UserID     string
    OrderID    string
    flusher    http.Flusher
    writer     http.ResponseWriter
    requestCtx context.Context
    done       chan struct{}
    mu         sync.Mutex
}

func NewConnectionManager() *ConnectionManager {
    return &ConnectionManager{
        orderConnections: make(map[string]map[*UserConnection]struct{}),
    }
}

// HandleSSE is the HTTP handler for SSE connections
func (cm *ConnectionManager) HandleSSE(w http.ResponseWriter, r *http.Request) {
    userID := r.URL.Query().Get("user_id")
    orderID := r.URL.Query().Get("order_id")

    if userID == "" || orderID == "" {
        http.Error(w, "missing user_id or order_id", http.StatusBadRequest)
        return
    }

    // Set SSE headers
    w.Header().Set("Content-Type", "text/event-stream")
    w.Header().Set("Cache-Control", "no-cache")
    w.Header().Set("Connection", "keep-alive")
    w.Header().Set("Access-Control-Allow-Origin", "*")

    flusher, ok := w.(http.Flusher)
    if !ok {
        http.Error(w, "streaming not supported", http.StatusInternalServerError)
        return
    }

    conn := &UserConnection{
        UserID:     userID,
        OrderID:    orderID,
        flusher:    flusher,
        writer:     w,
        requestCtx: r.Context(),
        done:       make(chan struct{}),
    }

    // Register connection
    cm.register(conn)
    defer cm.unregister(conn)

    log.Printf("SSE connected: user=%s order=%s", userID, orderID)

    // Send initial connection event
    cm.sendEvent(conn, "connected", map[string]string{
        "user_id": userID,
        "order_id": orderID,
    })

    // Keep connection alive with periodic comments
    ticker := time.NewTicker(15 * time.Second)
    defer ticker.Stop()

    for {
        select {
        case <-r.Context().Done():
            log.Printf("SSE disconnected: user=%s order=%s", userID, orderID)
            return
        case <-ticker.C:
            // Send keepalive comment
            conn.mu.Lock()
            fmt.Fprintf(conn.writer, ": keepalive\n\n")
            conn.flusher.Flush()
            conn.mu.Unlock()
        }
    }
}

// PushPosition sends a position update to all subscribers of an order
func (cm *ConnectionManager) PushPosition(orderID string, pos *model.SmoothedPosition) {
    cm.mu.RLock()
    connections, ok := cm.orderConnections[orderID]
    cm.mu.RUnlock()

    if !ok {
        return // no subscribers
    }

    data, err := json.Marshal(pos)
    if err != nil {
        log.Printf("marshal position: %v", err)
        return
    }

    for conn := range connections {
        cm.sendEvent(conn, "position", string(data))
    }
}

// PushETA sends estimated time of arrival to order subscribers
func (cm *ConnectionManager) PushETA(orderID string, etaMinutes int) {
    cm.mu.RLock()
    connections, ok := cm.orderConnections[orderID]
    cm.mu.RUnlock()

    if !ok {
        return
    }

    for conn := range connections {
        cm.sendEvent(conn, "eta", map[string]int{
            "minutes": etaMinutes,
        })
    }
}

// PushStatus sends order status update
func (cm *ConnectionManager) PushStatus(orderID string, status string) {
    cm.mu.RLock()
    connections, ok := cm.orderConnections[orderID]
    cm.mu.RUnlock()

    if !ok {
        return
    }

    for conn := range connections {
        cm.sendEvent(conn, "status", map[string]string{
            "status": status,
        })
    }
}

// sendEvent sends an SSE event to a connection
func (cm *ConnectionManager) sendEvent(conn *UserConnection, event string, data interface{}) {
    conn.mu.Lock()
    defer conn.mu.Unlock()

    var dataStr string
    switch v := data.(type) {
    case string:
        dataStr = v
    default:
        b, err := json.Marshal(v)
        if err != nil {
            log.Printf("marshal event data: %v", err)
            return
        }
        dataStr = string(b)
    }

    fmt.Fprintf(conn.writer, "event: %s\n", event)
    fmt.Fprintf(conn.writer, "data: %s\n\n", dataStr)
    conn.flusher.Flush()
}

func (cm *ConnectionManager) register(conn *UserConnection) {
    cm.mu.Lock()
    defer cm.mu.Unlock()

    if _, ok := cm.orderConnections[conn.OrderID]; !ok {
        cm.orderConnections[conn.OrderID] = make(map[*UserConnection]struct{})
    }
    cm.orderConnections[conn.OrderID][conn] = struct{}{}
}

func (cm *ConnectionManager) unregister(conn *UserConnection) {
    cm.mu.Lock()
    defer cm.mu.Unlock()

    if connections, ok := cm.orderConnections[conn.OrderID]; ok {
        delete(connections, conn)
        if len(connections) == 0 {
            delete(cm.orderConnections, conn.OrderID)
        }
    }
}

// GetSubscriberCount returns number of subscribers for an order
func (cm *ConnectionManager) GetSubscriberCount(orderID string) int {
    cm.mu.RLock()
    defer cm.mu.RUnlock()
    return len(cm.orderConnections[orderID])
}

// FanOutWorker menghubungkan RedisPubSub ke SSE ConnectionManager
type FanOutWorker struct {
    pubsub *RedisPubSub
    connMgr *ConnectionManager
    workerCount int
}

func NewFanOutWorker(pubsub *RedisPubSub, connMgr *ConnectionManager, workerCount int) *FanOutWorker {
    return &FanOutWorker{
        pubsub:    pubsub,
        connMgr:   connMgr,
        workerCount: workerCount,
    }
}

// ListenForOrders starts workers that subscribe to Redis for active orders
func (w *FanOutWorker) ListenForOrders(ctx context.Context) {
    var wg sync.WaitGroup
    for i := 0; i < w.workerCount; i++ {
        wg.Add(1)
        go func(workerID int) {
            defer wg.Done()
            log.Printf("fan-out worker %d started", workerID)
            // In production: listen to a Redis channel for new active orders
            // and create subscriptions dynamically
            <-ctx.Done()
        }(i)
    }
    wg.Wait()
}