dwarf-go/dwarfctl/internal/daemon/server.go

// Package daemon implements a persistent telescope service that keeps the
// WebSocket connection open, manages cameras, grabs RTSP frames, and runs the
// visual odometry tracker — exposing everything via a local HTTP API so that
// short-lived CLI invocations can control a long-lived session.
package daemon

import (
	"context"
	"encoding/json"
	"fmt"
	"io"
	"log"
	"net/http"
	"os"
	"os/signal"
	"sync"
	"syscall"
	"time"

	"github.com/antitbone/dwarfctl/internal/api"
	"github.com/antitbone/dwarfctl/internal/odometry"
	"github.com/antitbone/dwarfctl/internal/rtspgrab"
)

// DefaultAddr is the default HTTP listen address for the daemon.
const DefaultAddr = "127.0.0.1:7777"

// Config configures the daemon.
type Config struct {
	IP       string
	Addr     string
	FoVH     float64
	FoVV     float64
	GridSize int
	Camera   string // "wide" or "tele"
	Interval time.Duration // odometry frame interval
	SettleDelay time.Duration // pause after motor stop before resuming
	Debug    bool
}

// Server is the persistent telescope daemon.
type Server struct {
	cfg     Config
	scope   *api.Telescope
	tracker *odometry.Tracker

	mu      sync.Mutex
	camera  api.Camera
	camOpen bool
	stopCh  chan struct{}
	wg      sync.WaitGroup

	// paused disables odometry frame capture (e.g. during motor slew to avoid
	// motion blur which destroys phase correlation).
	paused bool
}

// New creates a daemon server.
func New(cfg Config) *Server {
	if cfg.Addr == "" {
		cfg.Addr = DefaultAddr
	}
	if cfg.FoVH <= 0 {
		cfg.FoVH = odometry.DefaultFoVH
	}
	if cfg.FoVV <= 0 {
		cfg.FoVV = odometry.DefaultFoVV
	}
	if cfg.GridSize <= 0 {
		cfg.GridSize = odometry.DefaultSize
	}
	if cfg.Camera == "" {
		cfg.Camera = "wide"
	}
	if cfg.Interval <= 0 {
		cfg.Interval = 5 * time.Second
	}
	if cfg.SettleDelay <= 0 {
		cfg.SettleDelay = 3 * time.Second
	}
	return &Server{
		cfg:     cfg,
		stopCh:  make(chan struct{}),
		tracker: odometry.NewTracker(cfg.FoVH, cfg.FoVV, cfg.GridSize),
	}
}

// Run starts the daemon: connects to telescope, opens camera, starts odometry,
// and serves HTTP until SIGTERM/SIGINT is received (systemd-compatible).
func (s *Server) Run() error {
	// 1. Connect to telescope.
	s.scope = api.New("dwarfctl-daemon")
	s.scope.SetDebug(s.cfg.Debug)
	log.Printf("Connecting to telescope at %s ...", s.cfg.IP)
	if err := s.scope.Connect(s.cfg.IP); err != nil {
		return fmt.Errorf("connect: %w", err)
	}
	log.Println("Connected.")

	// 2. Open camera.
	s.camera = api.CameraTele
	if s.cfg.Camera == "wide" {
		s.camera = api.CameraWide
	}
	log.Printf("Opening %s camera...", s.cfg.Camera)
	// Switch to shooting mode 1 (photo/preview) first — required for streaming.
	s.scope.SwitchShootingMode(1)
	if err := s.scope.OpenCamera(s.camera); err != nil {
		return fmt.Errorf("open camera: %w", err)
	}
	s.camOpen = true
	log.Println("Camera opened. Waiting 5s for RTSP to initialize...")
	time.Sleep(5 * time.Second)

	// 3. Start odometry loop in background.
	s.wg.Add(1)
	go s.odometryLoop()

	// 4. HTTP server.
	mux := http.NewServeMux()
	s.registerRoutes(mux)
	srv := &http.Server{Addr: s.cfg.Addr, Handler: mux}

	// 5. Signal handling for systemd (SIGTERM/SIGINT → clean shutdown).
	go func() {
		sigCh := make(chan os.Signal, 1)
		signal.Notify(sigCh, syscall.SIGTERM, syscall.SIGINT)
		sig := <-sigCh
		log.Printf("Received %s — shutting down...", sig)
		close(s.stopCh)
		ctx, cancel := context.WithTimeout(context.Background(), 5*time.Second)
		defer cancel()
		srv.Shutdown(ctx)
	}()

	log.Printf("Daemon listening on http://%s (Ctrl-C to stop)", s.cfg.Addr)
	if err := srv.ListenAndServe(); err != nil && err != http.ErrServerClosed {
		return err
	}

	// Cleanup.
	s.scope.CloseCamera(s.camera)
	s.scope.Close()
	s.wg.Wait()
	log.Println("Daemon stopped.")
	return nil
}

