#!/usr/bin/env python
# -*- coding: utf-8 -*-

import sys
import os
import time
import pandas as pd
from datetime import datetime
from threading import Thread

sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
from mecanumcarpy import MecanumCar

def control_car(car):
    """Control the movement of the car."""
    car.update_speed(100)
    car.move_forward()
    time.sleep(3)

    car.update_speed(110)
    car.move_back()
    time.sleep(3)

    car.update_speed(80)
    car.move_diagonally(direction='rb')
    time.sleep(3)

    car.stop()
    time.sleep(3)

    car.update_speed(70)
    car.move(direction='left')
    time.sleep(3)

    car.update_speed(127)
    car.send_rc_control(90, 100, 127)
    time.sleep(2)

    car.stop()

def print_state(car, duration, interval=1):
    """Print state information about the car every second."""
    time.sleep(5)

    start_time = time.time()
    while time.time() - start_time < duration-5:
        print(f"Timestamp: {datetime.now()}\n")
        print(f"Battery Voltage: {car.get_batteryVoltage()}\n")
        print(f"Battery Percent: {car.get_batteryPercent()}\n")
        print(f"Motor A: {car.get_motorA()}\n")
        print(f"Motor B: {car.get_motorB()}\n")
        print(f"Motor C: {car.get_motorC()}\n")
        print(f"Motor D: {car.get_motorD()}\n\n")

        time.sleep(interval)


def main():
    car = MecanumCar()
    car.send_keepalive()

    # Define duration for control and state collection
    duration = 60  # 60 seconds

    # Start state collection in a separate thread
    state_thread = Thread(target=print_state, args=(car, duration))
    
    # Start car control in a separate thread
    control_thread = Thread(target=control_car, args=(car,))

    # Start both threads
    state_thread.start()
    control_thread.start()

    # Wait for both threads to complete
    control_thread.join()
    state_thread.join()


if __name__ == "__main__":
    main()