using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace RaspiControl {
    public enum ProcessState {
        Startup,
        Init,
        Ready,
        StartMoveManual,
        ResumeMoveManual,
        MoveManual,
        MoveManualF,
        MoveManualB,
        MoveManualL,
        MoveManualR,
        StopManual,
        Auto,
        Final,
        Error
    }

    public enum Command {
        Connected,
        InitDone,
        JoystickUp,
        JoystickDown,
        JoystickLeft,
        JoystickRight,
        JoystickCenter,
        autoMode,
        finished,
        failure,
        none,
        manualMode
    }
    internal class StateMachine {

        class StateTransition {
            readonly ProcessState CurrentState;
            readonly Command Command;

            public StateTransition(ProcessState currentState, Command command) {
                CurrentState = currentState;
                Command = command;
            }

            public override int GetHashCode() {
                return 17 + 31 * CurrentState.GetHashCode() + 31 * Command.GetHashCode();
            }

            public override bool Equals(object obj) {
                StateTransition other = obj as StateTransition;
                return other != null && this.CurrentState == other.CurrentState && this.Command == other.Command;
            }
        }

        Dictionary<StateTransition, ProcessState> transitions;
        public ProcessState CurrentState { get; private set; }

        public StateMachine() {
            CurrentState = ProcessState.Startup;
            transitions = new Dictionary<StateTransition, ProcessState> {
                    {new StateTransition(ProcessState.Startup,Command.Connected), ProcessState.Init },

                    {new StateTransition(ProcessState.Init,Command.JoystickCenter), ProcessState.Ready },

                    {new StateTransition(ProcessState.Ready,Command.JoystickUp), ProcessState.StartMoveManual },
                    {new StateTransition(ProcessState.Ready,Command.JoystickDown), ProcessState.StartMoveManual },
                    {new StateTransition(ProcessState.Ready,Command.JoystickLeft), ProcessState.StartMoveManual },
                    {new StateTransition(ProcessState.Ready,Command.JoystickRight), ProcessState.StartMoveManual },
                    {new StateTransition(ProcessState.Ready,Command.autoMode), ProcessState.Auto },
                    {new StateTransition(ProcessState.Ready,Command.manualMode), ProcessState.Ready },


                    {new StateTransition(ProcessState.StartMoveManual,Command.none), ProcessState.MoveManual },
                    {new StateTransition(ProcessState.StartMoveManual, Command.autoMode), ProcessState.Auto },
                    {new StateTransition(ProcessState.StartMoveManual, Command.manualMode), ProcessState.StartMoveManual },

                    {new StateTransition(ProcessState.MoveManualF, Command.none), ProcessState.MoveManual },
                    {new StateTransition(ProcessState.MoveManualF, Command.autoMode), ProcessState.Auto },
                    {new StateTransition(ProcessState.MoveManualB, Command.none), ProcessState.MoveManual },
                    {new StateTransition(ProcessState.MoveManualB, Command.autoMode), ProcessState.Auto },
                    {new StateTransition(ProcessState.MoveManualL, Command.none), ProcessState.MoveManual },
                    {new StateTransition(ProcessState.MoveManualL, Command.autoMode), ProcessState.Auto },
                    {new StateTransition(ProcessState.MoveManualR, Command.none), ProcessState.MoveManual },
                    {new StateTransition(ProcessState.MoveManualR, Command.autoMode), ProcessState.Auto },

                    {new StateTransition(ProcessState.MoveManual, Command.JoystickUp), ProcessState.MoveManualF },
                    {new StateTransition(ProcessState.MoveManual, Command.JoystickDown), ProcessState.MoveManualB },
                    {new StateTransition(ProcessState.MoveManual, Command.JoystickLeft), ProcessState.MoveManualL },
                    {new StateTransition(ProcessState.MoveManual, Command.JoystickRight), ProcessState.MoveManualR },
                    {new StateTransition(ProcessState.MoveManual, Command.JoystickCenter), ProcessState.StopManual },
                    {new StateTransition(ProcessState.MoveManual, Command.autoMode), ProcessState.Auto },
                    {new StateTransition(ProcessState.MoveManual, Command.finished), ProcessState.Final },

                    {new StateTransition(ProcessState.StopManual,Command.JoystickUp), ProcessState.ResumeMoveManual },
                    {new StateTransition(ProcessState.StopManual,Command.JoystickDown), ProcessState.ResumeMoveManual },
                    {new StateTransition(ProcessState.StopManual,Command.JoystickLeft), ProcessState.ResumeMoveManual },
                    {new StateTransition(ProcessState.StopManual,Command.JoystickRight), ProcessState.ResumeMoveManual },
                    {new StateTransition(ProcessState.StopManual, Command.autoMode), ProcessState.Auto },
                    {new StateTransition(ProcessState.StopManual, Command.finished), ProcessState.Final },

                    {new StateTransition(ProcessState.ResumeMoveManual, Command.none),ProcessState.MoveManual },
                    {new StateTransition(ProcessState.ResumeMoveManual, Command.autoMode), ProcessState.Auto },

                    {new StateTransition(ProcessState.Auto, Command.finished), ProcessState.Final },
                    {new StateTransition(ProcessState.Auto, Command.failure), ProcessState.Error },
                    {new StateTransition(ProcessState.Auto, Command.manualMode), ProcessState.ResumeMoveManual },

                    {new StateTransition(ProcessState.Error, Command.JoystickUp), ProcessState.ResumeMoveManual },
                    {new StateTransition(ProcessState.Error, Command.JoystickDown), ProcessState.ResumeMoveManual },
                    {new StateTransition(ProcessState.Error, Command.JoystickLeft), ProcessState.ResumeMoveManual },
                    {new StateTransition(ProcessState.Error, Command.JoystickRight), ProcessState.ResumeMoveManual },
                    {new StateTransition(ProcessState.Error, Command.JoystickCenter), ProcessState.ResumeMoveManual },
                    {new StateTransition(ProcessState.Error, Command.finished), ProcessState.Final },
                    {new StateTransition(ProcessState.Error, Command.manualMode), ProcessState.ResumeMoveManual },

                };
        }

        public ProcessState GetNext(Command command) {
            StateTransition transition = new StateTransition(CurrentState, command);
            ProcessState nextState;
            if (!transitions.TryGetValue(transition, out nextState))
                throw new Exception("Invalid transition: " + CurrentState + " -> " + command);
            return nextState;
        }

        public ProcessState MoveNext(Command command) {
            CurrentState = GetNext(command);
            return CurrentState;
        }

    }
}