#!/usr/bin/python3
"""
Created on Tue Jan 11 2022

@author : ALKADRI Mohammad
E-mail : mohammad.alkadri.95@gmail.com
"""
import os
import threading
import time
import socket
import string
import serial
import numpy as np
import test_10MHz
from datetime import datetime
import struct
import subprocess
from pwm_cli import *
import one
import ad

deltaF0 = 0
deltaT0 = 0
fs=1.5e6
seuilpps=0.08*127*2;
B=125e3;
d=1/980;
freq_int=1e4;
longueur_chirps=8;
def changePPS(x):
	dict_cfg = {}
	dict_cfg["oneshot"] = x
	try:
		pwm_cli_handler(dict_cfg)
	except:
		print("error PPS")
	time.sleep(1)

def thread_TTN_Start():
	time.sleep(1.4)
	os.popen("echo -n 'S' > /dev/ttyUSB2")

def thread_TTN_Times():
	os.popen("echo -n 'T' > /dev/ttyUSB2")
	time.sleep(8)
	os.popen("echo -n 'T' > /dev/ttyUSB2")

def Data_RTL():
	client = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
	ip=socket.gethostbyname("127.0.0.1")
	port=1333
	address=(ip,port)
	client.connect(address)
	N=0
	i=0
	Ntot=1500000*2*3
	data=[]
	threading.Thread(target=thread_TTN_Start).start()
	while N<Ntot:
		data[N:N+1023]= client.recv(1024)
		N+=1024
	client.close()
	if ((len(data)%2)==1):
		data=data[0:-1]
	x=np.array(data).reshape(-1)
	#x.tofile("xxxx.txt")
	print(len(x))
	x=x[0::2]+1j*x[1::2]
	x=x-127-127j;
	return x

def PPS(x):
	# identification des fronts montants des 1-PPS sur endpoint
	pps1=(np.absolute(x)>seuilpps).nonzero()
	dpps=np.diff(pps1,n=1,axis=1).reshape(-1)
	k=np.array((dpps>50).nonzero()).reshape(-1) ; # (fs*0.1)); % elimine les communications LoRa
	pps1=np.array(pps1).flatten()
	pps=np.array(pps1)[k.astype(int)];
	"""
	k=(np.diff(pps,n=1,axis=0)>(fs*0.4)).nonzero()  # elimine les fronts descendants
	k=np.array(k).reshape(-1)
	if k[0]==0: 
		k=k[1:];                           # cas ou` acquisition commence dans un PPS haut
	"""
	k=(np.diff(pps,n=1,axis=0)==300000).nonzero()
	k=np.array(k).reshape(-1)
	try:
		ppsep=pps[k];                              # indice des 1-PPS sur ep
	except:
		ppsep=[1]
	if (ppsep.size == 0):
		ppsep=[1]
	print("ppsep = ", ppsep)
	print(type(ppsep))
	return ppsep

def Chirp_detect(x,t):
	ddd= x[int((t+1)*1e6):int((t+2)*1e6)]
	cc=[]
	cc[0:2000000]=np.fft.fft(ddd)
	sssr=[]
	sssd=[]
	cc=np.array(cc)
	ccris=np.fromfile('ccris.bin',dtype=np.complex128)
	ccdec=np.fromfile('ccdec.bin',dtype=np.complex128)
	N=0
	sssr[N:N+len(ccris)-1]=cc[N:N+len(ccris)]*ccris
	sssr=np.array(sssr).reshape(-1)
	chirp_detect_ris=np.fft.ifft(sssr)
	sssd[N:N+len(ccdec)-1]=cc[N:N+len(ccdec)]*ccdec
	sssd=np.array(sssd)
	chirp_detect_dec=np.fft.ifft(sssd)
	chirp_detect=np.absolute(chirp_detect_ris)+np.absolute(chirp_detect_dec);
	seuil=0.65*max(chirp_detect);

	k=np.array((chirp_detect>seuil).nonzero()).reshape(-1);
	try:
		debut=int(k[0]-d*fs+(t-1)*1e6+2000000) # fin_ep1=k(end); % /!\ ATTENTION RISQUE D'AVOIR LES DEUX COMM
	except:
		debut=int((t-1)*1e6+2000000)
	print("debut =",debut)
	return debut

