# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/tiptilt_ctrl/bcu/util/decimate.py 3457 2020-09-14 10:42:41Z depa $
from scipy import signal
import numpy as np

""" INPUT from embedding C++ code
dataHistory
downFactor
timeHistoryLen
OUTPUT to embedding C++ code
waveArrayFirst50
waveArrayLast50
dataHistoryFinal
"""

FIRCoeff = [1]

# first 50 elements of each axis  multiplied to a sinusoidal wave as:
waveArrayFirst50 = (1.-np.cos(np.linspace(0, np.pi, num=50, dtype=np.float32)))/2.
waveArrayLast50 = (1.-np.cos(np.linspace(0, np.pi, num=50, dtype=np.float32)))/2.

if ("vecXHistory" in globals()) and ("vecYHistory" in globals()):
    # decimation by 3
    dataInterpX = signal.upfirdn(FIRCoeff, vecXHistory, up=1, down=downFactor)
    dataInterpY = signal.upfirdn(FIRCoeff, vecYHistory, up=1, down=downFactor)

    # report to mean value
    dataInterpXDev = dataInterpX - np.mean(dataInterpX)
    dataInterpYDev = dataInterpY - np.mean(dataInterpY)

    dataInterpXDev[0:50] = np.multiply(dataInterpXDev[0:50], waveArrayFirst50, dtype=np.float32)
    dataInterpXDev[-50:] = np.multiply(dataInterpXDev[-50:], waveArrayLast50, dtype=np.float32)

    dataInterpYDev[0:50] = np.multiply(dataInterpYDev[0:50], waveArrayFirst50, dtype=np.float32)
    dataInterpYDev[-50:] = np.multiply(dataInterpYDev[-50:], waveArrayLast50, dtype=np.float32)


    # conversion to float32 for embedding C++ code
    dataInterpX2 = dataInterpX.astype(np.float32, order='C')
    dataInterpY2 = dataInterpY.astype(np.float32, order='C')

    # replicating arrays to have size equal or bigger then timeHistoryLen
    repTimes = np.ceil(timeHistoryLen/2. * 1/dataInterpX2.size)

    dataInterpX2 = np.tile(dataInterpX2,repTimes)
    dataInterpY2 = np.tile(dataInterpY2,repTimes)

else:
    if ("dataHistory" in globals()):
        dataHistoryInterp = signal.upfirdn(FIRCoeff, dataArrayDev, up=1, down=downFactor, axis=0)
        dataHistoryInterpDev = dataHistoryInterp - np.meam(dataHistoryInterp, axis=0)

        waveArrayFirst50 = np.reshape(waveArrayFirst50, (50,1), order='F')
        waveArrayFirst50 = np.hstack((waveArrayFirst50, waveArrayFirst50))

        waveArrayLast50 = np.reshape(waveArrayLast50, (50,1), order='F')
        waveArrayLast50 = np.hstack((waveArrayLast50, waveArrayLast50))

        dataHistoryInterpDev[0:][0:50] = np.multiply(dataHistoryInterpDev[0:][0:50], waveArrayFirst50, dtype=np.float32)
        dataHistoryInterpDev[0:][-50:] = np.multiply(dataHistoryInterpDev[0:][-50:], waveArrayLast50, dtype=np.float32)

        repTimes = np.ceil(timeHistoryLen/2. * 1/dataHistoryInterpDev.shape[0])
        dataHistoryFinal = np.tile(dataHistoryInterpDev, repTimes)

        # np.tile outputs the input array repeated repTimes on the colums axis
        # reshape is necessary to report the array to a 2 column array
        dataHistoryFinal = np.reshape(dataHistoryFinal, (dataHistoryInterpDev.shape[0]*nRep,dataHistoryInterpDev.shape[1]), order='F')
# these two vectors has to be resized to timeHistoryLen in C++ embedding code
