#!/usr/bin/env python
# -*- coding: utf-8 -*-
from __future__ import print_function
import roslib
roslib.load_manifest('formulapi_integration')
import sys
import traceback
import rospy
import cv2
import numpy
import math
import logging
import socket
import threading
import time
import datetime
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Twist

from formulapi_source import ImageProcessor

class formulapi_sim(object):
    def __init__(self):
      self.imgprocess = ImageProcessor.ImageProcessor()
      #self.carctrl = CarControl()

      """ROS Subscriptions """
      self.image_pub = rospy.Publisher("/image_converter/output_video",Image, queue_size=10)
      self.image_sub = rospy.Subscriber("/cam/camera_/image_raw",Image,self.imgprocess.ProcessingImage)

      self.cmdVelocityPub = rospy.Publisher('/platform_control/cmd_vel', Twist, queue_size=10)

      self.cmdvel = Twist()
      
    def run(self):
      self.cmdvel.linear.x = 1.0
      self.cmdvel.linear.y = self.imgprocess.angle
      self.cmdvel.linear.z = self.imgprocess.trackoffset
      self.cmdVelocityPub.publish(self.cmdvel)

def main(args):

  #rospy.init_node('formulapi_SITL', anonymous=True)

  sim = formulapi_sim() 
  sim.run() 


  """
  carcontrol = CarControl(0)
  carcontrol.Start()
  carcontrol.racestarted = True
  carcontrol.targetspeed = 20
  """

  try:
    rospy.spin()
  except KeyboardInterrupt:
    print("Shutting down")
  cv2.destroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)
