#!   /usr/bin/env   python
#    coding: utf8

# Copyright CERN, 2011
# Author: Matthieu Cattin <matthieu.cattin@cern.ch>
# Licence: GPL v2 or later.
# Website: http://www.ohwr.org
# Last modifications: 1/6/2012

# Import system modules
import sys
import time
import os

# Add common modules and libraries location to path
sys.path.append('../../../')
sys.path.append('../../../gnurabbit/python/')
sys.path.append('../../../common/')

# Import common modules
from ptsexcept import *
import rr

# Import specific modules
from numpy import *
from pylab import *
from ctypes import *
from fmc_adc_spec import *
from fmc_adc import *
from calibr_box import *
from cp210x_eeprom import *
import find_usb_tty
from PAGE.Agilent33250A import *
from PAGE.SineWaveform import *


"""
test19: Calibration

Note: Requires test00.py to run first to load the firmware!
"""

NB_CHANNELS = 4

AWG_SET_SLEEP = 0.3
SSR_SET_SLEEP = 0.005
BOX_SET_SLEEP = 0.01
DAC_SET_SLEEP = 0.01

ACQ_TIMEOUT = 10

PRE_TRIG_SAMPLES = 100
POST_TRIG_SAMPLES = 100000
NB_SHOTS = 1

ACQ_LENGTH = 50000 # in samples

ADC_NBITS = 16 # ADC chip is 14 bits, but shifted to 16 bits in the firmware
DAC_NBITS = 16
DAC_FS = 10 # DAC full scale range is 10V

# Full scale volatge values for input ranges
ADC_FS = {'10V':10.0, '1V':1.0, '100mV':0.1}

RANGES = ['10V', '1V', '100mV']

# Number of repeatition of the measurement, then the results are averaged
REPEAT = 2

# ADC LSB values for each input range
ADC_LSB = {'10V':10.0/2**14, '1V':1.0/2**14, '100mV':0.1/2**14}


# Disconnect inputs of all channels
def disconnect_channels(fmc):
    for i in range(1,NB_CHANNELS+1):
        fmc.set_ssr(i, 0x00)
        time.sleep(SSR_SET_SLEEP)

# Initialise acquisition
def fmc_adc_init(spec, fmc):
    print('Initialise FMC board\n')
    # Reset offset DACs
    fmc.dc_offset_reset()
    # Make sure all switches are OFF
    disconnect_channels(fmc)
    # Reset offset DACs
    fmc.dc_offset_reset()
    # Set ADC offset correction to 0 and gain correction to 1
    for ch in range(1, NB_CHANNELS+1):
        fmc.set_adc_gain_offset_corr(ch, 0x8000, 0x0)
    # Set trigger
    fmc.set_soft_trig()
    # Set acquisition
    fmc.set_pre_trig_samples(PRE_TRIG_SAMPLES)
    fmc.set_post_trig_samples(POST_TRIG_SAMPLES)
    fmc.set_shots(NB_SHOTS)
    # Print configuration
    #fmc.print_adc_core_config()

# Converts two's complement hex to signed
def hex2signed(value):
    if(value & 0x8000):
        return -((~value & 0xFFFF) + 1)
    else:
        return value

# Converts signed to two's complement hex
def signed2hex(value):
    if value < 0:
        return (((abs(value) ^ 0xffff) + 1) & 0xffff)
    else:
        return value

# Converts digital value to volts
def digital2volt(value, full_scale, nb_bit):
    return float(value) * float(full_scale)/2**nb_bit

# Converts volts to digital value with half full range offset
def volt2digital(value, full_scale, nb_bit):
    digital = (value + full_scale/2) * 2**nb_bit/full_scale
    if(digital > 2**nb_bit - 1):
        digital = 2**nb_bit - 1
    if(digital < 0):
        digital = 0
    #print('volt2digital: %2.9f > %2.9f')%(value,digital)
    return int(digital)

