From 202aad293f4ba08774fd3377af4b2a5931328b97 Mon Sep 17 00:00:00 2001
From: AntoineH <antoine.harle@proton.me>
Date: Wed, 4 Sep 2024 13:08:08 +0200
Subject: [PATCH] Initial setup of motor server

---
 src/cmd/controller/getJoints.go |  1 -
 src/cmd/motor/motor.go          | 17 +++++++-
 src/cmd/motor/serve.go          | 69 ++++++++++++++++++++++++++++++++-
 src/proto/motorsim.proto        |  8 ++--
 4 files changed, 88 insertions(+), 7 deletions(-)

diff --git a/src/cmd/controller/getJoints.go b/src/cmd/controller/getJoints.go
index 370f1ea..67ce55f 100644
--- a/src/cmd/controller/getJoints.go
+++ b/src/cmd/controller/getJoints.go
@@ -12,7 +12,6 @@ import (
 	"context"
 	"log"
 	"time"
-	"strings"
 
 	"google.golang.org/grpc"
 	"google.golang.org/grpc/credentials/insecure"
diff --git a/src/cmd/motor/motor.go b/src/cmd/motor/motor.go
index 799e099..4c2d622 100644
--- a/src/cmd/motor/motor.go
+++ b/src/cmd/motor/motor.go
@@ -12,6 +12,21 @@ import (
 	"github.com/spf13/cobra"
 )
 
+type config struct {
+	Id uint16
+	Port uint16 
+	Min_pos float64
+	Max_pos float64
+	Max_vel float64
+	Accel float64
+}
+
+var(
+	motorID uint16 = 0 //Requested ID
+	ip string = "localhost" //localhost=127.0.0.1
+	curr_config config //Current config of the motor
+)
+
 // motorCmd represents the motor command
 var motorCmd = &cobra.Command{
 	Use:   "motor",
@@ -29,7 +44,7 @@ func init() {
 
 	// Cobra supports Persistent Flags which will work for this command
 	// and all subcommands, e.g.:
-	motorCmd.PersistentFlags().Uint16("id", 0, "Identifier number")
+	motorCmd.PersistentFlags().Uint16Var(&motorID, "id", 0, "Identifier number")
 	motorCmd.MarkFlagRequired("id")
 
 	// Cobra supports local flags which will only run when this command
diff --git a/src/cmd/motor/serve.go b/src/cmd/motor/serve.go
index 67d60c3..54ba297 100644
--- a/src/cmd/motor/serve.go
+++ b/src/cmd/motor/serve.go
@@ -8,8 +8,16 @@ import (
 	"fmt"
 
 	"github.com/spf13/cobra"
+	"github.com/spf13/viper"
+
+	"context"
+	"log"
+	"net"
+	"google.golang.org/grpc"
+	pb "github.com/AntoineHX/multi-motors-controller/src/proto"
 )
 
+//Cobra CLI
 // serveCmd represents the serve command
 var serveCmd = &cobra.Command{
 	Use:   "serve",
@@ -17,7 +25,9 @@ var serveCmd = &cobra.Command{
 	Long: `serve command descritpion`,
 	Run: func(cmd *cobra.Command, args []string) {
 		fmt.Println("serve called with ID ", cmd.Flag("id").Value)
-		fmt.Println(cmd.CommandPath())
+		// fmt.Println(cmd.CommandPath())
+		updateConfig()
+		serve()
 	},
 }
 
@@ -35,4 +45,61 @@ func init() {
 	// serveCmd.Flags().Uint16P("port", "p", 8080, "Port")
 
 	//TODO: Support Motor ID flag
+}
+
+func updateConfig(){
+	//Find correct ID in config file
+	i := 0
+	config_id := fmt.Sprintf("motors.%d.id",i)
+	for viper.IsSet(config_id){
+		// log.Printf("%v / %v", viper.GetInt(config_id), int(motorID))
+		if viper.GetInt(config_id) == int(motorID) {
+			//Extract config for this motor
+			err := viper.UnmarshalKey(fmt.Sprintf("motors.%d",i), &curr_config)
+			if err != nil {
+				log.Fatalf("unable to decode into struct, %v", err)
+				break
+			}
+
+			//Sanity check
+			if curr_config.Id != motorID {
+				log.Fatalf("Failed to update config. Requested ID: %d. Got: %d.", motorID, curr_config.Id)
+			}
+			break
+		}
+		//Next motor
+		i++
+		config_id = fmt.Sprintf("motors.%d.id",i)
+	}
+
+	log.Printf("Using config: %+v", curr_config)
+}
+
+//gRPC server
+// server is used to implement MotorsControllerServer.
+type server struct {
+	pb.UnimplementedMotorsControllerServer
+}
+
+//TODO: Fix compiling issue with google.protobuf.Empty message
+func (s *server) SetVolicty(ctx context.Context, in *pb.Velocity) (*pb.Empty, error) {
+	log.Printf("Received: %v", in.GetVelocity())
+	return &pb.Empty{}, nil
+}
+
+func (s *server) GetData(ctx context.Context, in *pb.Empty) (*pb.MotorData, error) {
+	return &pb.MotorData{Angle: 0, Velocity: 0, Error: ""}, nil
+}
+
+func serve() {
+	lis, err := net.Listen("tcp", fmt.Sprintf(":%d", curr_config.Port))
+	if err != nil {
+		log.Fatalf("failed to listen: %v", err)
+	}
+	s := grpc.NewServer()
+	pb.RegisterMotorsControllerServer(s, &server{})
+	log.Printf("server listening at %v", lis.Addr())
+	if err := s.Serve(lis); err != nil {
+		log.Fatalf("failed to serve: %v", err)
+	}
 }
\ No newline at end of file
diff --git a/src/proto/motorsim.proto b/src/proto/motorsim.proto
index 165909f..657578b 100644
--- a/src/proto/motorsim.proto
+++ b/src/proto/motorsim.proto
@@ -12,10 +12,10 @@ service MotorsController {
     rpc GetJoints(Empty) returns (Angles) {} 
 }
 // Server converting target angles to velocity profile for mo
-//service Motor {
-//    rpc SetVolicty(Velocity) returns (Empty) {}
-//    rpc ReceiveDataStream(Empty) returns (stream
-//}
+service Motor {
+    rpc SetVolicty(Velocity) returns (Empty) {}
+    rpc GetData(Empty) returns (MotorData) {}
+}
 
 message Angles {
     repeated double angles = 1;