using System;
using System.Text;
using M2Mqtt;
using M2Mqtt.Messages;
using Swan.Formatters;

namespace RaspiControl {
    class Programm {
        private static MqttClient client;
        private static Navigation navigation;
        private static Joystick joystick;
        static void Main(string[] args) {
            try {
                client = new MqttClient("localhost");
                client.MqttMsgPublishReceived += Client_MqttMsgPublishReceived;
                string clientId = Guid.NewGuid().ToString();
                client.Connect(clientId);
                client.Subscribe(new string[] { "APROG/REQUEST" }, new byte[] { MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE });
                navigation= new Navigation();
                joystick= new Joystick();
                joystick.JoystickChanged += Joystick_JoystickChanged;

            } catch (Exception ex) {
                Console.WriteLine(ex.Message);
            }
        }

        private static void Joystick_JoystickChanged(object sender, JoystickEventArgs e) {
            switch (e.Button) {
                case JoystickButton.None:
                    break;
                case JoystickButton.Left:
                    client.Publish("mobile/cmd/nav/left", Encoding.UTF8.GetBytes($"{navigation.GetAngleLeftStep()}"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false);
                    break;
                case JoystickButton.Right:
                    client.Publish("mobile/cmd/nav/right", Encoding.UTF8.GetBytes($"{navigation.GetAngleRightStep()}"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false);
                    break;
                case JoystickButton.Up:
                    break;
                case JoystickButton.Down:
                    break;
                case JoystickButton.Center:
                    navigation.ResetAngle();
                    client.Publish("mobile/cmd/nav/drive", Encoding.UTF8.GetBytes("true"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false);
                    break;
            }
        }

        private static void Client_MqttMsgPublishReceived(object sender, MqttMsgPublishEventArgs e) {
            Console.Write(e.Topic + "\n");
            Console.Write(Encoding.UTF8.GetString(e.Message) + "\n");
        }
    }
}