# Converts volts to digital value
def volt2digital_without_offset(value, full_scale, nb_bit):
    if(value > (2**nb_bit)/2 - 1):
        value = (2**nb_bit)/2 - 1
    if(value < -((2**nb_bit)/2)):
        value = -((2**nb_bit)/2)
    digital = (value) * 2**nb_bit/full_scale
    #print('volt2digital: %2.9f > %2.9f')%(value,digital)
    return int(digital)

# Plot data from all channels
def plot_all(data, mean, ylimit):
    sample = arange(len(data)/4)
    clf()
    plot(sample, data[0::4], 'b', label='Channel 1')
    plot(sample, data[1::4], 'g', label='Channel 2')
    plot(sample, data[2::4], 'c', label='Channel 3')
    plot(sample, data[3::4], 'm', label='Channel 4')
    plot(sample, [mean[0]]*len(sample), 'r')
    plot(sample, [mean[1]]*len(sample), 'r')
    plot(sample, [mean[2]]*len(sample), 'r')
    plot(sample, [mean[3]]*len(sample), 'r')
    ylim(-ylimit-(ylimit/10.0), ylimit+(ylimit/10.0))
    grid(which='both')
    legend()
    draw()
    show()
    return 0

# Plot data from all channels
def plot_channel(ch, data, mean, ylimit):
    print("nb of samples to plot: %d"%len(data))
    sample = arange(len(data))
    lbl = "Channel %d"%ch
    clf()
    plot(sample, data, 'b', label=lbl)
    plot(sample, [mean]*len(sample), 'r')
    ylim(-ylimit-(ylimit/10.0), ylimit+(ylimit/10.0))
    grid(which='both')
    legend()
    draw()
    show()
    return 0

# Make an acquisition of a channel
def acq_channel(carrier, fmc, ch, adc_fs, adc_nbits=16, pause=0.01):
    # Make sure no acquisition is running
    fmc.stop_acq()
    # Start acquisition
    fmc.start_acq()
    #time.sleep(pause)
    # Trigger
    fmc.sw_trig()
    # Wait end of acquisition
    timeout = 0
    time.sleep(0.001)
    while('IDLE' != fmc.get_acq_fsm_state()):
        time.sleep(0.01)
        timeout += 1
        if(ACQ_TIMEOUT < timeout):
            print "Acquisition timeout. Missing trigger?."
            print "Acq FSM state: %s"%fmc.get_acq_fsm_state()
            return 1
    # Retrieve data trough DMA
    trig_pos = fmc.get_trig_pos()
    # Enable "DMA done" iinterrupt
    carrier.set_irq_en_mask(0x1)
    # Read ACQ_LENGTH samples after the trigger for all channels
    channels_data = carrier.get_data((trig_pos<<3), ACQ_LENGTH*8)
    # Disable "DMA done" iinterrupt
    carrier.set_irq_en_mask(0x0)
    channels_data = [hex2signed(item) for item in channels_data]
    channels_data = [digital2volt(item,adc_fs,adc_nbits) for item in channels_data]
    #plot_all(channels_data, [mean(channels_data[0::4]),mean(channels_data[1::4]),mean(channels_data[2::4]),mean(channels_data[3::4])], 0.1)
    #raw_input("press enter")
    channel_data = channels_data[ch-1::4]
    return channel_data

# Set channel offset with the DAC
def set_offset_dac(fmc, channel, offset_volt, dac_fs=10.0, dac_nbits=16, dac_corr_flag=False):
    dac_v = offset_volt
    dac_d = volt2digital(dac_v,dac_fs,dac_nbits)
    #print('DAC value: 0x%X (%fV)')%(dac_d, dac_v)
    if(True == dac_corr_flag):
        fmc.set_dc_offset_corrected(channel,dac_d)
    else:
        fmc.set_dc_offset(channel,dac_d)
    time.sleep(DAC_SET_SLEEP)

