TAG-MAC03118/tag.py

import socket
import sys
import threading
import numpy as np
from socket import error as SocketError


class Robot():
    #        frente
    #       ---------
    #       |       |
    #       |       |
    #       |  LLL  |
    #       |  LLL  |
    #       ---------

    # idd, id do robo
    # xsize, largura do robo
    # xsize, comprimento do robo
    # xtag, distancia horizontal do lado esquerdo ao cento do tag
    # ytag, distancia vertical do topo ao cento do tag
    # posx, posy, course, posicao do robo
    def __init__(self, idd, xsize, ysize, xtag, ytag, posx = 0, posy = 0, course = 0):
        self.idd = idd
        self.xsize = xsize
        self.ysize = ysize
        self.xtag = xtag
        self.ytag = ytag

        self.posx = posx
        self.posy = posy
        self.course = course

    def lines(self):
        p1 = np.array([-self.xtag, self.ytag])
        p2 = np.array([-self.xtag, -self.ysize+self.ytag])
        p3 = np.array([self.xsize-self.xtag, self.ytag])
        p4 = np.array([self.xsize-self.xtag, -self.ysize+self.ytag])

        pos = np.array([self.posx, self.posy])

        c, s = np.cos(self.course), np.sin(self.course)
        R = np.array(((c,-s), (s, c)))

        p1 = p1.dot(R)+pos
        p2 = p2.dot(R)+pos
        p3 = p3.dot(R)+pos
        p4 = p4.dot(R)+pos
        return np.array([[p1, p2], [p2, p4], [p4, p3], [p1, p3]])

    def ccw(self, A,B,C):
        return (C[1]-A[1]) * (B[0]-A[0]) > (B[1]-A[1]) * (C[0]-A[0])

    # Return true if line segments AB and CD intersect
    def line_intersect(self, A,B,C,D):
        return ccw(A,C,D) != ccw(B,C,D) and ccw(A,B,C) != ccw(A,B,D)

    # get robot and see if this intersect te other
    def intersect(self, robot):
        thislines = self.lines()
        other = robot.lines()
        for i in thislines:
            for j in thislines:
                if line_intersect(i[0], i[1], j[0], j[1]):
                    return True
        return False


class TagServer(threading.Thread):
    def __init__(self, port, board_size):
        threading.Thread.__init__(self)

        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server_address = ('0.0.0.0', port)
        print ('Starting up on %s port %s' % server_address)
        sock.bind(server_address)
        sock.listen(1)

        sock.settimeout(1)
        self.sock = sock
        self.clients_threads = []

        self.tag = None
        self.board_size = board_size

        self.runV = True
        self.start()

    def updatePosition (self, idd, posx, posy, course):
        for thread in self.clients_threads:
            if thread.robot is not None and thread.robot.idd == idd:
                msg = "P "
                msg = msg + str(posx)+","
                msg = msg + str(posy)+","
                msg = msg + str(course)+","
                msg = msg + str(thread.robot.idd)+","
                msg = msg + str(thread.robot.xsize)+","
                msg = msg + str(thread.robot.ysize)+","
                msg = msg + str(thread.robot.xtag)+","
                msg = msg + str(thread.robot.ytag)
                thread.robot.posx = posx
                thread.robot.posy = posy
                thread.robot.course = course
                self.brodcast(msg)

        # check for collision
        for i in self.robots():
            if i != self.tag:
                continue
            for j in self.robots():
                if i == j:
                    continue
                if i.intersect(j):
                    self.tag = j
                    self.brodcast("RUN "+str(self.tag.id)+" 10")

    def startGame (self):
        self.brodcast("STOP")

    def startGame (self):
        self.brodcast("START")
        if self.tag is None:
            tag = np.random.choice(self.robots(), 1)
            if len(tag) == 1:
                self.tag = tag
                self.brodcast("RUN "+str(self.tag.id)+" 10")
                return True
        return False

    def brodcast(self, msg):
        for thread in self.clients_threads:
            if thread.robot is not None:
                thread.send(msg+"\n")

    def run(self):
        while self.runV:
            try:
                (clientsock, (ip, port)) = self.sock.accept()
                newthread = TagThread(ip, port, clientsock, self)
                self.clients_threads.append(newthread)
            except socket.timeout as err:
                pass
        self.sock.close()

    def robots(self):
        r = []
        for thread in self.clients_threads:
            if thread.robot is not None:
                r.append(thread.robot)
        return r

    def stop(self):
        for thread in self.clients_threads:
            thread.stop()
        self.runV = False


class TagThread(threading.Thread):

    def __init__(self, ip, port, socket, server):
        threading.Thread.__init__(self)
        self.ip = ip
        self.port = port
        self.sock = socket

        self.points = 0

        self.robot = None
        self.bz = server.board_size
        self.server = server

        socket.settimeout(1)
        self.runV = True
        self.start()


    def run(self):
        while self.runV:
            try:
                data = self.sock.recv(64).decode('ascii')
            except socket.timeout as err:
                continue
            except SocketError as e:
                self.robot = None
                break
            data = data.split(" ")
            try:
                if data[0] == "SUB":
                    idd, xsize, ysize, xtag, ytag = data[1].split(",")
                    self.robot = Robot(int(idd), float(xsize), float(ysize), float(xtag), float(ytag))
                    self.send("OK "+str(self.bz[0])+","+str(self.bz[1])+"\n")
                if data[0] == "P":
                    posx, posy, curse = data[1].split(",")
                    posx, posy, curse = float(posx), float(posy), float(curse)
                    self.server.updatePosition(self.robot.idd, posx, posy, curse)
            except Exception as e:
                print(e)
                self.send("ERR\n")
        self.sock.close()

    def send(self, msg):
        self.sock.sendall(msg.encode('ascii'))

    def stop(self):
        self.runV = False

class TagClient(threading.Thread):
    def __init__(sself, ip, port):
        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server_address = (ip, port)
        sock.connect(server_address)
        self.sock = sock
        self.stop = None
        self.start = None
        self.tagger = None
        self.position = None

    def info (self, idd, xsize, ysize, xtag, ytag):
        message = "SUB "+idd+","+xsize+","+ysize+","+xtag+","+ytag+"\n"
        self.sock.sendall(message.encode('ascii'))
        self.start()

    def run(self):
        while True:
            try:
                data = self.sock.recv(64).decode('ascii')
            except SocketError as e:
                self.robot = None
                break
            data = data.split("\n")[0].split(" ")
            if data[0] == "OK":
                self.board_size = self.position(data[1].split(","))
                print("SUB OK")
            elif data[0] == "P" and self.position is not None:
                self.position(data[1].split(","))

    def stopCallback (self, function):
        self.stop = function

    def startCallback (self, function):
        self.start = function

    def taggerCallback (self, function):
        self.tagger = function

    def positionCallback (self, function):
        self.position = function

    def boardinfoCallback (self, function):
        self.boardinfo = function

    # Somente na simulacao
    def setPosition(px, py):
        message = "P "+px+","+py+"\n"
        self.sock.sendall(message.encode('ascii'))