go-emlid/cmd/average_position/main.go

package main

import (
	"context"
	"encoding/json"
	"flag"
	"fmt"
	"log/slog"
	"os"
	"time"

	reach "forge.cadoles.com/cadoles/go-emlid/reach/client"
	"forge.cadoles.com/cadoles/go-emlid/reach/client/logger"
	"forge.cadoles.com/cadoles/go-emlid/reach/client/protocol"
	"forge.cadoles.com/cadoles/go-emlid/reach/client/protocol/v2/model"
	"github.com/mitchellh/mapstructure"
	"github.com/pkg/errors"
)

var (
	host        string = "192.168.42.1"
	rawLogLevel string = "ERROR"
)

func init() {
	flag.StringVar(&rawLogLevel, "log-level", rawLogLevel, "log level")
	flag.StringVar(&host, "host", host, "the reachrs module host")
}

type Coordinates struct {
	Latitude  float64 `json:"latitude"`
	Longitude float64 `json:"longitude"`
	Height    float64 `json:"height"`
}

type Payload struct {
	Coordinates   Coordinates `json:"coordinates"`
	AntennaOffset float64     `json:"antenna_offset"`
}

type BaseConfig struct {
	Latitude      float64
	Longitude     float64
	Height        float64
	AntennaOffset float64
}

func main() {
	flag.Parse()

	ctx := context.Background()

	client, err := initializeClient(ctx)
	if err != nil {
		fmt.Printf("[FATAL] %+v", err)
		os.Exit(1)
	}
	defer closeClient(ctx, client)

	// Récupération de la configuration
	config, err := retrieveAndProcessConfig(ctx, client)
	if err != nil {
		fmt.Printf("[FATAL] %+v", err)
		os.Exit(1)
	}

	baseConfig := extractBaseConfig(config)
	fmt.Printf("Configuration actuelle de la base :\n lat=%v\n lon=%v\n height=%v\n antenna_offset=%v\n\n NTRIPSettings: \n address:%s\n port:%d \n username: %s\n password:%s\n sendPositionToBase:%t\n",
		baseConfig.Latitude, baseConfig.Longitude, baseConfig.Height, baseConfig.AntennaOffset, config.CorrectionInput.BaseCorrections.Settings.Ntripcli.Address, config.CorrectionInput.BaseCorrections.Settings.Ntripcli.Port, config.CorrectionInput.BaseCorrections.Settings.Ntripcli.Username, config.CorrectionInput.BaseCorrections.Settings.Ntripcli.Password, config.CorrectionInput.BaseCorrections.Settings.Ntripcli.SendPositionToBase)

	// Configuration des corrections NTRIP
	if err := setupNTRIPCorrections(ctx, client, config); err != nil {
		fmt.Printf("[FATAL] %+v", err)
		os.Exit(1)
	}

	//Configuration de la base
	if err := setBaseToModeAndHold(ctx, client, baseConfig, "float"); err != nil {
		fmt.Printf("[FATAL] %+v", err)
		os.Exit(1)
	}

	// Collecte des données de positions
	if err := averagePositionAndSave(ctx, client); err != nil {
		fmt.Printf("[FATAL] %+v", err)
		os.Exit(1)
	}

	fmt.Println("Configuration terminée avec succès")
}

func initializeClient(ctx context.Context) (*reach.Client, error) {
	logLevel, err := logger.ParseLevel(rawLogLevel)
	if err != nil {
		return nil, errors.WithStack(err)
	}

	slog.SetLogLoggerLevel(logLevel)

	client := reach.NewClient(host)
	if err := client.Connect(ctx); err != nil {
		return nil, errors.WithStack(err)
	}

	return client, nil
}

func closeClient(ctx context.Context, client *reach.Client) {
	if err := client.Close(ctx); err != nil {
		fmt.Printf("[ERROR] Erreur lors de la fermeture: %+v", errors.WithStack(err))
	}
}

func retrieveAndProcessConfig(ctx context.Context, client *reach.Client) (*model.Configuration, error) {
	configData, err := client.Configuration(ctx)
	if err != nil {
		return nil, errors.WithStack(err)
	}

	var config model.Configuration
	if err := mapstructure.Decode(configData, &config); err != nil {
		return nil, errors.WithStack(err)
	}

	return &config, nil
}

func extractBaseConfig(config *model.Configuration) BaseConfig {
	return BaseConfig{
		Latitude:      config.BaseMode.BaseCoordinates.Coordinates.Latitude,
		Longitude:     config.BaseMode.BaseCoordinates.Coordinates.Longitude,
		Height:        config.BaseMode.BaseCoordinates.Coordinates.Height,
		AntennaOffset: config.BaseMode.BaseCoordinates.AntennaOffset,
	}
}

func setupNTRIPCorrections(ctx context.Context, client *reach.Client, config *model.Configuration) error {
	fmt.Println("\nConfiguration des corrections NTRIP...")

	// Recherche de points de montage
	if err := client.GetNTRIPMountPoint(ctx); err != nil {
		return errors.WithStack(err)
	}

	return processNTRIPStreams(ctx, client, config)
}

func processNTRIPStreams(ctx context.Context, client *reach.Client, config *model.Configuration) error {
	messages, err := reach.OnMessageType(ctx, client, "task_status")
	if err != nil {
		return errors.WithStack(err)
	}

	timeout := time.NewTimer(30 * time.Second)
	defer timeout.Stop()

	for {
		select {
		case b := <-messages:
			if b.State == "completed" {
				return handleNTRIPResponse(ctx, client, config, b)
			}

		case <-timeout.C:
			return errors.New("timeout lors de la récupération des streams NTRIP")

		case <-ctx.Done():
			return ctx.Err()
		}
	}
}