def Time(x,debut,ppsep,t):
	global indice_pps
	xx=x[debut:(debut+int(d*fs*longueur_chirps))]; # transmission endpoint
	xx=np.fft.fft(xx)
	if t==1:
		ccy=np.fromfile('ccrisy.bin',dtype=np.complex128)
		print("ccy1")
	else:
		ccy=np.fromfile('ccdecy.bin',dtype=np.complex128)
		print("ccy2")
	y=np.absolute(np.fft.ifft(xx * ccy));
	pos1=np.array((y>(max(y)/2)).nonzero()).reshape(-1);
	try:
		pos1=pos1[0]; # premier chirp
	except:
		pos1=1
	yy=y[pos1:pos1+int(d*fs)]
	v=np.argmax(yy)
	print("np.max(y) ",np.max(y))
	print("np.max(yy) ",np.max(yy))
	print("v ",v)
	print("yy[v] ",yy[v])
	if v != 1529:
		v= v+ 1/2*((yy[v-1]-yy[v+1])/(yy[v-1]+yy[v+1]-2*yy[v]))
	print("v ",v)
	t1=v+pos1
	print("t1 ", t1)
	debut=pos1+v-1-1+debut
	if t==1:
		if (ppsep[0]<=debut):
			indice_pps=ppsep[(np.array((ppsep<debut).nonzero()).reshape(-1))[-1]]
		else:
			indice_pps=0
	debut=debut-indice_pps   # /!\ ecart premiere transmission
	print("debut = ", debut)
	pos2=pos1+6*1536
	yy=y[pos2:pos2+int(d*fs)]
	v=np.argmax(yy)
	print("np.max(y) ",np.max(y))
	print("np.max(yy) ",np.max(yy))
	print("v ",v)
	print("yy[v] ",yy[v])
	if v != 1529:
		v= v+ 1/2*((yy[v-1]-yy[v+1])/(yy[v-1]+yy[v+1]-2*yy[v]))
	print("v ",v)
	t2 = v+pos2
	print("t2 ",t2)
	te= t2-t1
	print("te ", te)
	return debut, indice_pps

def TEP():
	x=Data_RTL()
	pps=PPS(x)
	debut1=Chirp_detect(x,1)
	debut1, indice_pps= Time(x,debut1,pps,1)
	x[0:int(debut1+indice_pps+d*fs*600)]=0
	print(pps)
	debut2=Chirp_detect(x,2)
	debut2, indice_pps= Time(x,debut2,pps,2)
	Tep=debut2-debut1
	print("Tep = ", Tep)
	return debut1, Tep


while (1):
	print("deltaT0 = ", deltaT0)
	print("deltaF0 = ", deltaF0)
	#os.popen("./litex_server --uart --uart-port /dev/ttyUSB1 &")
	print(datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S'))
	debut1, Tep= TEP()

	time.sleep(5)
	threading.Thread(target=thread_TTN_Times).start()
	time.sleep(4)
	port = '/dev/ttyUSB2'
	ser = serial.Serial(port, 115200, timeout=7)
	Tgw=ser.readline()
	print("Tgw = ", Tgw)
	ser.close()
	print(len(Tgw))
	if len(Tgw)>5:
		Tgw = Tgw[0:-1]
		Tgw=int(Tgw,16)
		print(Tgw)
		#Tgw=np.int32(Tgw)
		Tgw=struct.unpack('dd',Tgw.to_bytes(16,'big')) 
		print("Tgw = ", Tgw)
		deltaT=(Tep-Tgw[1])/2
		print("deltaT = ", deltaT)
	time.sleep(10)

		if ((deltaT<50) and (deltaT>-50) and (Tgw[1]> 1576500 and Tgw[1]<1576600)):
			dpps= int((debut1 -Tgw[0])*10/1.5)-2000
			print(dpps)
			#os.popen(f"./pwm_cli.py  --oneshot {dpps}")
			changePPS(dpps)
			
			kp=2**32/25000000
			deltaF= int(deltaF0+ (deltaT- deltaT0)*kp +kp*deltaT/2) 
			print("deltaF = ", deltaF)
			deltaF0 = deltaF
			deltaT0 = deltaT
			#deltaF=int(deltaT/1.75)
			print("deltaF= ", deltaF)
			#os.popen(f"cd /sys/bus/iio/devices/iio\:device0/ && echo {deltaF} out_altvoltage0_frequency0")
			deltaF=deltaF+int(np.fromfile('freq_AD.txt', dtype=int))
			print("deltaF = ", deltaF)
			ad.asservissement(deltaF)
			deltaF=np.int64(deltaF)
			deltaF.tofile("freq_AD.txt")
			
			time.sleep(60)
		else:
			print("erreur du traitement")
			print("fin du cycle :)")
			time.sleep(10)

	else:
		print("Tgw = ", Tgw)
		time.sleep(10)