package net.heartshield.filter;

import net.heartshield.signal.DVec;
import net.heartshield.signal.IVec;
import net.heartshield.signal.Signal;

/**
 * @author David Madl (git@abanbytes.eu)
 * @date 2017-05-17
 */
public class PcgBeatDetector extends RFilterBlock {
    private PcgEnvelopeFilter mPcgFilter;
    private Splitter mSplitter = new Splitter();
    private Splitter mSplitterPerc = new Splitter();
    private Diff mDiff = new Diff(); // could also just have been FIR({-1, 1})
    private FirFilterRunning mMean1;
    //private FirFilterRunning mMean2;
    private PercentileFilter mPercentileFilter;
    private DiracUpsampler mUpsampler;
    private RectUpsampler mRectUpsampler;

    public PcgBeatDetector(double frontFps) {

        mPcgFilter = new PcgEnvelopeFilter(frontFps);
        double fps = mPcgFilter.getFps();
        //mPcgFilter.connect(mSplitter);
        //mPcgFilter.connect(mDiff);

        double PERCENTILE_WIN_SECS = 4.0;
        double PERCENTILE = 85;
        // for plotting, 50.0 -- otherwise, use Double.MAX_VALUE
        // why is input not normalized? (range until >30)
        final double PERCENTILE_INIT = Double.MAX_VALUE; // ignore all beats until PercentileFilter has settled
        mPercentileFilter = new PercentileFilter((int) (PERCENTILE_WIN_SECS * fps), PERCENTILE_INIT, PERCENTILE);

        final int N1 = (int) (0.08 * fps);
        //final int N2 = (int) (0.2334 * fps);
        mMean1 = new FirFilterRunning(DVec.ones(N1));
        //mMean2 = new FirFilterRunning(DVec.ones(N2));

        //mDiff.connect(mMean1);
        mPcgFilter.connect(mMean1);
        //mMean1.connect(mMean2);
        mMean1.connect(mSplitter);

        //mPercentileFilter.connect(mPercentiles);
        mPercentileFilter.connect(mSplitterPerc);
        mSplitter.connect(mPercentileFilter); // must be before mBeatDetector, which uses mPercentiles.buf
        mSplitter.connect(mBeatDetector);

        mUpsampler = new DiracUpsampler(mPcgFilter.getDownsamplingRatio());
        mRectUpsampler = new RectUpsampler(mPcgFilter.getDownsamplingRatio());
        mBeatDetector.connect(mUpsampler);

        mSplitterPerc.connect(mPercentiles);
        mSplitterPerc.connect(mRectUpsampler);

        // dummy consumer, unless someone connects
        mRectUpsampler.connect(new IRFilterBlock() {
            @Override
            public void put(double[] x) {}
        });
    }

    static class FilterBuf implements IRFilterBlock {
        public double[] prevBuf = new double[1];
        public double[] buf = new double[1];

        @Override
        public void put(double[] x) {
            //System.out.println(" mPercentiles got x.length=" + x.length);
            //if(x.length > 0) {
                prevBuf = buf;
                buf = x;
            //}
        }
    }

    private FilterBuf mPercentiles = new FilterBuf();

    private RFilterBlock mBeatDetector = new RFilterBlock() {
        private double[] prevSlice = new double[1];

        // note: arrives in background thread
        @Override
        protected double[] batch(double[] x) {
            //System.out.println(" mBeatDetector got x.length=" + x.length);
            //double[] perc = mPercentileFilter.batch(x);

            final double DIFF_THRESHOLD = 0.35;
            double[] percentiles = DVec.stack(mPercentiles.prevBuf, mPercentiles.buf);
            double[] threshold = DVec.mul(percentiles, DIFF_THRESHOLD);

            double pval = mPercentiles.buf[0];
            System.out.println("PcgBeatDetector: percentile = " + pval);
            //Log.i(TAG, );

            // would be nice if we could receive an edge-padded sig
            // simulate by storing `prevSlice`
            double[] sig = DVec.stack(prevSlice, x);

            for(int i = 0; i < sig.length; i++)
                if(sig[i] < threshold[i])
                    sig[i] = 0;

            int[] sigBeats = Signal.heartbeat_localmax(sig);
            // shave off the last entry - it may appear to be spurious max
            int[] beats = IVec.get(sigBeats, sigBeats.length - x.length - 1, sigBeats.length - 1);

            prevSlice = x;

            return IVec.toDouble(beats);
        }
    };

    static class DiracUpsampler extends RFilterBlock {
        private int mRatio;

        DiracUpsampler(int ratio) {
            mRatio = ratio;
            this.mUpsamplingRatio = ratio;
        }

        @Override
        protected double[] batch(double[] x) {
            double[] ret = new double[x.length * mRatio];
            for(int i = 0; i < x.length; i++)
                ret[i*mRatio] = x[i];
            return ret;
        }
    }

    static class RectUpsampler extends RFilterBlock {
        private int mRatio;

        RectUpsampler(int ratio) {
            mRatio = ratio;
            this.mUpsamplingRatio = ratio;
        }

        @Override
        protected double[] batch(double[] x) {
            double[] ret = new double[x.length * mRatio];
            for(int i = 0; i < x.length; i++)
                for(int j = i * mRatio; j < (i+1) * mRatio; j++)
                    ret[j] = x[i];
            return ret;
        }
    }

    @Override
    protected double[] batch(double[] x) {
        throw new IllegalStateException();
    }

    @Override
    public void connect(IRFilterBlock consumer) {
        mUpsampler.connect(consumer);
    }

    public void connectPercentiles(IRFilterBlock consumer) {
        mRectUpsampler.connect(consumer);
    }

    @Override
    public void put(double[] x) {
        mPcgFilter.put(x);
    }
}