func handleNTRIPResponse(ctx context.Context, client *reach.Client, config *model.Configuration, message reach.Broadcast) error {
	var response model.NTRIPResponse
	if err := mapstructure.Decode(message, &response); err != nil {
		return errors.WithStack(err)
	}

	displayAvailableStreams(response)

	return updateNTRIPMountPoint(ctx, client, config)
}

func displayAvailableStreams(response model.NTRIPResponse) {
	streams := response.Payload.Str
	fmt.Printf("=== %d Streams disponibles ===\n", len(streams))
	fmt.Printf("=== Base < 50 km disponibles ===\n")

	for i, stream := range streams {
		if stream.Distance < 50 {
			fmt.Printf("%d. %-15s | %s (%s) | %.1fkm\n",
				i+1,
				stream.Mountpoint,
				stream.ID,
				stream.Country,
				stream.Distance)
		}
	}
}

func updateNTRIPMountPoint(ctx context.Context, client *reach.Client, config *model.Configuration) error {
	opts := []protocol.SetBaseCorrectionsFunc{
		protocol.WithNTRIPAddress(config.CorrectionInput.BaseCorrections.Settings.Ntripcli.Address),
		protocol.WithNTRIPPort(config.CorrectionInput.BaseCorrections.Settings.Ntripcli.Port),
		protocol.WithNTRIPUsername(config.CorrectionInput.BaseCorrections.Settings.Ntripcli.Username),
		protocol.WithNTRIPPassword(config.CorrectionInput.BaseCorrections.Settings.Ntripcli.Password),
		// todo modification du point de montage ici
		protocol.WithNTRIPMountPoint("EPI21"),
		protocol.WithSendPositionToBase(true),
	}

	if err := client.SetBaseCorrections(ctx, opts...); err != nil {
		return errors.WithStack(err)
	}

	fmt.Println("MountPoint NTRIP mis à jour")
	return nil
}

func setBaseToModeAndHold(ctx context.Context, client *reach.Client, baseConfig BaseConfig, mode string) error {
	fmt.Println("Configuration de la base en mode float-and-hold...")
	opts := []protocol.SetBaseOptionFunc{
		protocol.WithBaseLatitude(baseConfig.Latitude),
		protocol.WithBaseLongitude(baseConfig.Longitude),
		protocol.WithBaseHeight(baseConfig.Height),
		protocol.WithBaseAntennaOffset(baseConfig.AntennaOffset),
		protocol.WithBaseMode(fmt.Sprintf("%s-and-hold", mode)),
	}

	if err := client.SetBase(ctx, opts...); err != nil {
		return errors.WithStack(err)
	}

	fmt.Println("Base configurée en mode float-and-hold")
	return nil
}

func averagePositionAndSave(ctx context.Context, client *reach.Client) error {
	fmt.Println("Démarrage de la moyenne des position...")

	messageTask, err := client.AveragePosition(ctx)
	if err != nil {
		return errors.WithStack(err)
	}
	logTaskProgress(messageTask)
	return handleAverageCompletion(ctx, client, messageTask)

}

// func waitForAverageCompletion(ctx context.Context, client *reach.Client) error {
// 	broadcasts, err := reach.OnMessageType(ctx, client, "task_status")
// 	if err != nil {
// 		return errors.WithStack(err)
// 	}

// 	timeout := time.NewTimer(5 * time.Minute)
// 	defer timeout.Stop()

// 	for {
// 		select {
// 		case b := <-broadcasts:
// 			if err := logTaskProgress(b); err != nil {
// 				fmt.Printf("[WARNING] %+v", err)
// 				continue
// 			}

// 			if b.State == "completed" {
// 				return handleAverageCompletion(ctx, client, b)
// 			}

// 		case <-timeout.C:
// 			return errors.New("timeout lors du moyennage de position")

// 		case <-ctx.Done():
// 			return ctx.Err()
// 		}
// 	}
// }

func logTaskProgress(message *protocol.TaskMessage) error {
	data, err := json.MarshalIndent(message, "", "  ")
	if err != nil {
		return errors.WithStack(err)
	}
	fmt.Println(string(data))
	return nil
}

func handleAverageCompletion(ctx context.Context, client *reach.Client, message *protocol.TaskMessage) error {
	var payload Payload
	if err := mapstructure.Decode(message.Payload, &payload); err != nil {
		return errors.WithStack(err)
	}

	coords := payload.Coordinates
	fmt.Printf("Position moyennée: lat=%g, lon=%g, altitude=%g\n",
		coords.Latitude, coords.Longitude, coords.Height)

	return saveAveragedPosition(ctx, client, payload)
}

func saveAveragedPosition(ctx context.Context, client *reach.Client, payload Payload) error {
	opts := []protocol.SetBaseOptionFunc{
		protocol.WithBaseLatitude(payload.Coordinates.Latitude),
		protocol.WithBaseLongitude(payload.Coordinates.Longitude),
		protocol.WithBaseHeight(payload.Coordinates.Height),
		protocol.WithBaseAntennaOffset(payload.AntennaOffset),
		protocol.WithBaseMode("manual"),
	}

	if err := client.SetBase(ctx, opts...); err != nil {
		return errors.WithStack(err)
	}

	fmt.Println("Position sauvegardée en configuration")
	return nil
}