// Stop shuts down the daemon.
func (s *Server) Stop() {
	close(s.stopCh)
	if s.scope != nil {
		s.scope.CloseCamera(s.camera)
		s.scope.Close()
	}
}

// odometryLoop continuously grabs RTSP frames and feeds the tracker.
func (s *Server) odometryLoop() {
	defer s.wg.Done()
	ch := s.rtspChannel()
	url := fmt.Sprintf("rtsp://%s:554/%s/stream0", s.cfg.IP, ch)
	tmpFile := "/tmp/dwarf_daemon_frame.jpg"

	ticker := time.NewTicker(s.cfg.Interval)
	defer ticker.Stop()

	// Prime with first frame.
	if img, err := rtspgrab.GrabFrame(url, tmpFile); err == nil {
		s.tracker.Update(img)
		log.Printf("Odometry primed (grid=%d, fov=%.1fx%.1f)", s.cfg.GridSize, s.cfg.FoVH, s.cfg.FoVV)
	} else {
		log.Printf("Odometry prime failed: %v", err)
	}

	for {
		select {
		case <-s.stopCh:
			return
		case <-ticker.C:
			// Skip frame capture while motors are moving (motion blur destroys
			// phase correlation). The slew handler sets paused=true/false.
			s.mu.Lock()
			if s.paused {
				s.mu.Unlock()
				continue
			}
			s.mu.Unlock()

			img, err := rtspgrab.GrabFrame(url, tmpFile)
			if err != nil {
				if s.cfg.Debug {
					log.Printf("grab error: %v", err)
				}
				continue
			}
			o, err := s.tracker.Update(img)
			if err != nil {
				log.Printf("odometry error: %v", err)
				continue
			}
			if s.cfg.Debug {
				log.Printf("[odometry] %s", o)
			}
		}
	}
}

func (s *Server) rtspChannel() string {
	s.mu.Lock()
	defer s.mu.Unlock()
	if s.camera == api.CameraWide {
		return "ch1"
	}
	return "ch0"
}

// --- HTTP API ---

func (s *Server) registerRoutes(mux *http.ServeMux) {
	mux.HandleFunc("/health", s.handleHealth)
	mux.HandleFunc("/orientation", s.handleOrientation)
	mux.HandleFunc("/grab", s.handleGrab)
	mux.HandleFunc("/reset", s.handleReset)
	mux.HandleFunc("/camera", s.handleCamera)
	mux.HandleFunc("/slew", s.handleSlew)
	mux.HandleFunc("/stop", s.handleStop)
	mux.HandleFunc("/pause", s.handlePause)
	mux.HandleFunc("/resume", s.handleResume)
	mux.HandleFunc("/fov", s.handleFoV)
}

func writeJSON(w http.ResponseWriter, v any) {
	w.Header().Set("Content-Type", "application/json")
	json.NewEncoder(w).Encode(v)
}

func (s *Server) handleHealth(w http.ResponseWriter, _ *http.Request) {
	writeJSON(w, map[string]any{
		"status":   "ok",
		"ip":       s.cfg.IP,
		"camera":   s.cfg.Camera,
		"cam_open": s.camOpen,
	})
}

func (s *Server) handleOrientation(w http.ResponseWriter, _ *http.Request) {
	writeJSON(w, s.tracker.Orientation())
}

