#!/usr/bin/env python2
from __future__ import print_function


##### add python path #####
import sys
import rospkg
PATH = rospkg.RosPack().get_path("sim2real") + "/scripts"
sys.path.append(PATH)
###########################

import gym
import env
import numpy as np
import rospy
from std_msgs.msg import Int32

env = gym.make('RCCar-v0')
env.seed(1)
env = env.unwrapped
env.reset()

class KeyboardController:
    def __init__(self):
        self.sub = rospy.Subscriber("key_op", Int32, self.callback)
        self.rate = rospy.Rate(10)

        self.omega = 0.0
        self.velocity = 0.0
        self.MAX_OMEGA = 1.0
        self.MAX_VELOCITY = 2.0
        self.is_alive = True
        self.controller()

    def callback(self, msg):
        signal = msg.data
        # TO DO


    def controller(self):
        while True:
            if not self.is_alive:
                break
            _, _, done, _ = env.step(self.omega, self.velocity)
            if done:
                env.reset()
            self.rate.sleep()

if __name__ == '__main__':
    try:
        KeyboardController()
    except rospy.ROSInterruptException:
        pass
