{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "### UE Command Test\n",
    "# This script is used to test the UE command"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys\n",
    "import time\n",
    "from pathlib import Path\n",
    "sys.path.append(str(Path().resolve().parent))\n",
    "from simworld.communicator.unrealcv import UnrealCV\n",
    "from simworld.communicator.communicator import Communicator\n",
    "from simworld.agent.humanoid import Humanoid\n",
    "from simworld.agent.scooter import Scooter\n",
    "from simworld.utils.vector import Vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Play in UE first\n",
    "ucv = UnrealCV()\n",
    "communicator = Communicator(ucv)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.get_objects()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Camera test"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.get_cameras()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Spawn a robot dog"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot_dog_name = \"Demo_Robot\"\n",
    "robot_dog_asset = \"/Game/Robot_Dog/Blueprint/BP_SpotRobot.BP_SpotRobot_C\"\n",
    "ucv.spawn_bp_asset(robot_dog_asset, robot_dog_name)\n",
    "ucv.set_location((0, 0, 20), robot_dog_name)\n",
    "ucv.enable_controller(robot_dog_name, True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(ucv.get_camera_location(1))\n",
    "print(ucv.get_camera_rotation(1))\n",
    "print(ucv.get_camera_fov(1))\n",
    "print(ucv.get_camera_resolution(1))\n",
    "ucv.show_img(ucv.get_image(1, 'lit'))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.set_camera_location(1, (0, 0, 0))\n",
    "ucv.set_camera_rotation(1, (0, 0, 0))\n",
    "ucv.set_camera_fov(1, 90)\n",
    "ucv.set_camera_resolution(1, (320, 240))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(ucv.get_camera_location(1))\n",
    "print(ucv.get_camera_rotation(1))\n",
    "print(ucv.get_camera_fov(1))\n",
    "print(ucv.get_camera_resolution(1))\n",
    "ucv.show_img(ucv.get_image(1, 'lit'))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.show_img(ucv.get_image(1, 'lit'))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Action test"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Robot dog test"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Look up"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.dog_look_up(robot_dog_name)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### look down"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.dog_look_down(robot_dog_name)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### transition"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#### Move forward\n",
    "speed = 200\n",
    "duration = 1\n",
    "direction = 0\n",
    "move_parameter = [speed, duration, direction]\n",
    "ucv.dog_move(robot_dog_name, move_parameter)\n",
    "time.sleep(duration)\n",
    "\n",
    "#### Move backward\n",
    "direction = 1\n",
    "move_parameter = [speed, duration, direction]\n",
    "ucv.dog_move(robot_dog_name, move_parameter)\n",
    "time.sleep(duration)\n",
    "\n",
    "#### Move left\n",
    "direction = 2\n",
    "move_parameter = [speed, duration, direction]\n",
    "ucv.dog_move(robot_dog_name, move_parameter)\n",
    "time.sleep(duration)\n",
    "\n",
    "#### Move right\n",
    "direction = 3\n",
    "move_parameter = [speed, duration, direction]\n",
    "ucv.dog_move(robot_dog_name, move_parameter)\n",
    "time.sleep(duration)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### rotation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#### Turn right\n",
    "angle = 90\n",
    "duration = 0.7\n",
    "clockwise = 1\n",
    "rotate_parameter = [duration, angle, clockwise]\n",
    "ucv.dog_rotate(robot_dog_name, rotate_parameter)\n",
    "time.sleep(duration)\n",
    "\n",
    "#### Turn left\n",
    "angle = -90\n",
    "counter_clockwise = -1\n",
    "rotate_parameter = [duration, angle, counter_clockwise]\n",
    "ucv.dog_rotate(robot_dog_name, rotate_parameter)\n",
    "time.sleep(duration)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### destory"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.destroy(robot_dog_name)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Humanoid avatar test "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Spawn humanoid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "humanoid_1 = Humanoid(Vector(0, 0), Vector(1, 0))\n",
    "# humanoid_path = '/Game/TrafficSystem/Pedestrian/Base_User_Agent.Base_User_Agent_C'\n",
    "humanoid_path = '/Game/Human_Avatar/DefaultCharacter/Blueprint/BP_Default_Character.BP_Default_Character_C'\n",
    "humanoid_name = 'GEN_BP_Humanoid_0'"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "communicator.spawn_agent(agent=humanoid_1, name=humanoid_name, model_path=humanoid_path)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Interaction"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.get_objects()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# arguing\n",
    "ucv.humanoid_argue(humanoid_name, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.humanoid_stop_current_action(humanoid_name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# discuss\n",
    "ucv.humanoid_discuss(humanoid_name, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.humanoid_stop_current_action(humanoid_name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# listen\n",
    "ucv.humanoid_listen(humanoid_name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.humanoid_stop_current_action(humanoid_name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# directing\n",
    "ucv.humanoid_directing_path(humanoid_name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# wave to dog\n",
    "ucv.humanoid_wave_to_dog(humanoid_name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.humanoid_stop_current_action(humanoid_name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# pick up light object\n",
    "communicator.spawn_object('BP_Mug_C_1', '/Game/InteractableAsset/Cup/BP_Mug.BP_Mug_C', (290, -110, 0), (0, 0, 0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.humanoid_pick_up_object(humanoid_name, \"BP_Mug_C_1\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# drop off object\n",
    "ucv.humanoid_drop_object(humanoid_name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.destroy('BP_Mug_C_1')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "communicator.spawn_object('BP_Interactable_Box_C_1', '/Game/InteractableAsset/Box/BP_Interactable_Box.BP_Interactable_Box_C', (290, -110, 0), (0, 0, 0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# pick up heavy object\n",
    "ucv.humanoid_pick_up_object(humanoid_name, \"BP_Interactable_Box_C_1\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# drop off object\n",
    "ucv.humanoid_drop_object(humanoid_name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.destroy('BP_Interactable_Box_C_1')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# enter and exit vehicle\n",
    "communicator.spawn_object('BP_VehicleBase_Destruction_C_1', '/Game/Interactable_Vehicle/Blueprint/BP_VehicleBase_Destruction.BP_VehicleBase_Destruction_C', (100, 200, 0), (0, 0, 0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.humanoid_enter_vehicle(humanoid_name, \"BP_VehicleBase_Destruction_C_1\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.humanoid_exit_vehicle(humanoid_name, \"BP_VehicleBase_Destruction_C_1\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.destroy('BP_VehicleBase_Destruction_C_1')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.destroy('GEN_BP_Humanoid_0')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucv.clean_garbage()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Scooter"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "agent_path = '/Game/TrafficSystem/Pedestrian/Base_User_Agent.Base_User_Agent_C'\n",
    "scooter_path = '/Game/ScooterAssets/Blueprints/BP_Scooter_Pawn.BP_Scooter_Pawn_C'"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "agent = Humanoid(Vector(0, 0), Vector(0, 1))\n",
    "scooter = Scooter(Vector(100, 0), Vector(0, 1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "communicator.spawn_agent(agent, agent_path)\n",
    "communicator.spawn_scooter(scooter, scooter_path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "communicator.humanoid_get_on_scooter(agent.id)\n",
    "agent.scooter_id = scooter.id"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "communicator.set_scooter_attributes(agent.scooter_id, 0, 0, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "communicator.humanoid_get_off_scooter(agent.id, scooter.id)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "communicator.humanoid_step_forward(agent.id, 2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "communicator.humanoid_rotate(agent.id, 90, 'left')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "communicator.disconnect()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "simworld",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.18"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}