using System;
using UnityEngine;

namespace HexapodSim
{
    // Stand-in for the handheld remote. Produces CmdPackets exactly per the
    // protocol contract and sends them over the simulated radio. Toggles and
    // interlocks mirror remote/RemoteState.cpp and remote/PacketBuilder.cpp -
    // keep them in sync if those change.
    //
    // Input sources:
    //   Joystick           - the real remote as a USB game controller, read through
    //                        Unity input axes "1".."6" like the old HandleHoystick.
    //   Keyboard           - test without any remote (keys listed in SimTestPanel).
    //   RobotSerialHarness - no radio at all; typed keys go to the firmware's own
    //                        TEST_SERIAL_INPUT harness, like the Arduino Serial Monitor.
    // Toggles, the speed slider and the handset tilt sliders live in SimTestPanel.
    public class SimRemote : MonoBehaviour
    {
        public enum InputSource { Joystick, Keyboard, RobotSerialHarness }

        [Serializable]
        public class AxisBinding
        {
            public string axis;
            public bool invert;
            public AxisBinding(string axis, bool invert) { this.axis = axis; this.invert = invert; }
        }

        public InputSource source = InputSource.Keyboard;

        // Defaults follow the old HandleHoystick: axes 4/6/5 were the walk stick
        // (b2/b1/b3), 2/1/3 the rotation stick (a1/a2/a3), with 1 and 2 inverted.
        // Which of 4/6 is forward vs sideways wasn't recoverable from the old code:
        // check the raw axis readout in the test panel and fix here.
        [Header("Joystick axes (Input Manager names)")]
        [Tooltip("Stick convention after invert: X + = right, Y + = forward, twist + = counterclockwise.")]
        public AxisBinding leftX = new AxisBinding("4", false);
        public AxisBinding leftY = new AxisBinding("6", false);
        public AxisBinding leftTwist = new AxisBinding("5", false);
        public AxisBinding rightX = new AxisBinding("2", true);
        public AxisBinding rightY = new AxisBinding("1", true);
        public AxisBinding rightTwist = new AxisBinding("3", false);
        [Range(0, 0.5f)] public float deadzone = 0.08f;

        [Header("Operator state (same toggles as the Nextion screen)")]
        public bool armed;
        public bool stand;
        public bool gaitWave;
        public bool modeB;
        public bool rstickXlate;
        public bool remoteGyro;
        public bool robotGyro;
        [Range(0, 255)] public int speedTarget = 128;
        [Tooltip("Standing height setting, -1000 lowest .. 0 default .. 1000 highest (CmdPacket.body_height).")]
        [Range(-1000, 1000)] public int height = 0;

        [Header("Simulated handset tilt (the remote's MPU6050)")]
        [Range(-30, 30)] public float handsetRollDeg;
        [Range(-30, 30)] public float handsetPitchDeg;
        public float handsetFullScaleDeg = 30f;   // remote Config.h REMOTE_TILT_FULL_SCALE_DEG

        [Header("Smoothing, as on the remote")]
        public float speedRatePerSec = 255f;      // SPEED_SCALE_RATE_PER_SEC
        public float walkLingerSec = 0.25f;       // WALK_LINGER_MS
        public float armCenterTolerance = 0.05f;  // ARM_CENTER_TOLERANCE / 1000
        public float keyStickRatePerSec = 4f;     // how fast a held key moves the virtual stick

        // Latest processed sticks, -1..1: [L_X, L_Y, L_TWIST, R_X, R_Y, R_TWIST].
        public readonly float[] sticks = new float[6];
        public readonly float[] rawAxes = new float[6];   // Input axes "1".."6" as read
        public CmdPacket LastPacket { get; private set; }
        public string Message { get; private set; } = "";
        public bool JoystickAxesMissing { get; private set; }

        float speedCurrent_ = 128;
        float lastWalkInputTime_ = -999f;
        byte seq_;
        string serialKeys_ = "";
        float messageUntil_;

