import os
import logging
logging.basicConfig(level='INFO')

class StateMachineHandler:
    def __init__(self) -> None:
        self.state_machine_file = "/Users/max/Desktop/THEA_CODE/Control/Jetson/tests/MACHINE_STATE"
        # self.state_machine_file = "/home/pi/MACHINE_STATE"
        self.__MACHINE_STATES = {
                "IDLE": 0,
                "NAVIGATING_TO_POINT_GPS": 1,
                "NAVIGATING_TO_POINT_VISION": 2,
                "MANIPULATING_CARGO": 3,
                "READY_FOR_NEXT_STAGE": 4,
                "RC_CONTROL": 5,
                "PROXIMITY_STOP": 6,
                "ESTOP": 7,
                "OTHER_ERROR": 8
            }
        self.init_file()

    def get_state(self):
        if os.path.isfile(self.state_machine_file):
            with open(self.state_machine_file, 'r') as smfile:
                state = int(smfile.read())
                return state, list(self.__MACHINE_STATES.keys())[list(self.__MACHINE_STATES.values()).index(int(state))]

    def init_file(self, force=False):
        if not os.path.isfile(self.state_machine_file) or force:
            logging.info(f"Creating new state file, force flag is {force}")
            with open(self.state_machine_file, 'w+') as smfile:
                smfile.write(str(self.__MACHINE_STATES["IDLE"]))
        else:
            logging.info("State file already exists and force flag is absent, leaving file as is")

    def set_state(self, state):
        if self.check_state_validity():
            with open(self.state_machine_file, 'w') as smfile:
                smfile.write(str(self.__MACHINE_STATES.get(state, 8)))

    def check_state_validity(self):
        _, state = self.get_state()
        if state == "ESTOP" or state == "OTHER_ERROR":
            logging.critical(f"Machine state is {state}! Cannot perform operation")
            return False
        return True

sm = StateMachineHandler()
print(sm.get_state())
sm.set_state("RC_CONTROL")
print(sm.get_state())
sm.set_state("ESTOP")
print(sm.get_state())
sm.set_state("NAVIGATING_TO_POINT_VISION")
print(sm.get_state())