#!/usr/bin/env python3
import pygame
import time
import os
import sys
from motor_controller import MotorController
from controller import Joystick

if __name__ == '__main__':
    os.environ["SDL_VIDEODRIVER"] = "dummy"
    pygame.init()
    pygame.joystick.init()

    motoCon = MotorController()
    # if controllerJoystickIsUp:
    #   motoCon.forward()
    with Joystick(0) as controller:
        while True:
            controller.update()
            inputs = controller.get_all_vals()
            if inputs['button_0']:
                motoCon.forward()
            elif inputs['button_1']:
                motoCon.stop()
            elif inputs['button_2']:
                motoCon.reverse()
            motoCon.set_steering(inputs['axis_0'])
            motoCon.print_stat()
            time.sleep(0.1)
            
