SatMaster/backend/rotor/pstrotator.go

package rotor

import (
	"fmt"
	"log"
	"math"
	"net"
	"sync"
	"sync/atomic"
	"time"
)

// PstRotatorClient controls PstRotator via UDP XML protocol.
// Protocol: send XML to port N, responses come back on port N+1
// Format: <AZIMUTH>180.0</AZIMUTH>  or  <ELEVATION>45.0</ELEVATION>
// Multiple commands: <AZIMUTH>180.0</AZIMUTH><ELEVATION>45.0</ELEVATION>
type PstRotatorClient struct {
	mu          sync.Mutex
	conn        *net.UDPConn
	addr        *net.UDPAddr
	connected   atomic.Bool
	lastAz      float64
	lastEl      float64
	azThreshold float64
	elThreshold float64
}

func NewPstRotatorClient() *PstRotatorClient {
	return &PstRotatorClient{
		azThreshold: 5.0, // 5° dead-band for Az
		elThreshold: 5.0,
	}
}

func (r *PstRotatorClient) Connect(host string, port int) error {
	r.mu.Lock()
	defer r.mu.Unlock()

	if r.connected.Load() {
		r.disconnectLocked()
	}

	addr, err := net.ResolveUDPAddr("udp", fmt.Sprintf("%s:%d", host, port))
	if err != nil {
		return fmt.Errorf("resolve UDP addr: %w", err)
	}

	conn, err := net.DialUDP("udp", nil, addr)
	if err != nil {
		return fmt.Errorf("UDP connect to PstRotator: %w", err)
	}

	r.conn = conn
	r.addr = addr
	r.connected.Store(true)
	r.lastAz = -999
	r.lastEl = -999

	log.Printf("[Rotor] Connected to PstRotator at %s:%d", host, port)
	return nil
}

func (r *PstRotatorClient) Disconnect() {
	r.mu.Lock()
	defer r.mu.Unlock()
	r.disconnectLocked()
}

func (r *PstRotatorClient) disconnectLocked() {
	if r.conn != nil {
		r.conn.Close()
		r.conn = nil
	}
	r.connected.Store(false)
	log.Println("[Rotor] Disconnected")
}

func (r *PstRotatorClient) IsConnected() bool {
	return r.connected.Load()
}

// SetAzEl sends azimuth (and optionally elevation) to PstRotator.
// El < 0 is treated as azimuth-only (no elevation rotor).
func (r *PstRotatorClient) SetAzEl(az, el float64) error {
	if !r.connected.Load() {
		return fmt.Errorf("not connected to PstRotator")
	}

	// Normalize azimuth 0-360
	az = math.Mod(az, 360.0)
	if az < 0 {
		az += 360
	}
	// Clamp elevation
	if el < 0 {
		el = 0
	}
	if el > 90 {
		el = 90
	}

	r.mu.Lock()
	defer r.mu.Unlock()

	if r.conn == nil {
		return fmt.Errorf("not connected")
	}

	// Dead-band check (inside mutex — no race on lastAz/lastEl)
	azChanged := math.Abs(az-r.lastAz) >= r.azThreshold
	elChanged := math.Abs(el-r.lastEl) >= r.elThreshold

	if !azChanged && !elChanged {
		return nil
	}

	// Build XML command wrapped in <PST>...</PST>
	var cmd string
	if azChanged && elChanged {
		cmd = fmt.Sprintf("<PST><AZIMUTH>%.0f</AZIMUTH><ELEVATION>%.0f</ELEVATION></PST>", az, el)
	} else if azChanged {
		cmd = fmt.Sprintf("<PST><AZIMUTH>%.0f</AZIMUTH></PST>", az)
	} else {
		cmd = fmt.Sprintf("<PST><ELEVATION>%.0f</ELEVATION></PST>", el)
	}

	log.Printf("[Rotor] UDP → %s", cmd)
	r.conn.SetWriteDeadline(time.Now().Add(2 * time.Second))
	if _, err := r.conn.Write([]byte(cmd)); err != nil {
		r.connected.Store(false)
		return fmt.Errorf("UDP write: %w", err)
	}

	if azChanged {
		r.lastAz = az
	}
	if elChanged {
		r.lastEl = el
	}
	log.Printf("[Rotor] → AZ=%.1f° EL=%.1f°", az, el)
	return nil
}

// SetAzOnly sends only azimuth — for stations with no elevation rotor.
func (r *PstRotatorClient) SetAzOnly(az float64) error {
	if !r.connected.Load() {
		return fmt.Errorf("not connected")
	}
	az = math.Mod(az, 360.0)
	if az < 0 {
		az += 360
	}

	r.mu.Lock()
	defer r.mu.Unlock()

	if r.conn == nil {
		return fmt.Errorf("not connected")
	}

	// Dead-band check (inside mutex — no race on lastAz)
	if math.Abs(az-r.lastAz) < r.azThreshold {
		return nil
	}

	cmd := fmt.Sprintf("<PST><AZIMUTH>%.0f</AZIMUTH></PST>", az)
	log.Printf("[Rotor] UDP → %s", cmd)
	r.conn.SetWriteDeadline(time.Now().Add(2 * time.Second))
	if _, err := r.conn.Write([]byte(cmd)); err != nil {
		r.connected.Store(false)
		return fmt.Errorf("UDP write: %w", err)
	}
	r.lastAz = az
	log.Printf("[Rotor] → AZ=%.1f°", az)
	return nil
}

// StopRotor sends stop command.
func (r *PstRotatorClient) StopRotor() error {
	return r.sendRaw("<PST><STOP>1</STOP></PST>")
}

// QueryAzEl requests current position — answer comes back on port+1.
func (r *PstRotatorClient) QueryAzEl() error {
	return r.sendRaw("<AZ?>1</AZ?><EL?>1</EL?>")
}

func (r *PstRotatorClient) sendRaw(cmd string) error {
	r.mu.Lock()
	defer r.mu.Unlock()

	if r.conn == nil {
		return fmt.Errorf("not connected")
	}

	log.Printf("[Rotor] UDP → %s", cmd)
	r.conn.SetWriteDeadline(time.Now().Add(2 * time.Second))
	n, err := r.conn.Write([]byte(cmd))
	if err != nil {
		r.connected.Store(false)
		return fmt.Errorf("UDP write: %w", err)
	}
	log.Printf("[Rotor] UDP sent %d bytes to %s", n, r.addr)
	return nil
}

// ResetDeadband forces next azimuth command to be sent immediately.
func (r *PstRotatorClient) ResetDeadband() {
	r.mu.Lock()
	defer r.mu.Unlock()
	r.lastAz = -999
	r.lastEl = -999
}

func (r *PstRotatorClient) SetThresholds(azDeg, elDeg float64) {
	r.mu.Lock()
	defer r.mu.Unlock()
	r.azThreshold = azDeg
	r.elThreshold = elDeg
}