func (s *Server) handleGrab(w http.ResponseWriter, r *http.Request) {
	ch := s.rtspChannel()
	url := fmt.Sprintf("rtsp://%s:554/%s/stream0", s.cfg.IP, ch)
	out := "/tmp/dwarf_daemon_grab.jpg"
	if q := r.URL.Query().Get("path"); q != "" {
		out = q
	}
	if _, err := rtspgrab.GrabFrame(url, out); err != nil {
		http.Error(w, err.Error(), http.StatusInternalServerError)
		return
	}
	if r.URL.Query().Get("raw") == "1" {
		data, _ := os.ReadFile(out)
		w.Header().Set("Content-Type", "image/jpeg")
		w.Write(data)
		return
	}
	writeJSON(w, map[string]string{"path": out})
}

func (s *Server) handleReset(w http.ResponseWriter, _ *http.Request) {
	s.tracker.Reset()
	writeJSON(w, map[string]string{"status": "reset"})
}

func (s *Server) handleCamera(w http.ResponseWriter, r *http.Request) {
	action := r.URL.Query().Get("action") // open/close
	cam := r.URL.Query().Get("cam")       // wide/tele

	s.mu.Lock()
	target := s.camera
	if cam == "wide" {
		target = api.CameraWide
	} else if cam == "tele" {
		target = api.CameraTele
	}

	var err error
	switch action {
	case "open":
		err = s.scope.OpenCamera(target)
		if err == nil {
			s.camera = target
			s.camOpen = true
		}
	case "close":
		err = s.scope.CloseCamera(target)
		if err == nil {
			s.camOpen = false
		}
	default:
		err = fmt.Errorf("action must be 'open' or 'close'")
	}
	s.mu.Unlock()

	if err != nil {
		http.Error(w, err.Error(), http.StatusBadRequest)
		return
	}
	writeJSON(w, map[string]any{"camera": cam, "action": action, "ok": true})
}

func (s *Server) handleSlew(w http.ResponseWriter, r *http.Request) {
	var req struct {
		Angle  float64 `json:"angle"`
		Length float64 `json:"length"`
	}
	if r.Body != nil {
		body, _ := io.ReadAll(r.Body)
		json.Unmarshal(body, &req)
	}
	// Pause odometry during slew to avoid motion blur.
	s.mu.Lock()
	s.paused = true
	s.mu.Unlock()

	if err := s.scope.SlewJoystick(req.Angle, req.Length); err != nil {
		http.Error(w, err.Error(), http.StatusInternalServerError)
		return
	}
	writeJSON(w, map[string]any{"angle": req.Angle, "length": req.Length, "odometry": "paused"})
}

func (s *Server) handleStop(w http.ResponseWriter, _ *http.Request) {
	s.scope.MotorJoystickStop()
	// Keep odometry paused — a settle delay prevents motion-blurred frames
	// from corrupting the tracker. The odometry loop auto-resumes after the
	// delay via a timer.
	s.mu.Lock()
	s.paused = true
	s.mu.Unlock()
	go func() {
		time.Sleep(s.cfg.SettleDelay)
		s.mu.Lock()
		s.paused = false
		s.mu.Unlock()
	}()
	writeJSON(w, map[string]string{"status": "motors stopped, settling..."})
}

func (s *Server) handlePause(w http.ResponseWriter, _ *http.Request) {
	s.mu.Lock()
	s.paused = true
	s.mu.Unlock()
	writeJSON(w, map[string]string{"status": "odometry paused"})
}

func (s *Server) handleResume(w http.ResponseWriter, _ *http.Request) {
	s.mu.Lock()
	s.paused = false
	s.mu.Unlock()
	writeJSON(w, map[string]string{"status": "odometry resumed"})
}

func (s *Server) handleFoV(w http.ResponseWriter, r *http.Request) {
	if r.Method == http.MethodPost {
		var req struct {
			H float64 `json:"h"`
			V float64 `json:"v"`
		}
		body, _ := io.ReadAll(r.Body)
		json.Unmarshal(body, &req)
		s.tracker.SetFoV(req.H, req.V)
		s.cfg.FoVH = req.H
		s.cfg.FoVV = req.V
	}
	writeJSON(w, map[string]float64{"fov_h": s.cfg.FoVH, "fov_v": s.cfg.FoVV})
}