# Make a measurement, where the input and offset DAC values are set to a known value
def make_meas(carrier, fmc, box, dac_value, in_value, in_range, adc_fs, repeat):
    ch_meas = []
    ch_diff = []
    for ch in range(1,NB_CHANNELS+1):
        # Set offset DAC
        set_offset_dac(fmc, ch, dac_value)
        # Configure calibration box
        if in_value == 0.0:
            # Configure analogue input, ground input
            fmc.set_input_range(ch, ('CAL_'+in_range))
            time.sleep(SSR_SET_SLEEP)
            # Connect AWG to current channel
            box.select_output_ch(ch)
            time.sleep(BOX_SET_SLEEP)
        else:
            # Configure analogue input range
            fmc.set_input_range(ch, in_range)
            time.sleep(SSR_SET_SLEEP)
            # Connect Vref for selected input range to current channel
            # Note that channel in parameter is the one connected to AWG
            if ch == 1:
                awg_ch = 4
            else:
                awg_ch = ch-1
            box.select_output_ch(awg_ch, in_range)
            time.sleep(BOX_SET_SLEEP)
        # Perform an acquisition
        meas = []
        for i in range(repeat):
            data = acq_channel(carrier, fmc, ch, adc_fs)
            #plot_channel(ch, data, mean(data), adc_fs)
            #raw_input("press enter")
            meas.append(mean(data))
        # Calculate mean and delta of acquisitions
        ch_meas.append(mean(meas))
        ch_diff.append(max(meas)-min(meas))
    return ch_meas, ch_diff


# Calculates ADC + input stage gain
def calc_ga(Vm1, Vm2, Vref1):
    return ((Vm2-Vm1)/Vref1)

# Calculates ADC + input stage offset
def calc_oa(Vm2, Vm3, Vm4):
    return (Vm2 + Vm3 - Vm4)

# Calculates DAC gain
def calc_gd(Vm1, Vm2, Vm3, Vref1, Vref2):
    return ((Vref1*(Vm3-Vm1))/(Vref2*(Vm1-Vm2)))

# Calculates DAC offset
def calc_od(Vm1, Vm2, Vm3, Vm4, Vref1):
    return ((Vref1*(Vm1-Vm2-Vm3+Vm4))/(Vm1-Vm2))


