# ---------------------------------------------------------------------------- #
#                                                                              #
# 	Module:       main.py                                                      #
# 	Author:       Harry Esses                                                  #
# 	Created:      3/28/2026, 11:04:06 PM                                       #
# 	Description:  V5 project                                                   #
#                                                                              #
# ---------------------------------------------------------------------------- #

# Library imports
from vex import *
from math import log
brain = Brain()
controller = Controller(PRIMARY)
button = Limit(brain.three_wire_port.e)
left_motor = Motor(Ports.PORT2, GearSetting.RATIO_18_1, False)
right_motor = Motor(Ports.PORT9, GearSetting.RATIO_18_1, True)
top_motor = Motor(Ports.PORT8, GearSetting.RATIO_18_1, False)
bottom_motor = Motor(Ports.PORT3, GearSetting.RATIO_18_1, True)
run_bottom_motor = False
run_invert_bottom_motor = False
def intake():
    top_motor.spin(FORWARD, 100, PERCENT)
    # bottom_motor.spin(FORWARD, 100, PERCENT)
    wait(500, MSEC)
    top_motor.stop()
    # bottom_motor.stop()
def autonomous():
    brain.screen.clear_screen()
    brain.screen.print("autonomous code")
    # place automonous code here
    # button.pressed(intake(True))
    # button.released(intake(False))

def run_run_invert_bottom_motor():
    global run_invert_bottom_motor
    run_invert_bottom_motor = not run_invert_bottom_motor
def run_run_bottom_motor():
    global run_bottom_motor
    run_bottom_motor = not run_bottom_motor
def user_control():
    brain.screen.clear_screen()
    brain.screen.print("driver control")
    controller.buttonA.pressed(run_run_invert_bottom_motor)
    bottom_motor.spin(FORWARD, 100, PERCENT)
    controller.buttonY.pressed(run_run_bottom_motor)
    # button.pressed(intake)
    # button.released(intake(False))
    # place driver control in this while loop
    brain.screen.new_line()
    while True:
        brain.sdcard.savefile("motors/left_motor/velocity.txt", bytearray(str(left_motor.velocity())))
        brain.sdcard.savefile("motors/left_motor/power.txt", bytearray(str(left_motor.power())))
        brain.sdcard.savefile("motors/left_motor/torque.txt", bytearray(str(left_motor.torque())))
        brain.sdcard.savefile("motors/left_motor/efficiency.txt", bytearray(str(left_motor.efficiency())))
        brain.sdcard.savefile("motors/left_motor/temperature.txt", bytearray(str(left_motor.temperature())))
        if run_bottom_motor:
            if run_invert_bottom_motor:
                not run_invert_bottom_motor
            bottom_motor.spin(FORWARD, 100, PERCENT)
            top_motor.spin(FORWARD, 100, PERCENT)
        elif run_invert_bottom_motor:
            if run_bottom_motor:
                not run_bottom_motor
            bottom_motor.spin(REVERSE, 100, PERCENT)
            top_motor.spin(REVERSE, 100, PERCENT)
        else:
            bottom_motor.stop()
            top_motor.stop()
        # if run_invert_bottom_motor:
        #     if run_bottom_motor:
        #         not run_bottom_motor
        #     bottom_motor.spin(REVERSE, 100, PERCENT)
        #     top_motor.spin(REVERSE, 100, PERCENT)
        # else:
        #     bottom_motor.stop()
        #     top_motor.stop()
        print(button.pressing())
        brain.screen.clear_screen()
        # controller.screen.print("controller code")
        # controler.axis3.position() 
        
        if True:
            # print = brain.screen.print
            stright = controller.axis3.position() 
            turn = controller.axis1.position()
            turn_modifer = 1
            left = stright + turn * turn_modifer
            right = stright - turn * turn_modifer
            left_motor.spin(FORWARD, left, PERCENT)
            right_motor.spin(FORWARD, right, PERCENT)
            # if controller.axis3.position() != 0:
            #     left_motor.spin(FORWARD, controller.axis3.position()*0.7, PERCENT)
            #     right_motor.spin(REVERSE, controller.axis3.position()*0.7, PERCENT)
            # else:
            #     left_motor.stop()
            #     right_motor.stop()
            # print(controller.axis3.position())
            # # brain.screen.new_line()
            # print(left_motor.velocity())
            # # brain.screen.new_line()
            # print(right_motor.velocity())
            # # brain.screen.new_line()
        # Drive code
        # wait(20, MSEC)

# create competition instance
comp = Competition(user_control, autonomous)

# actions to do when the program starts
brain.screen.clear_screen()