import logging
import argparse
from opcuaserv import opcuaserv
from opcuaserv import i2client
from opcuaserv import yamlreader
#from opcuaserv import pypcc2
from i2cserv import i2cthread
import threading
import time
import sys
import signal
from yamlconfig import Find;
import yamlconfig as yc

logging.basicConfig(level="WARNING",format='%(asctime)s [%(levelname)-8s,%(filename)-20s:%(lineno)-3d] %(message)s')

RunTimer=True;
#def signal_handler(sig, frame):
#    logging.warn('Stop signal received!')
#    global RunTimer; 
#    RunTimer=False
#signal.signal(signal.SIGINT, signal_handler)

#logging.info("Start I2C processes")   
#threads=[]
#I2Cclients=[]
name='RCUL'
#RCU_I2C=i2client.i2client(name=name)

conf=yc.yamlconfig(name)
conf.linkdevices()
conf.loaddrivers()
conf.linkdrivers()

def GetVal(name,N=1):
 varid=conf.getvarid(name);
 var1=conf.getvars()[varid]
 drv=var1.get('drivercls');
 mask=[True]*N+[False]*((32-1)*N);
 data=drv.OPCUAReadVariable(varid,var1,mask)
 return data[0].data,var1

data,var1=GetVal('RCU_ID');
print("ID=",[hex(d) for d in data[:4]]);

if False:
 data,var1=GetVal('RCU_temperature');
 D=((data[0]*256+data[1])*256+data[2])*var1.get('scale',1.)
 print("Temp=",D,"K")
 if (D<290) or (D>350): exit()

 data,var1=GetVal('RCU_3V3');
 D=((data[0]*256+data[1])*256+data[2])*var1.get('scale',1.)
 print("3V3=",D,"V")
 if (D<3.2) or (D>3.4): exit()
#print("data=",[hex(d) for d in data[:3]]);


def SetRegister(regname,value):
  methodid=conf.getmethodid("RCU_on");
  var1=conf.getmethod(methodid)
  drv=var1.get('drivercls');
  v1=conf.getdevreg(regname)
  drv2=v1.get('drivercls')
  mask=[True]+[False]*31;
  if drv:  drv.Setdevreg(v1,value,mask)
  elif drv2: drv2.Setdevreg(v1,value,mask)
  else: logging.warn("Driver not specified for instruction"+key)

if False:
 SetRegister("IO1.GPIO1",[0])
 SetRegister("IO1.GPIO2",[0x80]) #Analog power on
 SetRegister("IO2.GPIO1",[0x40]) #Digital power on
 SetRegister("IO2.GPIO2",[0])

 SetRegister("IO1.CONF1",[0])
 SetRegister("IO1.CONF2",[0])
 SetRegister("IO2.CONF1",[0x80]) #Pgood on P07
 SetRegister("IO2.CONF2",[0])

 print("IO expander status:");
 data,var=GetVal('RCU_IO1_GPIO1');print("IO1_1",hex(data[0]))
 data,var=GetVal('RCU_IO1_GPIO2');print("IO1_2",hex(data[0]))
 data,var=GetVal('RCU_IO2_GPIO1');print("IO2_1",hex(data[0]))
 data,var=GetVal('RCU_IO2_GPIO2');print("IO2_2",hex(data[0]))

if False:
 data,var1=GetVal('RCU_1V8');
 D=((data[0]*256+data[1])*256+data[2])*var1.get('scale',1.)
 print("1V8=",D,"")

 data,var1=GetVal('RCU_2V5');
 D=((data[0]*256+data[1])*256+data[2])*var1.get('scale',1.)
 print("2V5=",D,"")

if False:
 data,var1=GetVal('ANT_Vin');
 D0=((data[0]*256+data[1])*256+data[2])*var1.get('scale',1.)
 D1=((data[3]*256+data[4])*256+data[5])*var1.get('scale',1.)
 D2=((data[6]*256+data[7])*256+data[8])*var1.get('scale',1.)
 print("Vant_in=",D0,D1,D2)

# SetRegister("IO1.GPIO1",[0xC0]) #Antenna power on
# SetRegister("IO1.GPIO2",[0xC0]) #Analog power on

 data,var1=GetVal('ANT_Vout');
 D0=((data[0]*256+data[1])*256+data[2])*var1.get('scale',1.)
 D1=((data[3]*256+data[4])*256+data[5])*var1.get('scale',1.)
 D2=((data[6]*256+data[7])*256+data[8])*var1.get('scale',1.)
 print("Vant_out=",D0,D1,D2)

 data,var1=GetVal('ANT_I');
 D0=((data[0]*256+data[1])*256+data[2])*var1.get('scale',1.)
 D1=((data[3]*256+data[4])*256+data[5])*var1.get('scale',1.)
 D2=((data[6]*256+data[7])*256+data[8])*var1.get('scale',1.)
 print("Iant=",D0,D1,D2)

if False:
 SetRegister("IO3.GPIO1",[0x15]) #ADC_SDIO=high, clk=low,  DTH_EN=low
 SetRegister("IO3.GPIO2",[0x47]) #ADC SC=high, DTH_SDA=high
 SetRegister("IO3.CONF1",[0]) #All output
 SetRegister("IO3.CONF2",[0])
 data,var=GetVal('RCU_IO3_GPIO1');print("IO3_1",hex(data[0]))
 data,var=GetVal('RCU_IO3_GPIO2');print("IO3_2",hex(data[0]))

if False:
 #Test reading register from ADCs
 data,var=GetVal('RCU_ADC_JESD');print("ADC JESD",[hex(d) for d in data[:3]])

 #Test writing ADC register
 SetRegister("ADC1.SYNC_control",[1])
 SetRegister("ADC1.Update",[1])
 data,var=GetVal('RCU_ADC_sync');print("ADC sync",[hex(d) for d in data[:3]])

#Need to update I2c bitbang....
if True:
 SetRegister("IO3.GPIO1",[0x15]) #ADC_SDIO=high, clk=low,  DTH_SDN=low
 SetRegister("IO3.GPIO2",[0x3F]) #ADC SC=high, DTH_SDA=low, DTH_CLK=high
 SetRegister("IO3.CONF1",[0]) #All output
 SetRegister("IO3.CONF2",[0x40]) #DTH_SDA=input

# data,var=GetVal('RCU_IO3_GPIO1');print("IO3_1",hex(data[0]))
# data,var=GetVal('RCU_IO3_GPIO2');print("IO3_2",hex(data[0]))
 f=int(150e6)
 print("Frequency set=",f)
 d=[0]*4;
 for i in range(4):
   d[3-i]=f%256;f//=256
# print([hex(h) for h in d])
 SetRegister("DTH1.Freq",d) #DTH_SDA=input

 data,var1=GetVal("RCU_DTH_freq")
# data,var1=GetVal("RCU_DTH_Rev")
 f=0;
 for i in range(4):
  f=f*256+data[i];
 print("Frequency read back=",f)
# print([hex(h) for h in data[:30]])


#print(data)
#scale=float(scale)
#data2=[(d*scale) for d in data2]

#print("ID=",[hex(d) for d in data[:4]]);