        public void ResetState()
        {
            armed = stand = false;
            speedCurrent_ = speedTarget;
            lastWalkInputTime_ = -999f;
            serialKeys_ = "";
        }

        void Awake()
        {
            if (Input.GetJoystickNames().Length > 0 && !string.IsNullOrEmpty(Input.GetJoystickNames()[0]))
                source = InputSource.Joystick;
        }

        void Update()
        {
            for (int i = 0; i < 6; i++) rawAxes[i] = ReadAxis((i + 1).ToString());

            if (source == InputSource.RobotSerialHarness)
            {
                serialKeys_ += Input.inputString;   // raw keystrokes for the firmware's harness
                return;
            }

            HandleToggleKeys();
            if (source == InputSource.Joystick) ReadJoystick();
            else ReadKeyboardSticks(Time.deltaTime);
            if (Time.time > messageUntil_) Message = "";
        }

        // ---- Operator commands (also called by SimTestPanel buttons) ----

        public void ToggleArm()
        {
            if (armed) { Disarm(); return; }
            foreach (float v in sticks)
                if (Mathf.Abs(v) > armCenterTolerance) { Say("Center both sticks to arm"); return; }
            armed = true;
        }
        public void Disarm() { armed = false; stand = false; }   // disarm always clears stand
        public void ToggleStand()
        {
            if (!armed) { Say("Arm first"); return; }
            stand = !stand;
        }
        public void ToggleRemoteGyro() => remoteGyro = !remoteGyro;

        void Say(string msg) { Message = msg; messageUntil_ = Time.time + 2.5f; }

        void HandleToggleKeys()
        {
            if (Input.GetKeyDown(KeyCode.Alpha1)) ToggleArm();
            if (Input.GetKeyDown(KeyCode.Alpha2)) ToggleStand();
            if (Input.GetKeyDown(KeyCode.Alpha3)) gaitWave = !gaitWave;
            if (Input.GetKeyDown(KeyCode.Alpha4)) modeB = !modeB;
            if (Input.GetKeyDown(KeyCode.Alpha5)) rstickXlate = !rstickXlate;
            if (Input.GetKeyDown(KeyCode.Alpha6)) ToggleRemoteGyro();
            if (Input.GetKeyDown(KeyCode.Alpha7)) robotGyro = !robotGyro;
            if (Input.GetKeyDown(KeyCode.Backspace)) Disarm();
            if (Input.GetKeyDown(KeyCode.Equals) || Input.GetKeyDown(KeyCode.KeypadPlus)) speedTarget = Mathf.Min(255, speedTarget + 25);
            if (Input.GetKeyDown(KeyCode.Minus) || Input.GetKeyDown(KeyCode.KeypadMinus)) speedTarget = Mathf.Max(0, speedTarget - 25);
            if (Input.GetKeyDown(KeyCode.PageUp)) height = Mathf.Min(1000, height + 100);
            if (Input.GetKeyDown(KeyCode.PageDown)) height = Mathf.Max(-1000, height - 100);
        }

        // ---- Sticks ----

        float ReadAxis(string name)
        {
            try { return Input.GetAxis(name); }
            catch (ArgumentException) { JoystickAxesMissing = true; return 0; }   // axis not defined in Input Manager
        }

        float Read(AxisBinding b)
        {
            float v = ReadAxis(b.axis) * (b.invert ? -1 : 1);
            float mag = Mathf.Abs(v);
            if (mag <= deadzone) return 0;
            return Mathf.Sign(v) * Mathf.Clamp01((mag - deadzone) / (1 - deadzone));
        }

        void ReadJoystick()
        {
            sticks[0] = Read(leftX);
            sticks[1] = Read(leftY);
            sticks[2] = Read(leftTwist);
            sticks[3] = Read(rightX);
            sticks[4] = Read(rightY);
            sticks[5] = Read(rightTwist);
        }

