#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Pose2D, Pose
from math import *
import time
import socket
import readchar
import sys
from struct import *
from std_msgs.msg import ByteMultiArray


class HexaGuide():

    def __init__(self):
        self.pose1      = Pose2D()
        self.pose2      = Pose2D()
        self.marker     = Pose2D()
        self.marker2    = Pose2D()

        self.IP1        = '192.250.9.9'
        self.IP2        = '192.168.4.1'
        #self.IP2        = '192.155.5.5' #if Lein hex then ovaj IP
        self.Port       = 80

        self.dat = [0,0,0]
        self.a = ByteMultiArray(data=self.dat)

        #self.array = ByteMultiArray(self.data)
        self.power1     = 0
        self.angle1     = 0
        self.rotation1  = 0
        self.staticTilt = 0
        self.movingTilt = 0
        self.onoff1     = 1
        self.accX       = 0
        self.accY       = 0
        self.sliders1   = 50
        self.sliders2   = 25
        self.sliders3   = 0
        self.sliders4   = 0
        self.sliders5   = 0
        self.sliders6   = 50
        self.sliders7   = 0
        self.sliders8   = 0
        self.sliders9   = 0
        self.duration   = 20

        self.power2     = 0
        self.angle2     = 0
        self.rotation2  = 0
        self.onoff2     = 1
        self.switch1    = False
        self.switch2    = False
        self.switch3    = False
        rospy.Subscriber("/pose_node/pose_id1", Pose2D, self.pose_callback1)
        rospy.Subscriber("/pose_node/pose_id2", Pose2D, self.pose_callback2)
        rospy.Subscriber("/pose_node/pose_id15", Pose2D, self.pose_callback15)
        rospy.Subscriber("/pose_node/pose_id4", Pose2D, self.pose_callback4)
        self.pub1 = rospy.Publisher("/algoritam/naredbe1",ByteMultiArray, queue_size=10)
        self.pub2 = rospy.Publisher("/algoritam/naredbe2",ByteMultiArray, queue_size=10)


    def pose_callback1(self, data):
        self.marker.x = data.x
        self.marker.y = data.y
        self.marker.theta = data.theta

    def pose_callback2(self, data):
        self.pose1 = data
        self.pose1.x = data.x
        self.pose1.y = data.y
        self.pose1.theta = data.theta

    def pose_callback15(self, data):
        self.pose2.x = data.x
        self.pose2.y = data.y
        self.pose2.theta = data.theta

    def pose_callback4(self, data):
        self.marker2.x = data.x
        self.marker2.y = data.y
        self.marker2.theta = data.theta


    def run(self):
        s1 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        print "JA 1"
        s2 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        print "JA 2"
        s1.connect((self.IP1, self.Port))
        print "JA 3"
        s2.connect((self.IP2, self.Port))
        print "JA 4"
        print rospy.is_shutdown()
        while not(rospy.is_shutdown()):
            print "JA 5"


            self.dat[0] = 56
            self.dat[1] = 12
            self.dat[2] = 12
            self.a = ByteMultiArray(data=[self.dat[0],self.dat[1],self.dat[2]])
            print self.a
            self.pub1.publish(self.a)
            time.sleep(0.2)

        #self.power1 = 0
        #self.power2 = 0
        #self.niz1()
        #self.niz2()
        #s1.send(self.paket1)
        #s2.send(self.paket2)
        #time.sleep(0.2)




    def niz1(self):

        self.paket1=pack('cccbbbbbbbbbbbbbbbbbBB','P','K','T', self.power1,(self.angle1/2), self.rotation1, self.staticTilt, self.movingTilt, self.onoff1, self.accX, self.accY, self.sliders1, self.sliders2, self.sliders3, self.sliders4, self.sliders5, self.sliders6, self.sliders7, self.sliders8, self.sliders9, (self.duration/256), (self.duration%256))

    def niz2(self):

        self.paket2=pack('cccbbbbbbbbbbbbbbbbbBB','P','K','T', self.power2,(self.angle2/2), self.rotation2, self.staticTilt, self.movingTilt, self.onoff2, self.accX, self.accY, self.sliders1, self.sliders2, self.sliders3, self.sliders4, self.sliders5, self.sliders6, self.sliders7, self.sliders8, self.sliders9, (self.duration/256), (self.duration%256))


    def nula(self):
        s1 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s2 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s1.connect((self.IP1, self.Port))
        s2.connect((self.IP2, self.Port))
        self.power1     = 0
        self.angle1     = 0
        self.power2     = 0
        self.angle2     = 0
        self.rotation1  = 0
        self.rotation1  = 0
        self.niz1()
        self.niz2()
        s1.send(self.paket1)
        s2.send(self.paket2)
        time.sleep(1)

if __name__ == '__main__':

    try:
        print "prvi"
        rospy.init_node('Control', anonymous = True)
        Hex = HexaGuide()
        # Hex.nula()
        Hex.run()
        rospy.spin()
        print "drugi"
    except KeyboardInterrupt:
        print "cetvrti"
    except rospy.ROSInterruptException:
        print "treci"
        Hex.nula()
        s1.close()
        s2.close()
        sys.exit()
        print "cetvrti"