#!/usr/bin/env python

# This is a python test program that refresh the Noto minor servo system
#who                                   when           what
#Andrea Orlati(a.orlati@ira.inaf.it)   02/12/2016     Creation
from __future__ import print_function

import getopt, sys
import Acspy.Common.Err
import maciErrType
import maciErrTypeImpl
import ClientErrorsImpl
import ACSLog
from Acspy.Clients.SimpleClient import PySimpleClient
from Acspy.Common.TimeHelper import getTimeStamp
from Acspy.Common.EpochHelper import EpochHelper
from Acspy.Common.DurationHelper import DurationHelper
import string
import math
import time
import socket
import struct
from math import *
import os
import thread

offsets=[0.0,0.0,0.0,0.0,0.0]
exitFlag=False

def readPipe(pipeName):
	print("Opening pipe......")
	pipeIn=os.open(pipeName,os.O_RDONLY|os.O_NONBLOCK)
	print("Pipe opened for reading......")
	while not exitFlag:
		try:
			pipeString=os.read(pipeIn,64)
			if len(pipeString)>0:
				print("received offsets %s"%pipeString)
				info=pipeString.split(',')
				print(info)
				offsets[0]=float(info[0])
				offsets[1]=float(info[1])
				offsets[2]=float(info[2])
				offsets[3]=float(info[2])
				offsets[4]=float(info[2])
				time.sleep(1)
		except Exception as ex:
			pass
			#print(ex)
	os.close(pipeIn)
        
def usage():
	print("subr [-h|--help] [-c|--code=]")
	print("")
	print("[-h|--help]      displays this help")
	print("[-c|--code=]     select the setup code")

def sendData(socket,msg):
	length=len(msg)	
	socket.sendall(struct.pack('=i',length))
	socket.sendall(msg)

def main():
    
	try:
		opts, args = getopt.getopt(sys.argv[1:],"hc:",["help","code="])
	except getopt.GetoptError as err:
		print(str(err))
		usage()
		sys.exit(1)
        
	code=""
	polX=[0.0,0.0,0.0]
	polY=[0.0,0.0,0.0]
	polZ1=[0.0,0.0,0.0]
	polZ2=[0.0,0.0,0.0]
	polZ3=[0.0,0.0,0.0]
	pipeName="/tmp/subrPipe"

	for o, a in opts:
		if o in ("-h", "--help"):
			usage()
			sys.exit()
		elif o in ("-c", "--code"):
			code = a

	print("The code is %s" % code)

	if code=="QQC":
		polX[0]=0.0
		polX[1]=0.0
		polX[2]=0.89
		maxX=100
		minX=-100

		polY[0]=8.3689e-4
		polY[1]=0.152495
		polY[2]=20.91
		maxY=100
		minY=-100

		polZ1[0]=0.00168640  
		polZ1[1]=-0.271430
		polZ1[2]=67.55
		maxZ1=100
		minZ1=-100

		polZ2[0]=0.00168640  
		polZ2[1]=-0.271430
		polZ2[2]=84.37
		maxZ2=100
		minZ2=-100

		polZ3[0]=0.00168640  
		polZ3[1]=-0.271430
		polZ3[2]=-57.40
		maxZ3=100
		minZ3=-100

	elif code=="KKC":
		polX[0]=0.0
		polX[1]=0.0
		polX[2]=-0.5
		maxX=85
		minX=-85

		polY[0]=0.0069654
		polY[1]=-0.3007735
		polY[2]=-10.6464206
		maxY=85
		minY=-85

		polZ1[0]=0.00168640  
		polZ1[1]=-0.271430
		polZ1[2]=7.3
		maxZ1=85
		minZ1=-85

		polZ2[0]=0.00168640
		polZ2[1]=-0.271430
		polZ2[2]=-0.2
		maxZ2=85
		minZ2=-85

		polZ3[0]=0.00168640
		polZ3[1]=-0.271430
		polZ3[2]=2.6
		maxZ3=85
		minZ3=-85

	else:
		print("Unknown code")
		sys.exit(1);

	print("Preparing pipe.....")
	if not os.path.exists(pipeName):
		print("Creating pipe")
		os.mkfifo(pipeName,0777)
		print("Created")
	else:
		print("pipe already exists")

	try:
		pipeThread=thread.start_new_thread(readPipe,(pipeName, ) )
	except Exception as ex:
		print(ex)
		sys.exit(1)

	#get the link to the SRTmountcomponent
	simpleClient = PySimpleClient()
	compName=""
	compType = "IDL:alma/Antenna/AntennaBoss:1.0"
	try:
		component=simpleClient.getDefaultComponent(compType)
		compName=component._get_name()
	except Exception as ex:
		newEx = ClientErrorsImpl.CouldntAccessComponentExImpl( exception=ex, create=1 )
		newEx.setComponentName(compType)
		newEx.log(simpleClient.getLogger(),ACSLog.ACS_LOG_ERROR)
		sys.exit(1)  
		
	print("Antenna boss retrieved")

	client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
	client_socket.connect(('192.167.187.92', 5003))

	print("socket connected")
	time.sleep(2)

	try:
		while 1:
			ctime=getTimeStamp().value
			try:
				az,el=component.getRawCoordinates(ctime)
			except Exception as ex:
				print("Errore nella lettura della posizione")
			delev=degrees(el)
			print(el,delev)
			posX=polX[0]*delev*delev+polX[1]*delev+polX[2]
			posY=polY[0]*delev*delev+polY[1]*delev+polY[2]
			posZ1=polZ1[0]*delev*delev+polZ1[1]*delev+polZ1[2]
			posZ2=polZ2[0]*delev*delev+polZ2[1]*delev+polZ2[2]
			posZ3=polZ3[0]*delev*delev+polZ3[1]*delev+polZ3[2]
			posX=posX+offsets[0]
			posY=posY+offsets[1]
			posZ1=posZ1+offsets[2]
			posZ2=posZ2+offsets[3]
			posZ3=posZ3+offsets[4]
			if posX>maxX:
				posX=maxX
			if posX<minX:
				posX=minX
			if posY>maxY:
				posY=maxY
			if posY<minY:
				posY=minY
			if posZ1>maxZ1:
				posZ1=maxZ1
			if posZ1<minZ1:
				posZ1=minZ1
			if posZ2>maxZ2:
				posZ2=maxZ2
			if posZ2<minZ2:
				posZ2=minZ2
			if posZ3>maxZ3:
				posZ3=maxZ3
			if posZ3<minZ3:
				posZ3=minZ3

			print("comando %6.2lf %6.2lf %6.2lf %6.2lf %6.2lf" % (posX,posY,posZ1,posZ2,posZ3))
			buffer="0,1,7,%6.2lf,%6.2lf,%6.2lf,%6.2lf,%6.2lf" % (posX,posY,posZ1,posZ2,posZ3)
			#print(buffer)
			sendData(client_socket,buffer)
			time.sleep(5)
			data=client_socket.recv(128)
			#print("risposta ", data)
			time.sleep(1)
	finally:
		exitFlag=True
		pipeThread.join(timeout=5)
		if not (compName==""):
			simpleClient.releaseComponent(compName)
			simpleClient.disconnect()
		


if __name__=="__main__":
   main()
