#!/usr/bin/env python



# Copyright (c) 2020
#      Daniel O'Connor <darius@dons.net.au>.  All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
# 1. Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in the
#    documentation and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
# OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
# OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
# SUCH DAMAGE.
#

# Expected DB schema
# CREATE TABLE phasediff (
#   name TEXT,
#   time TIMESTAMP WITH TIME ZONE,
#   delta NUMERIC(15, 12),
#   cumulative NUMERIC(15, 12)
# );

import datetime
import matplotlib.pylab as pylab
import numpy
import psycopg2
import scipy.signal
import sqlite3
import sys
import time
# Should try this code instead: https://github.com/python-ivi/python-usbtmc
import usb488

def main():
    u = usb488.USB488Device()
    print('Found device')

    # See "TDS2000 Programmer.pdf"
    u.write('*IDN?')
    print(('IDN reports ' + u.read()))

    #dbh = sqlite3.connect('phasediff.db')
    dbh = psycopg2.connect('host=vm11 user=phasediff dbname=phasediff')
    test(u, dbh, 'delamerelts')

def test(u, dbh = None, name = None):
    if dbh != None:
        cur = dbh.cursor()

    u.write('DATA:ENC RIB')		# Big endian signed
    u.write('DATA:WIDTH 2')		# 2 bytes wide

    u.write('CH1:SCALE?')
    vscale1 = float(u.read(1).split()[1])
    print(('Channel 1 scale is %.2f volts/div' % (vscale1)))
    u.write('CH2:SCALE?')
    vscale2 = float(u.read(1).split()[1])
    print(('Channel 2 scale is %.2f volts/div' % (vscale2)))
    u.write('HOR:MAIN:SCALE?')
    hscale = float(u.read(1).split()[1])
    print(('Horizontal scale is %.5f nsec/div' % (hscale * 1e9)))
    # TEK2024B doesn't grok HOR:DIV? so hard code 10 (has 8 vertically)
    acqwindow = hscale * 10.0
    nomclockperiod = 1 / 10.7e6

    cumdiff = 0
    lastdiff = None
    while True:
        ary1, ary2 = acquire(u, vscale1, vscale2)

        sampletime = acqwindow / len(ary1)

        # Compute clock periods
        # XXX: this is pretty noisy so we use the nominal clock period above for unwrapping
        #period1 = getperiod(ary1) * sampletime
        #period2 = getperiod(ary2) * sampletime

        # Compute phased difference between the two
        d = getpdiff(ary1, ary2) * sampletime
        #d = getpdiffedge(ary1, ary2) * sampletime
        if lastdiff == None:
            # First time, init variables and start again
            # XXX: should get the most recent cumuulative diff from the DB really..
            lastdiff = d
            cumdiff = d
            continue

        # Difference between difference
        diff = d - lastdiff

        # Check if it wrapped
        if abs(diff) > nomclockperiod / 2:
            if d > nomclockperiod / 2:
                diff -= nomclockperiod
            else:
                diff += nomclockperiod

        # Accumulate deltas
        cumdiff += diff

        # Log / insert into DB
        now = datetime.datetime.now()
        print(("%s Cumulative: %.2f nsec Delta: %.2f nsec Diff: %.2f nsec" % (
            now.strftime('%Y/%m/%d %H:%M:%S.%f'),
            cumdiff * 1e9, d * 1e9, diff * 1e9)))
        if dbh != None:
            cur.execute('INSERT INTO phasediff(name, time, delta, cumulative) VALUES(%s, %s, %s, %s)', (name, now, d, cumdiff))
            dbh.commit()
        lastdiff = d

def acquire(u, vscale1, vscale2):
        u.write('ACQ:STATE 1')	# Do a single acquisition
        u.write('*OPC?')
        u.read(2.0)	# Wait for it to complete

        u.write('DAT:SOU CH1')	# Set the curve source to channel 1
        u.write('CURVE?')		# Ask for the curve data
        then = time.time()
        result = u.read(1.0)	# Takes the CRO a while for this
        now = time.time()
        #print('CURVE read took %f milliseconds' % ((now - then) * 1000.0))
        data1 = result[13:]		# Chop off the header (should verify this really..)
        ary1 = numpy.fromstring(data1, dtype = '>h')
        ary1 = ary1 / 32768.0 * vscale1 # Scale to volts

        u.write('DAT:SOU CH2')	# Set the curve source to channel 2
        u.write('CURVE?')		# Ask for the curve data
        then = time.time()
        result = u.read(1.0)	# Takes the CRO a while for this
        now = time.time()
        #print('CURVE read took %f milliseconds' % ((now - then) * 1000.0))
        data2 = result[13:]		# Chop off the header (should verify this really..)
        ary2 = numpy.fromstring(data2, dtype = '>h')
        ary2 = ary2 / 32768.0 * vscale2 # Scale to volts

        return ary1, ary2

def getpdiff(ary1, ary2):
    '''Return phase difference in samples between two signals by cross correlation'''

    # Rescale to 0-1
    ary1 = ary1 - ary1.min()
    ary1 = ary1 / ary1.max()
    ary2 = ary2 - ary2.min()
    ary2 = ary2 / ary2.max()

    # Cross correlate
    corr = scipy.signal.correlate(ary1, ary2)

    # Find peak
    amax = corr.argmax()

    return amax - (len(ary1) - 1)

def getpdiffedge(ary1, ary2):
    '''Return phase difference in samples between two signals by edge detection'''

    # Rescale to 0-1
    ary1 = ary1 - ary1.min()
    ary1 = ary1 / ary1.max()
    ary2 = ary2 - ary2.min()
    ary2 = ary2 / ary2.max()

    # Find rising edge of each
    ary1pos = numpy.argmax(ary1 > 0.2)
    ary2pos = numpy.argmax(ary2 > 0.2)

    return ary1pos - ary2pos

def getperiod(ary):
    '''Return period of signal in ary in samples'''
    # Compute auto correlation
    corr = scipy.signal.correlate(ary, ary)

    # Find peaks
    peaks, _ = scipy.signal.find_peaks(corr)

    # Find time differences between peaks
    deltapeak = (peaks[1:-1] - peaks[0:-2])

    # Return average of the middle few
    return deltapeak[2:-2].mean()

def trend(dbh, name):
    cur = dbh.cursor()
    cur2 = dbh.cursor()
    cur.execute('SELECT time, deltansec FROM phasediff WHERE name = %s', (name, ))

    _, lastdelta = cur.fetchone()

    count = 0
    cumdiff = 0
    for row in cur:
        time, delta = row
        diff = float(delta - lastdelta)
        if abs(diff) > 45e-9:
            if delta > 45e-9:
                diff -= 1.0 / 10.7e6
            else:
                diff += 1.0 / 10.7e6
        cumdiff += diff
        #print('Cumulative: %.5f nsec Delta: %.5f nsec Last: %.5f nsec Diff: %.5f nsec' % (
        #    float(cumdiff) * 1e9, float(delta) * 1e9, float(lastdelta) * 1e9, float(diff) * 1e9))
        lastdelta = delta
        cur2.execute('INSERT INTO cumdiff (name, time, cummdiff) VALUES (%s, %s, %s)',
                        ( name, time, cumdiff))
        count += 1
        #if count % 20 == 0:
        #    sys.stdin.readline()
        if count % 1000 == 0:
            dbh.commit()
            print(('Count %d: %s' % (count, str(time))))

if __name__ == '__main__':
    main()