import socket
import re
import sys
import threading
import math
import time

class Sensors:
    x = 0.0
    y = 0.0
    compass = 0.0
    bump = False
    sonar = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]

class Motors:
    forward = 0.0
    turn = 0
    def apply():
        Allen.getInstance().send()

class Allen:

    instance = None
    def getInstance():
        if not Allen.instance:
            Allen.instance = Allen()
        return Allen.instance
    
    def open(self):
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.connect(('localhost',7171))
        print('connected')
        self.buffer = ""
        self.t = threading.Thread(target=self.receive)
        self.t.start()
    
    # read a single line of text from a socket
    def getline(self):
        while self.buffer.find('\n')==-1:
            self.buffer += self.sock.recv(1024).decode()
        result = re.sub('[\r\n].*','',self.buffer)
        self.buffer = self.buffer[self.buffer.find('\n')+1:]
        return result
        
    def receive(self):
        print('receiving')
        try:
            while True:
                line = self.getline() 
                #print(line)
                if line.startswith('X = '):
                    Sensors.x = float(line[4:].split()[0])
                elif line.startswith('Y = '):
                    Sensors.y = float(line[4:].split()[0])
                elif line.startswith('Compass = '):
                    Sensors.compass = round(180*float(line[10:].split()[0])/math.pi)
                elif line.startswith('Bump = '):
                    Sensors.bump = line[7:].startswith('true')
                elif line.startswith('Sonar = '):
                    Sensors.sonar = [ float(d) for d in line[8:].split() ]
                else:
                    print("ERROR ",line)
        except ConnectionAbortedError:
            pass
        except ConnectionResetError:
            pass
        except OSError:
            pass

    # write a single line of text to a socket
    def putline(self,line):
        self.sock.send(line.encode())

    def send(self):
        try:
            if Motors.turn == 0:
                if Motors.forward >= 0.0001:
                    self.putline("f\r\n")
                elif Motors.forward <= -0.0001:
                    self.putline("b\r\n")
                else:
                    self.putline("z\r\n")
            else:
                if Motors.turn < 0:
                    self.putline("l\r\n")
                else:
                    self.putline("r\r\n")  
        except ConnectionAbortedError:
            pass
        except ConnectionResetError:
            pass
        except OSError:
            pass
        
    def close(self):
        self.sock.close()

if __name__ == "__main__":

    robot = Allen.getInstance()
    robot.open()
    while True:
        print(Sensors.x)
        print(Sensors.y)
        print(Sensors.compass)
        time.sleep(0.5)
#    input()
#    print(Sensors.x)
#    print(Sensors.y)
#    print(Sensors.compass)
#    print(Sensors.bump)
#    print(Sensors.sonar)
#    input()
#    print(Motors.forward)
#    Motors.forward = 1.0
#    Motors.apply()
#    print(Motors.forward)   
#    input()
#    print(Sensors.x)
#    print(Sensors.y)
#    print(Sensors.compass)
#    print(Sensors.bump)
#    print(Sensors.sonar)
#    input()
#    robot.close()
#    input()