def main (default_directory = '.'):

    # Constants declaration
    TEST_NB = 19
    EXPECTED_BITSTREAM_TYPE = 0x1

    # Calibration box vendor and product IDs
    BOX_USB_VENDOR_ID = 0x10c4 # Cygnal Integrated Products, Inc.
    BOX_USB_PRODUCT_ID = 0xea60 # CP210x Composite Device
    # Agilent AWG serial access vendor and product IDs
    AWG_USB_VENDOR_ID = 0x0403 # Future Technology Devices International, Ltd
    AWG_USB_PRODUCT_ID = 0x6001 # FT232 USB-Serial (UART) IC
    AWG_BAUD = 57600

    CALIBR_FILENAME = "calibration_data.txt"
    CALIBR_FILENAME = os.path.join(default_directory, CALIBR_FILENAME)
    RAW_CALIBR_FILENAME = "calibration_raw_data.txt"
    RAW_CALIBR_FILENAME = os.path.join(default_directory, RAW_CALIBR_FILENAME)
    MEAS_FILENAME = "calibration_meas.txt"
    MEAS_FILENAME = os.path.join(default_directory, MEAS_FILENAME)


    start_test_time = time.time()
    print "================================================================================"
    print "Test%02d start\n" % TEST_NB

    # SPEC object declaration
    print "Loading hardware access library and opening device.\n"
    spec = rr.Gennum()

    # Carrier object declaration (SPEC board specific part)
    # Used to check that the firmware is loaded.
    try:
        carrier = CFmcAdc100mSpec(spec, EXPECTED_BITSTREAM_TYPE)
    except FmcAdc100mSpecOperationError as e:
        raise PtsCritical("Carrier init failed, test stopped: %s" % e)

    # Mezzanine object declaration (FmcAdc100m14b4cha board specific part)
    try:
        fmc = CFmcAdc100m(spec)
    except FmcAdc100mOperationError as e:
        raise PtsCritical("Mezzanine init failed, test stopped: %s" % e)


    try:
        # Others objects declaration
        usb_tty = find_usb_tty.CttyUSB()
        awg_tty = usb_tty.find_usb_tty(AWG_USB_VENDOR_ID, AWG_USB_PRODUCT_ID)
        box_tty = usb_tty.find_usb_tty(BOX_USB_VENDOR_ID, BOX_USB_PRODUCT_ID)
        gen = Agilent33250A(device=awg_tty[0], bauds=AWG_BAUD)
        sine = SineWaveform()
        box = CCalibr_box(box_tty[0])
        box_eeprom = CCP210x_Eeprom("%X"%BOX_USB_VENDOR_ID, "%X"%BOX_USB_PRODUCT_ID)

        # Initialise fmc adc
        fmc_adc_init(spec, fmc)

        # Connect to AWG
        gen.connect()

        # Switch AWG output and sync OFF
        gen.output = False
        gen.sync = False

        # Measure FMC and carrier temperature
        print('SPEC temperature: %3.3f°C') % carrier.get_temp()
        print('FMC temperature : %3.3f°C') % fmc.get_temp()

        # Open all switches, reset offset DAC to mid-scale (0V)
        for channel in range(1,NB_CHANNELS+1):
            fmc.set_input_range(channel, 'OPEN')
            fmc.set_input_term(channel, 'OFF')
        fmc.dc_offset_reset()

        # Get calibration data from calibration box
        box_calibr_data = box_eeprom.get_calibr_data()
        print "\nCalibration box reference voltages:"
        for item in box_calibr_data.iteritems():
            print "%5s: %1.5fV" % (item[0], float(item[1]))

        # Offset DAC reference voltage
        offset_dac_data = {'10V':4.096, '1V':0.4096, '100mV':0.04096}
        print "\nOffset DAC reference voltages:"
        for item in offset_dac_data.iteritems():
            print "%5s: %1.5fV" % (item[0], float(item[1]))

        # Dict to store ADC calibration data
        adc_corr_data = {'10V':{'offset':[],'gain':[],'temp':0},
                         '1V':{'offset':[],'gain':[],'temp':0},
                         '100mV':{'offset':[],'gain':[],'temp':0}}

        # Dict to store DAC calibration data
        dac_corr_data = {'10V':{'offset':[],'gain':[],'temp':0},
                         '1V':{'offset':[],'gain':[],'temp':0},
                         '100mV':{'offset':[],'gain':[],'temp':0}}


        ############################################################################
        # Calibration

        error = 0
        f_out = open(RAW_CALIBR_FILENAME, 'w')
        for IN_RANGE in RANGES:

            print "\n--------------------------------------------------------------------------------"
            print "%s range calibration\n----------------------------------\n"%(IN_RANGE)

            for ch in range(1,NB_CHANNELS+1):
                print "Channel %d ADC gain correction register  : 0x%04X" % (ch, fmc.get_adc_gain_corr(ch))
                print "Channel %d ADC offset correction register: 0x%04X" % (ch, fmc.get_adc_offset_corr(ch))

            print ""

            print "SPEC temperature: %3.3f°C" % carrier.get_temp()
            adc_corr_data[IN_RANGE]['temp'] = fmc.get_temp()
            print "FMC temperature : %3.3f°C" % adc_corr_data[IN_RANGE]['temp']

            print "\nADC LSB:%1.9f"%(ADC_LSB[IN_RANGE])

            # Reference voltage for calibration
            vref_in  = float(box_calibr_data[IN_RANGE]) # from box cailbration data in CP2103 EEPROM
            vref_dac = offset_dac_data[IN_RANGE]  # reference voltage to set offset DAC

            # Input and offset DAC volatges for the four measurements
            v_in  = [0.0, vref_in, 0.0,      vref_in]
            v_dac = [0.0, 0.0,     vref_dac, vref_dac]

            #-----------------------------------------------------------------------
            # Make the measurements
            #-----------------------------------------------------------------------
            v_meas = []
            d_meas = []
            error = 0
            for i in range(len(v_in)):
                print("\nMeasurement %d: channel input = %2.8fV, offset DAC = %2.8fV"%((i+1), v_in[i], v_dac[i]))
                mean, diff = make_meas(carrier, fmc, box, v_dac[i], v_in[i], IN_RANGE, ADC_FS[IN_RANGE], REPEAT)
                v_meas.append(mean)
                d_meas.append(diff)
                for ch in range(1,NB_CHANNELS+1):
                    print("Channel %d: v_meas=%02.9fV delta=%1.9fV"%(ch, v_meas[i][ch-1], d_meas[i][ch-1]))

                    meas = v_meas[i][ch-1]
                    expect = (v_in[i] - v_dac[i])
                    tol = 0.05 * ADC_FS[IN_RANGE]
                    print("   Check voltage level (tolerance=+/-%1.4fV):" % tol)
                    if abs(meas - expect) > tol:
                        print("      ERROR measure:%1.9fV expect:+/-%1.9fV                      ###"%(abs(meas - expect), tol))
                        error += 1
                    else:
                        print("      OK")

                    meas = d_meas[i][ch-1]
                    expect = (2*ADC_LSB[IN_RANGE])
                    print("   Check voltage stability (tolerance=+/-%1.6fV):" % expect)
                    if abs(meas) > expect:
                        print("      ERROR measure:%1.9fV expect:+/-%1.9fV                      ###"%(meas, expect))
                        error += 1
                    else:
                        print("      OK")


            #-----------------------------------------------------------------------
            # Calculate gain and offset parameters
            # ga = ADC + input stage gain
            # oa = ADC + input stage offset
            # gd = DAC gain
            # od = DAC offset
            #-----------------------------------------------------------------------
            print "\n%s range correction parameters\n----------------------------------"%(IN_RANGE)

            ga = []
            for ch in range(1,NB_CHANNELS+1):
                ga.append(calc_ga(v_meas[0][ch-1], v_meas[1][ch-1], vref_in))
                print "Channel %d ADC gain coeff: %02.9f"%(ch, ga[ch-1])
                f_out.write('%.30f\n'%(ga[ch-1]))

            oa = []
            for ch in range(1,NB_CHANNELS+1):
                oa.append(calc_oa(v_meas[1][ch-1], v_meas[2][ch-1], v_meas[3][ch-1]))
                print "Channel %d ADC offset    : %02.9f"%(ch, oa[ch-1])
                f_out.write('%.30f\n'%(oa[ch-1]))

            gd = []
            for ch in range(1,NB_CHANNELS+1):
                gd.append(calc_gd(v_meas[0][ch-1], v_meas[1][ch-1], v_meas[2][ch-1], vref_in, vref_dac))
                print "Channel %d DAC gain coeff: %02.9f"%(ch, gd[ch-1])
                f_out.write('%.30f\n'%(gd[ch-1]))

            od = []
            for ch in range(1,NB_CHANNELS+1):
                od.append(calc_od(v_meas[0][ch-1], v_meas[1][ch-1], v_meas[2][ch-1], v_meas[3][ch-1], vref_in))
                print "Channel %d DAC offset    : %02.9f"%(ch, od[ch-1])
                f_out.write('%.30f\n'%(od[ch-1]))

            #-----------------------------------------------------------------------
            # Calculate ADC correction register values
            #-----------------------------------------------------------------------
            # 0x8000 corresponds to 1.0
            # Gain correction register is written with 1/gain * 1.0
            adc_corr_data[IN_RANGE]['gain'] = [int(round((1/item)*0x8000)) for item in ga]

            # oa is in volts and has to be converted to digital raw value,
            # to be written in offset correction register
            adc_corr_data[IN_RANGE]['offset'] = [-(volt2digital_without_offset(item,ADC_FS[IN_RANGE],ADC_NBITS)) for item in oa]

            #-----------------------------------------------------------------------
            # Calculate DAC correction register values
            #-----------------------------------------------------------------------
            # 0x8000 corresponds to 1.0
            # Gain correction register is written with 1/gain * 1.0
            dac_corr_data[IN_RANGE]['gain'] = [int(round((1/item)*0x8000)) for item in gd]

            # oa is in volts and has to be converted to digital raw value,
            # to be written in offset correction register
            dac_corr_data[IN_RANGE]['offset'] = [-(volt2digital_without_offset(item,DAC_FS,DAC_NBITS)) for item in od]

        f_out.close()

        ############################################################################


        ############################################################################
        # Write correction data to file

        out_filename = CALIBR_FILENAME
        f_out = open(out_filename, 'w')

        print "\n--------------------------------------------------------------------------------"
        print "\nWrite ADC correction data to %s"%(out_filename)
        for IN_RANGE in RANGES:
            #print "%s range"%(IN_RANGE)
            for item in adc_corr_data[IN_RANGE]['offset']:
                print "0x%04X" % (signed2hex(item))
                f_out.write('0x%.4X\n'%(signed2hex(item)))
            for item in adc_corr_data[IN_RANGE]['gain']:
                print "0x%04X" % (signed2hex(item))
                f_out.write('0x%.4X\n'%(signed2hex(item)))
            temp = adc_corr_data[IN_RANGE]['temp']*100
            print "0x%04X, %d" % (temp, temp)
            f_out.write('0x%.4X\n'%(temp))

        print "\nWrite DAC correction data to %s"%(out_filename)
        for IN_RANGE in RANGES:
            #print "%s range"%(IN_RANGE)
            for item in dac_corr_data[IN_RANGE]['offset']:
                print "0x%04X" % (signed2hex(item))
                f_out.write('0x%.4X\n'%(signed2hex(item)))
            for item in dac_corr_data[IN_RANGE]['gain']:
                print "0x%04X" % (signed2hex(item))
                f_out.write('0x%.4X\n'%(signed2hex(item)))
            temp = adc_corr_data[IN_RANGE]['temp']*100
            print "0x%04X, %d" % (temp, temp)
            f_out.write('0x%.4X\n'%(temp))
        f_out.close()
        ############################################################################

        ############################################################################
        # Write calibration measurment to file

        out_filename = MEAS_FILENAME
        f_out = open(out_filename, 'w')

        print "\n--------------------------------------------------------------------------------"
        print "\nWrite calibration measurement to %s"%(out_filename)
        print v_meas
        for meas in v_meas:
            for ch in meas:
                print "%.30f" % ch
                f_out.write('%.30f\n'%ch)
        f_out.close()
        ############################################################################

        # Open all switches, reset offset DAC to mid-scale (0V)
        for channel in range(1,NB_CHANNELS+1):
            fmc.set_input_range(channel, 'OPEN')
            fmc.set_input_term(channel, 'OFF')
        fmc.dc_offset_reset()

        # Close AWG
        gen.close()


    except(FmcAdc100mSpecOperationError,FmcAdc100mOperationError,
           CalibrBoxOperationError, CP210xEepromOperationError) as e:
        raise PtsError("Test failed: %s" % e)


    print ""
    print "==> End of test%02d" % TEST_NB
    print "================================================================================"
    end_test_time = time.time()
    print "Test%02d elapsed time: %.2f seconds\n" % (TEST_NB, end_test_time-start_test_time)

    # Check if an error occured during calibration
    if(error != 0):
        raise PtsError('An error occured during calibration (unstable voltage), check log for details.')


if __name__ == '__main__' :
    main()
