From 47644ad0169c9ce7a4ff5c3ee72dba987d818161 Mon Sep 17 00:00:00 2001
From: Kester <kester@avy.eu>
Date: Wed, 3 May 2023 11:19:15 +0200
Subject: [PATCH] Using the whole common dialect, rather than just a few
 messages from it

Added basic routing rules for COMMAND_LONG, COMMAND_ACK and COMMAND_INT

Fixed some liniting suggestions

Added additional print statement to allow route debbugging
---
 main.go | 55 +++++++++++++++++++++++++++++++++++++++++--------------
 1 file changed, 41 insertions(+), 14 deletions(-)

diff --git a/main.go b/main.go
index 3a1a92e..38d17c2 100644
--- a/main.go
+++ b/main.go
@@ -14,9 +14,7 @@ import (
 
 	"github.com/alecthomas/kong"
 	"github.com/bluenviron/gomavlib/v2"
-	"github.com/bluenviron/gomavlib/v2/pkg/dialect"
 	"github.com/bluenviron/gomavlib/v2/pkg/dialects/common"
-	"github.com/bluenviron/gomavlib/v2/pkg/message"
 )
 
 var version = "v0.0.0"
@@ -92,6 +90,7 @@ var cli struct {
 	Version            bool `help:"print version."`
 	Quiet              bool `short:"q" help:"suppress info messages."`
 	Print              bool `help:"print routed frames."`
+	PrintRoutes        bool `help:"print routes applied for messages with targets."`
 	PrintErrors        bool
 	HbDisable          bool   `help:"disable heartbeats."`
 	HbVersion          string `enum:"1,2" help:"set mavlink version of heartbeats." default:"1"`
@@ -185,17 +184,6 @@ func newProgram(args []string) (*program, error) {
 		econfs[i] = conf
 	}
 
-	// decode/encode only a minimal set of messages.
-	// other messages change too frequently and cannot be integrated into a static tool.
-	msgs := []message.Message{}
-	if !cli.HbDisable || !cli.StreamreqDisable {
-		msgs = append(msgs, &common.MessageHeartbeat{})
-	}
-	if !cli.StreamreqDisable {
-		msgs = append(msgs, &common.MessageRequestDataStream{})
-	}
-	dialect := &dialect.Dialect{3, msgs} //nolint:govet
-
 	ctx, ctxCancel := context.WithCancel(context.Background())
 
 	p := &program{
@@ -205,7 +193,7 @@ func newProgram(args []string) (*program, error) {
 
 	p.node, err = gomavlib.NewNode(gomavlib.NodeConf{
 		Endpoints: econfs,
-		Dialect:   dialect,
+		Dialect:   common.Dialect,
 		OutVersion: func() gomavlib.Version {
 			if cli.HbVersion == "2" {
 				return gomavlib.V2
@@ -309,6 +297,45 @@ func (p *program) run() {
 					}
 				}
 
+				routeMsg := false // Flag to mark a msg for routing
+				targetSystem := uint8(0)
+				targetComponent := uint8(0)
+				switch msg := evt.Message().(type) {
+				case *common.MessageCommandLong:
+					routeMsg = true
+					targetSystem = msg.TargetSystem
+					targetComponent = msg.TargetComponent
+				case *common.MessageCommandAck:
+					routeMsg = true
+					targetSystem = msg.TargetSystem
+					targetComponent = msg.TargetComponent
+				case *common.MessageCommandInt:
+					routeMsg = true
+					targetSystem = msg.TargetSystem
+					targetComponent = msg.TargetComponent
+				}
+
+				if routeMsg {
+					if targetSystem > 0 { // Route only if it's non-broadcast command
+						for remoteNode := range p.nodeHandler.remoteNodes { // Iterates through connected nodes
+							if remoteNode.SystemID == targetSystem {
+								if remoteNode.ComponentID == targetComponent ||
+									targetComponent < 1 { // Route if compid matches or is a broadcast
+									if remoteNode.Channel != evt.Channel { // Prevents Loops
+										if cli.PrintRoutes {
+											fmt.Println("Routing msg ", evt.Message().GetID(), " from ", evt.Channel, "--->", remoteNode.Channel)
+										}
+										p.node.WriteFrameTo(remoteNode.Channel, evt.Frame)
+									} else {
+										fmt.Println("Warning: channel ", remoteNode.Channel, " attempted to send to itself, discarding ")
+									}
+								}
+							}
+						}
+						continue
+					}
+				}
+
 				// route message to every other channel
 				p.node.WriteFrameExcept(evt.Channel, evt.Frame)