        // WASD + Q/E = left stick (walk, turn), IJKL + U/O = right stick.
        void ReadKeyboardSticks(float dt)
        {
            float Key(KeyCode pos, KeyCode neg) => (Input.GetKey(pos) ? 1 : 0) - (Input.GetKey(neg) ? 1 : 0);
            float[] target =
            {
                Key(KeyCode.D, KeyCode.A), Key(KeyCode.W, KeyCode.S), Key(KeyCode.Q, KeyCode.E),
                Key(KeyCode.L, KeyCode.J), Key(KeyCode.I, KeyCode.K), Key(KeyCode.U, KeyCode.O),
            };
            for (int i = 0; i < 6; i++) sticks[i] = Mathf.MoveTowards(sticks[i], target[i], keyStickRatePerSec * dt);
        }

        // ---- Transmit ----

        // Called by HexapodSimulation once per remote TX period.
        public void Transmit(HexapodSimNative native, float dt)
        {
            if (source == InputSource.RobotSerialHarness)
            {
                native.SerialInput(serialKeys_);   // no radio: the firmware's harness injects its own packets
                serialKeys_ = "";
                return;
            }
            speedCurrent_ = Mathf.MoveTowards(speedCurrent_, speedTarget, speedRatePerSec * dt);
            LastPacket = BuildPacket();
            native.RadioSend(LastPacket);
        }

        static short Wire(float v) => (short)Mathf.RoundToInt(Mathf.Clamp(v, -1f, 1f) * 1000f);

        // Same mapping as remote/PacketBuilder.cpp.
        CmdPacket BuildPacket()
        {
            var p = new CmdPacket
            {
                seq = seq_++,
                speed_scale = (byte)Mathf.RoundToInt(speedCurrent_),
                body_height = (short)Mathf.Clamp(height, -1000, 1000),   // a setting: sent even while disarmed
            };
            byte f = 0;
            if (armed) f |= CmdPacket.FLAG_ARMED;
            if (stand) f |= CmdPacket.FLAG_STAND;
            if (modeB) f |= CmdPacket.FLAG_MODE_B;
            if (rstickXlate) f |= CmdPacket.FLAG_RSTICK_XLATE;
            if (gaitWave) f |= CmdPacket.FLAG_GAIT_WAVE;
            if (robotGyro) f |= CmdPacket.FLAG_ROBOT_GYRO;

            if (!armed) { p.flags = f; return p; }   // disarmed: neutral command

            // Left stick: forward -> +vx, right -> -vy (vy + = left), twist CCW -> +yaw_rate.
            p.vx = Wire(sticks[1]);
            p.vy = Wire(-sticks[0]);
            p.yaw_rate = Wire(sticks[2]);

            if (rstickXlate)
            {
                p.rx = Wire(sticks[4]);    // X offset: forward
                p.ry = Wire(-sticks[3]);   // Y offset: right -> -Y
                p.rz = Wire(sticks[5]);    // Z offset: twist CCW -> up
            }
            else
            {
                p.rx = Wire(sticks[3]);    // roll: right side down
                p.ry = Wire(sticks[4]);    // pitch: nose down
                p.rz = Wire(sticks[5]);    // yaw: CCW
            }

            bool walkInput = p.vx != 0 || p.vy != 0 || (!modeB && p.yaw_rate != 0);
            if (walkInput) lastWalkInputTime_ = Time.time;
            if (stand && (walkInput || Time.time - lastWalkInputTime_ < walkLingerSec)) f |= CmdPacket.FLAG_WALK;

            if (remoteGyro)
            {
                f |= CmdPacket.FLAG_REMOTE_GYRO;
                p.remote_roll = Wire(handsetRollDeg / handsetFullScaleDeg);
                p.remote_pitch = Wire(handsetPitchDeg / handsetFullScaleDeg);
            }

            p.flags = f;
            return p;
        }
    }
}