package main

import (
	"fmt"
	"log"
	"sync"
	"time"

	"github.com/bluenviron/gomavlib/v3"
	"github.com/bluenviron/gomavlib/v3/pkg/dialect"
	"github.com/bluenviron/gomavlib/v3/pkg/dialects/ardupilotmega"
	"github.com/bluenviron/gomavlib/v3/pkg/message"
)

type MessageCustom struct {
	Param1 uint8
	Param2 uint8
	Param3 uint32
}

func (*MessageCustom) GetID() uint32 {
	return 22
}

var (
	myDialect = &dialect.Dialect{
		Version: 3,
		Messages: []message.Message{
			&MessageCustom{},
		},
	}
)

func main() {
	// Select between custom dialect or ardupilotmega
	custom := true

	var dialect *dialect.Dialect
	if custom {
		dialect = myDialect
	} else {
		dialect = ardupilotmega.Dialect
	}

	senderNode, err := gomavlib.NewNode(gomavlib.NodeConf{
		Endpoints: []gomavlib.EndpointConf{
			gomavlib.EndpointSerial{
				Device: "/dev/ttyUSB4",
				Baud:   57600,
			},
		},
		Dialect:     dialect,
		OutVersion:  gomavlib.V2,
		OutSystemID: 10,
	})
	if err != nil {
		log.Fatal("Creating receiver node")
	}
	defer senderNode.Close()

	receiverNode, err := gomavlib.NewNode(gomavlib.NodeConf{
		Endpoints: []gomavlib.EndpointConf{
			gomavlib.EndpointSerial{
				Device: "/dev/ttyUSB5",
				Baud:   57600,
			},
		},
		Dialect:     dialect,
		OutVersion:  gomavlib.V2,
		OutSystemID: 11,
	})
	if err != nil {
		log.Fatal("Creating receiver node")
	}
	defer receiverNode.Close()

	done := make(chan struct{})
	var wg sync.WaitGroup
	wg.Add(2)
	go func() {
	loop:
		for {
			select {
			case evt := <-senderNode.Events():
				if frm, ok := evt.(*gomavlib.EventFrame); ok {
					fmt.Printf("[TX-RX] %T\n", frm.Message())
				}
			case <-done:
				break loop
			}
		}
		wg.Done()
	}()
	go func() {
	loop:
		for {
			select {
			case evt := <-receiverNode.Events():
				if frm, ok := evt.(*gomavlib.EventFrame); ok {
					fmt.Printf("[RX] id=%d, %+v\n", frm.Message().GetID(), frm.Message())
				}
			case <-done:
				break loop
			}
		}
		wg.Done()
	}()

	for range 5 {
		if custom {
			senderNode.WriteMessageAll(&MessageCustom{
				Param1: 1,
				Param2: 2,
				Param3: 3,
			})
		} else {
			senderNode.WriteMessageAll(&ardupilotmega.MessageParamValue{
				ParamId:    "test_parameter",
				ParamValue: 123456,
				ParamType:  ardupilotmega.MAV_PARAM_TYPE_UINT32,
				ParamCount: 1,
				ParamIndex: 1,
			})
		}
		time.Sleep(time.Second)
	}

	fmt.Println("CLOSING channel")
	close(done)
	wg.Wait()
	fmt.Println("GOODBYE")
}