import numpy as np
from quanser.hardware import HIL
import time

def calculate_mtr_cmd():
    # your code goes here
    return True
def calculate_str_cmd():
    # your code goes here
    return True
def process_IMU_data():
    # your code goes here
    return True

# IMU Read - Other channels 3000:3002 + 4000:4002 are for IMU 
channels = [3000, 3001, 3002, 4000, 4001, 4002]
read_other_channels_IMU = np.array(channels, dtype=np.int32)
read_other_buffer_IMU = np.zeros(6, dtype=np.float64)
# Throttle Write Only - PWM channel 0 is mtr cmd
write_pwm_channel_throttle = np.array([0], dtype=np.int32)
# Steering Write Only - Other channel 1000 is steering cmd
write_other_channel_steering = np.array([1000], dtype=np.int32)
# Timing
sample_time = 10 # milliseconds
simulation_time = 60000 # 1 minute
# HIL Setup
card = HIL("qcar", "0")

while time.time() < simulation_time:
    start = time.time()
    # Pre-read/write processing
    mtr_cmd = calculate_mtr_cmd()
    str_cmd = calculate_str_cmd()
    write_pwm_buffer_throttle = np.array([mtr_cmd], dtype=np.float64)
    write_other_buffer_steering = np.array([str_cmd], dtype=np.float64)
    # Read-write operation
    card.write_pwm(write_pwm_channel_throttle, 1, write_pwm_buffer_throttle)
    card.write_other(write_other_channel_steering, 1, write_other_buffer_steering)
    card.read_other(read_other_channels_IMU, 6, read_other_buffer_IMU)
    # Post-read/write processing
    process_IMU_data(read_other_buffer_IMU)  
    end = time.time()
    # Timing step
    computation_time = start - end
    time.sleep(sample_time - computation_time)

card.close()