#import socket
from socket import *
#import pandas as pd
import os
import struct

import sys
#import numpy  # Import numpy
#import matplotlib.pyplot as plt #import matplotlib library
from drawnow import *

encoder = []

#host = socket.gethostbyname(socket.gethostbyname())
def sendeth(src, dst, eth_type, payload, interface = 'lo'):

	s = socket(AF_PACKET, SOCK_RAW)
	s.bind((interface, 0))
	return s.send(src + dst + eth_type + payload)

#s.setsockopt(socket.IPPROTO_IP, socket.IP_HDRINCL, 1)

def getData():
	dir_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__)))
	dat = pd.read_csv(dir_path + '/randomdata.csv')
	return 0

#def sendeth(src_mac, payload):
def sendByteRequested(srcMac, instruction, int):
	#Src mac in integer
	#dstMac may/should be a program constant
	#instruction is single byte 0x1, 0x2, 0x3 or 0x4
	#payload is char array

	src = "\x00\x00\x00\x00\x00" + chr(srcMac)

	#dstMac = "\x89\x34\x55\xEF\xFE\x12"


	s.send(dstMac + src + "\x88\xB5" + instruction + bytearray(struct.pack("!c",chr(int))))
def sendInt(srcMac, instruction, int):
	src = "\x00\x00\x00\x00\x00" + chr(srcMac)
	s.send(dstMac + src + "\x88\xB5" + instruction + bytearray(struct.pack("!i",int)))
def sendPacket(srcMac, instruction, payload):
	#Src mac in integer
	#dstMac may/should be a program constant
	#instruction is single byte 0x1, 0x2, 0x3 or 0x4
	#payload is char array
	src = "\x00\x00\x00\x00\x00" + str(srcMac)

	src = "\x00\x00\x00\x00\x00" + chr(srcMac)

	#dstMac = "\x00\x0c\x29\x8F\xE0\x19"


	s.send(dstMac + src + "\x88\xB5" + instruction + bytearray(struct.pack("f",payload)))
def sendPindriverPacket(srcMac, votlage, current, temperature):
	s.send( dstMac + "\x00\x00\x00\x00\x00" + chr(srcMac) + "\x88\xB5" + "\x04" + bytearray(struct.pack("=f", votlage)) + bytearray(struct.pack("=f", current)) + bytearray(struct.pack("=f", temperature)))


def receiveHandler():
	#dstMac = "\x00\x25\x99\x03\x00\x06"

	packet  = s.recvfrom(1024)
	packet = packet[0]
	eth_length = 15
	eth_header = packet[:eth_length]
	eth = struct.unpack('!6s6sHc', eth_header)

	eth_protocol = eth[2]
	instruction = eth[3]
	instruction = ord(instruction)
	destMac = eth[0]
	recevingMacIndex = ord(destMac[5])
	#print(ord(destMac[5]))
	print(eth_protocol)
	#print(':'.join(x.encode('hex') for x in eth[1]))
	#print((instruction))


	if(eth_protocol == 34997):
		if (instruction == 1):
			print("1 instruction received")
			sendByteRequested(recevingMacIndex, "\x01", 10)

		elif(instruction == 2):
			#what do i do now?
			print("2 instruction received")
			#print(len(packet[eth_length:eth_length+4]))
			data = struct.unpack('i', packet[eth_length:eth_length+4])
			data2 = struct.unpack('!i', packet[eth_length:eth_length+4])
			print("Offset: ", data[0])
			sendInt(recevingMacIndex, "\x02", data2[0])

		elif(instruction == 3):
			print("3 instruction received")
			index[recevingMacIndex] = 1
		elif(instruction == 4):
			print(len(packet))
			if (len(packet)>=60):
				data = struct.unpack_from('=ffffffff', packet[eth_length:eth_length+60])
				voltage = data[0]
				current = data[1]
				mode = data[2]
				print('time: ', data[0])
				print('Servo: ', data[1])
				print('Smooth servo: ', data[2])
				print('LVDT', data[3])

				print('Encoder: ', data[4])
				print('Winch', data[5])
				print('Brake Sensor: ', data[6])
				print('Pressure: ', data[7])

			#	encodervals.append(data[10])
			#	brakePressure.append(data[7])
			#	global cnt
			#	cnt=cnt+1
			#	if(cnt>50):                            #If you have 50 or more points, delete the first one from the array
			#		encodervals.pop(0)                       #This allows us to just see the last 50 data points
			#		brakePressure.pop(0)
			#	drawnow(makeFig)                       #Call drawnow to update our live graph
			#	plt.pause(.000001)                     #Pause Briefly. Important to keep drawnow from crashing

				#sendPindriverPacket(1,0, -0.03, 789.0)
				#sendPindriverPacket(2, 0, data[4])




	#print(packet[:eht_length+1])
	# print(':'.join(x.encode('hex') for x in packet[:eht_length+2]))
	# if (len(packet) == 15 ): #only instruction
	# 	instruction = struct.unpack('!c', packet[eth_length:])
	# 	print(instruction[0])
	# elif (len(packet) > 15):
	# 	print(len(packet))
	# 	data = struct.unpack('!cfff', packet)
	# #print(hex(struct.unpack()))
	#if (eth_protocol == 34997):
		#print(eth[3])
# def makeFig(): #Create a function that makes our desired plot
#     plt.ylim(-5,5)                                 #Set y min and max values
#     plt.title('My Live Streaming Sensor Data')      #Plot the title
#     plt.grid(True)                                  #Turn the grid on
#     plt.ylabel('Rotation')                            #Set ylabels
#     plt.plot(encodervals, 'ro-', label='rotation F')       #plot the temperature
#     plt.legend(loc='upper left')                    #plot the legend
#     plt2=plt.twinx()                                #Create a second y axis
#     plt.ylim(0,200)                           #Set limits of second y axis- adjust to readings you are getting
#     plt2.plot(brakePressure, 'b^-', label='Pressure (Bars)') #plot pressure data
#     plt2.set_ylabel('Pressrue (Pa)')                    #label second y axis
#     plt2.ticklabel_format(useOffset=False)           #Force matplotlib to NOT autoscale y axis
#     plt2.legend(loc='upper right')                  #plot the legend


def float_to_hex(f):

	#not used function
	return hex(struct.unpack('<I', struct.pack('<f', f))[0])
if __name__ == "__main__":
	global dstMac
	global index
	index = []
	encodervals = []
	brakePressure = []
	plt.ion()
	global cnt
	cnt = 0

	dstMac = "\x00\x25\x99\x03\x00\x06"


	interface = 'ens33'
	s = socket(AF_PACKET, SOCK_RAW)
	s.bind((interface, 3))
	#sendPacket(3,0x03, 1.3)
	#sendPindriverPacket(1,2.2,0,50)
	while True:
		receiveHandler()


		#sendInt(2, "\x02", 10)
	#sendeth("\x12\x34\x56\x78\xBE\xEF","\x89\x34\x55\xEF\xFE\x12", "\xF1\x12","hello")