package net.heartshield.control;

import android.os.Handler;
import android.os.Looper;
import android.util.Log;
import android.view.TextureView;

import net.heartshield.prevent.MeasureActivity;
import net.heartshield.sensors.DummyAudioSensor;
import net.heartshield.sensors.IAudioSensor;
import net.heartshield.sensors.ISensor;
import net.heartshield.sensors.ISensorFactory;
import net.heartshield.sensors.IntensityDetector;
import net.heartshield.data.Measurement;
import net.heartshield.data.MeasurementSeries;
import net.heartshield.sensors.SensorData;
import net.heartshield.signal.DVec;
import net.heartshield.signal.IVec;

import org.json.JSONException;
import org.json.JSONObject;

import java.util.ArrayList;
import java.util.List;

/**
 * Coordinates sensors and returns measurement data.
 *
 * @author David Madl (git@abanbytes.eu)
 * @date 2017-04-07
 */
public class MeasurementController implements IMeasurementController {
    private static final String TAG = "MeasurementController";

    private State mState = State.STOPPED;
    private Listener mListener;
    private Handler mHandler;
    private boolean isUnderTest;

    private IntensityDetector mIntensitySensor;
    private IAudioSensor mAudioSensor;
    private ISensor mAccelerationSensor;
    private Measurement mMeasurement;
    private long mStartTimeNanos; // start time in nanoTime timebase
    private long mCameraStartTimeNanos; // first camera frame in nanoTime timebase
    private long mStartTimeCamera; // start time in camera timebase
    private IDevice mDevice;

    public MeasurementController(Looper looper, IDevice device, ISensorFactory sensorFactory, TextureView cameraPreview) {
        mDevice = device;
        if(looper != null)
            mHandler = new Handler(looper);
        isUnderTest = (looper == null);

        /*
         * nice-to: benchmark the CPU and choose a resolution which we can process in the
         * time available between frames (30 ms)
         */
        mIntensitySensor = sensorFactory.getIntensityDetector(cameraPreview);

        /*
         * The event rate below controls the chunk size processed in the filter pipeline.
         * nice-to: decouple GUI updates from the chunk size being processed.
         * Instead, use a timer that fetches the required data from a FIFO buffer.
         */
        mAudioSensor = sensorFactory.getAudioSensor();

        mAccelerationSensor = sensorFactory.getAccelerationSensor();

        mIntensitySensor.setIntensityListener(mInternalIntensityListener);
        mAudioSensor.setListener(mAudioListener);
        mAccelerationSensor.setListener(mAccelerationListener);
    }

    public void setUnderTest(boolean underTest) { isUnderTest = underTest; }

    private double mQualityLockTime;
    /** preliminary "lock" time for UI */
    private long mLockTimeCameraNanos;
    private int mLockSampleIdx;
    private List<Double> mLockTimes;
    private List<Double> mUnlockTimes;

    private IntensityDetector.IntensityListener mInternalIntensityListener = new IntensityDetector.IntensityListener() {
        private int mI;
        private List<Integer> mCameraSettled = new ArrayList<>();

        private void clear() {
            mCameraSettled = new ArrayList<>();
        }

        @Override
        public void onState(IntensityDetector.State newState) {
            if(newState == IntensityDetector.State.LOCKED) {
                setState(State.SAMPLING);
            } else if(newState == IntensityDetector.State.FAILED) {
                logi("IntensityDetector: FAILED");
                mFailureReason = FailureReason.CAMERA_FAIL;
                stopSensors();
                clear();
                setState(State.FAILED);
            } else if(newState == IntensityDetector.State.STOPPED) {
                clear();
            } else if(newState == IntensityDetector.State.WAITING) {
                setState(State.WAITING);
            }
        }

        @Override
        public void onFrame(long timestamp, long cpuTimeNanos, double[] rgb, IntensityDetector.ImageSummary summary) {
            IntensityDetector.State state = mIntensitySensor.getState();

            if(state == IntensityDetector.State.WAITING) {
                if(mStartTimeCamera == 0) {
                    // timestamp timebase reference is NOT the same as nanoTime, only coincidental (and NOT on S7 and S7 edge)
                    mStartTimeCamera = timestamp;
                    mLockTimeCameraNanos = timestamp;
                    mLockSampleIdx = 0;
                    mCameraStartTimeNanos = cpuTimeNanos;
                    Log.i(TAG, "timestampSource=" + mIntensitySensor.getFeatures().timestampSource);
                }
            }
            final double timeSinceStart = ((double) (timestamp - mStartTimeCamera)) / 1e9;
            final double timeSinceLock = ((double) (timestamp - mLockTimeCameraNanos)) / 1e9;

            if(state == IntensityDetector.State.WAITING) {
                boolean ok = cameraCheck(rgb, summary);
                mCameraSettled.add(ok ? 1 : 0);
                //if(ok) {
                final double CAM_SETTLED_SECS = 1.0;
                int camSettledWin = (int) (CAM_SETTLED_SECS * mIntensitySensor.getFps());
                if(mCameraSettled.size() > camSettledWin) {
                    boolean recentOk = IVec.all(IVec.toArray(mCameraSettled.subList(mCameraSettled.size() - camSettledWin, mCameraSettled.size())));
                    if(recentOk) {
                        logi("stabilized, locking camera A3 at rgb=["+ lockStats.mr +"," + lockStats.mg + "," + lockStats.mb + "]");
                        mLockTimeCameraNanos = timestamp;
                        mLockSampleIdx = mIntensitySensor.getSamples().get(1).length;
                        mIntensitySensor.lock();
                        // this is vanilla camera data, and not influenced by qualityLockTime
                        mLockTimes.add(timeSinceStart);
                    }
                }
            } else if(state == IntensityDetector.State.LOCKED) {
                // TODO
                // TODO  these checks do not belong here. Move them, e.g. to MeasureActivity.
                // TODO

                // TODO: crap counter 2  (increment every time you visit here, and estimate the rewrite effort)


                // TODO is this efficient? do we care? (copying around whole buffer every frame)
                double[] r = mIntensitySensor.getSamples().get(1);
                // TODO define as 2*fps instead

                // S7 camera locking fix:
                // when locking camera on S7 edge, we get a large bounce in red amplitude
                // see https://gitlab.com/HeartShield/prevent-android/issues/99
                //
                // work around this behavior by only using the window after the locking has taken effect.
                //
                //int rangeStart = Math.min(mLockSampleIdx + 2, r.length); // DVec.get() may use zero-length
                int rangeStart = Math.min(mLockSampleIdx + 14, r.length); // DVec.get() may use zero-length

                double[] recent = DVec.get(r, Math.max(r.length - 60, rangeStart), r.length);

                double amplitudeSwing = (recent.length > 0) ? Math.abs(DVec.max(recent) - DVec.min(recent)) : 0.0;

                if(amplitudeSwing > 20.0 &&
                        (
                            mMode.equals(MeasurementMode.RISK) ||
                            (mMode.equals(MeasurementMode.ENDOTHELIAL_FUNCTION) && timeSinceLock < MeasureActivity.FRONT_TIME_ENDOTHELIAL_FUNCTION) ||
                            (mMode.equals(MeasurementMode.VITAL_CHECK) && timeSinceLock < MeasureActivity.FRONT_TIME_VITAL_CHECK)
                        )) {
                    logi("amplitudeSwing = " + amplitudeSwing + " too large, restarting measurement at timeSinceLock=" + timeSinceLock);
                    // TODO actually, reset time to here, but keep running - so server has full stats of how people measure
                    mCameraSettled.add(0);
                    mIntensitySensor.unlock();
                    mUnlockTimes.add(timeSinceStart);
                }

                if(mIntensitySensor.getDroppedCount() > 5*75) { // TODO FIXME Tom ugly hack suggested
                    mFailureReason = FailureReason.CAMERA_FRAMES_DROPPED;

                    try {
                        stop();
                    } catch (FramesDroppedException e) {}

                    setState(State.FAILED);
                    return;
                }
            }

            // TODO: check timestamp source from camera, and use SystemClock.elapsedRealtimeNanos()

            final double timeSinceQualityLock = timeSinceStart - mQualityLockTime;
            //Log.i(TAG, "postOnFrame() timestamp=" + timestamp + " " + " mStartTimeNanos=" + mStartTimeNanos + " mLockTimeCameraNanos=" + mLockTimeCameraNanos + " nanoTime=" + System.nanoTime() + " // mQualityLockTime=" + mQualityLockTime);
            //Log.i(TAG, "postOnFrame() timeSinceStart=" + timeSinceStart + " timeSinceLock=" + timeSinceLock + " timeSinceQualityLock=" + timeSinceQualityLock);
            postOnFrame(rgb, summary, timeSinceStart, Math.min(timeSinceQualityLock, timeSinceLock));
        }

        /**
         * check whether camera has settled into sane values (and user's finger is on the camera).
         */
        private boolean cameraCheck(double[] rgb, IntensityDetector.ImageSummary summary) {
            //if(summary == null) // may happen if we are not in State.WAITING
            //    return false;

            double v = DVec.mean(rgb);
            //double mr = DVec.mean(summary.meanRow[0]), mg = DVec.mean(summary.meanRow[1]), mb = DVec.mean(summary.meanRow[2]);
            double mr = rgb[0], mg = rgb[1], mb = rgb[2];

            // check if auto-exposure has found a good intensity
            boolean exposureOk = (30.0 < v) && (v < 230.0);
            //boolean redExposureOk = (30.0 < mr) && (mr < 230.0);
            boolean redExposureOk = (30.0 < mr);

            // redExposureOk: S7 edge bottoms out at 241. --> rgb=[241.08345052083334,0.0,0.0116015625]
            // ideas:
            // * take the exposure settings in the moment of lock, and pass them explicitly (avoids pipeline delay, both directions)
            // * send lock command twice (avoids request pipeline delay? [I have no idea why requests should be pipelined, but heck)
            // * adjust exposure settings after locking, to one step below the one where we locked

            // check for quite uniform image
            // TODO: make it less strict, not IVec.all() but some threshold // also, Sony XA has trouble with this. I remember trouble even thresholded. find out why?
            final double STDDEV_RED_THRESHOLD = 40.0;
            boolean colStddevOk = IVec.sum(DVec.lt(summary.stdCol[0], DVec.mul(DVec.ones(summary.stdCol[0].length), STDDEV_RED_THRESHOLD))) > ((int) (0.9 * summary.stdCol[0].length));
            boolean rowStddevOk = IVec.sum(DVec.lt(summary.stdRow[0], DVec.mul(DVec.ones(summary.stdRow[0].length), STDDEV_RED_THRESHOLD))) > ((int) (0.9 * summary.stdRow[0].length));
            //boolean colStddevOk = true;
            //boolean rowStddevOk = true;

            // check whether red enough
            boolean fractionRedOk = summary.fractionRed > 0.9;
            //boolean fractionRedOk = true;

            // check for reddish image
            boolean reddishOk = (mr/(mg + 0.1) > 2.0) && (mr/(mb + 0.1) > 2.0); // +0.1: avoid division by zero
            //boolean reddishOk = true;

            boolean ok = exposureOk && redExposureOk && colStddevOk && rowStddevOk && reddishOk && fractionRedOk;

            if(++mI % 30 == 0) {
                // TODO: log everything
                logi("ok=" + ok + " <- exposureOk=" + exposureOk + " && redExposureOk=" + redExposureOk + " && colStddevOk=" + colStddevOk + " && rowStddevOk=" + rowStddevOk + " && reddishOk=" + reddishOk + " && fractionRedOk=" + fractionRedOk);
                //  + " fractionRed=" + summary.fractionRed // 'summary' may be null now
                logi("rgb=["+ mr +"," + mg + "," + mb + "]");
            }
            lockStats.mr = mr;
            lockStats.mg = mg;
            lockStats.mb = mb;

            return ok;
        }

        class LockStats {
            public double mr, mg, mb;
        }
        private LockStats lockStats = new LockStats();
    };

    private ISensor.Listener mAudioListener = new ISensor.Listener() {
        @Override
        public void onData(int offset, int length) {

        }

        @Override
        public void onState(ISensor.State newState) {
            logi("AudioSensor: FAILED");
            if(newState == ISensor.State.FAILED) {
                stopSensors();
                mFailureReason = FailureReason.AUDIO_FAIL;
                setState(State.FAILED);
            }
        }
    };

    private ISensor.Listener mAccelerationListener = new ISensor.Listener() {
        @Override
        public void onData(int offset, int length) {

        }

        @Override
        public void onState(ISensor.State newState) {
            logi("AccelerationSensor: newState=" + newState);
            if(newState == ISensor.State.FAILED) {
                logi("AccelerationSensor: FAILED");
                stopSensors();
                mFailureReason = FailureReason.ACCELERO_FAIL;
                setState(State.FAILED);
            }
        }
    };

    @Override
    public void start() {
        if(mState != State.STOPPED && mState != State.FAILED)
            throw new IllegalStateException("start() must be called in STOPPED/FAILED state, but was in " + mState);

        mMeasurement = new Measurement(mDevice.getAppId());
        mStartTimeNanos = System.nanoTime();
        mStartTimeCamera = 0; // set later
        mLockTimeCameraNanos = 0;
        mCameraStartTimeNanos = 0;
        mQualityLockTime = 0.0;
        mLockTimes = new ArrayList<>();
        mUnlockTimes = new ArrayList<>();

        mIntensitySensor.setDebugMode(true);
        mIntensitySensor.start();
        mAudioSensor.start();
        mAccelerationSensor.start();

        setState(State.WAITING);
    }

    @Override
    public void stop() throws FramesDroppedException {
        if(mState == State.STOPPED) {
            logi("already stopped, ignoring spurious call");
            return;
        }

        stopSensors();
        updateMeasurementSeries();

        if(mIntensitySensor.getDroppedCount() > 5*75) { // TODO FIXME Tom ugly hack suggested
            mState = State.FAILED; // silent state change: not emitted
            throw new FramesDroppedException(mIntensitySensor.getDroppedCount());
        }

        setState(State.STOPPED);
    }

    private void stopSensors() {
        Log.i(TAG, "stopSensors()");
        mIntensitySensor.stop();
        mAudioSensor.stop();
        mAccelerationSensor.stop();
    }

    @Override
    public double getLockTime() {
        final double lockTime1 = ((double) (mLockTimeCameraNanos - mStartTimeCamera)) / 1e9;
        return lockTime1;
    }

    private void updateMeasurementSeries() {

        // both of these are currently since the start of camera track
        final double lockTime1 = ((double) (mLockTimeCameraNanos - mStartTimeCamera)) / 1e9;
        final double lockTime2 = mQualityLockTime; // qualityLockTime comes from timeSinceStart.

        final double audioStartTime = (mAudioSensor.getStartTimeNanos() - mStartTimeNanos) / 1e9;

        // Measurement.startTime coincides with mStartTimeNanos, that's the most accurate we can do.
        mMeasurement.meta.lockTime = Math.max(lockTime1, lockTime2);
        mMeasurement.meta.lockTimes = DVec.toArray(mLockTimes);
        mMeasurement.meta.unlockTimes = DVec.toArray(mUnlockTimes);

        // TODO: read and save ISO exposure level.
        mMeasurement.meta.cameraFeatures = mIntensitySensor.getFeatures();
        JSONObject cameraMeta = mIntensitySensor.getCameraMeta();
        try {
            // should usually be NONE
            cameraMeta.put("failureReason", mFailureReason.toString());
        } catch (JSONException e) {
            e.printStackTrace();
        }
        mMeasurement.meta.cameraMeta = cameraMeta.toString();

        double[] ts = mIntensitySensor.getSamples().get(0);
        mMeasurement.meta.ppgMeanFps = ((double) (ts.length-1)) / (ts[ts.length-1] - ts[0]);
        mMeasurement.meta.ppgNumDropped = mIntensitySensor.getDroppedCount();

        mMeasurement.meta.audioStartTime = audioStartTime;
        mMeasurement.meta.audioStartTimeAccuracy = mAudioSensor.getExpectedStartTimeAccuracy();
        mMeasurement.meta.audioFps = mAudioSensor.getFps();
        mMeasurement.meta.bcgFps = mAccelerationSensor.getFps();

        mMeasurement.meta.test = isUnderTest;

        mMeasurement.series = new MeasurementSeries();

        mMeasurement.series.ppgData = SensorData.transposeSamples(mIntensitySensor.getSamples(), (mStartTimeCamera - (mCameraStartTimeNanos - mStartTimeNanos)) / 1e9);
        mMeasurement.series.bcgData = SensorData.transposeSamples(mAccelerationSensor.getSamples(), mStartTimeNanos / 1e9);
        mMeasurement.series.setAudio(mAudioSensor.getPCM(), audioStartTime);
    }

    @Override
    public Measurement getMeasurement() { return mMeasurement; }

    @Override
    public void setListener(Listener listener) { mListener = listener; }

    @Override
    public void setQualityLockTime(double timeSinceStart) { mQualityLockTime = timeSinceStart; }

    private void setState(State newState) {
        logi("setState(" + newState.toString() + ")");
        State prevState = mState;
        mState = newState;
        if(prevState != newState)
            postOnState(newState);
    }

    @Override
    public State getState() {
        return mState;
    }

    private void postOnFrame(final double[] rgb, final IntensityDetector.ImageSummary summary, final double timeSinceStart, final double timeSinceLock) {
        if(mListener != null) {
            final Runnable runnable = new Runnable() {
                @Override
                public void run() {
                    mListener.onFrame(timeSinceStart, timeSinceLock, rgb, summary);
                }
            };
            if(mHandler != null)
                mHandler.post(runnable);
            else
                runnable.run(); // for unit tests
        }
    }

    private void postOnState(final State state) {
        if(mListener != null) {
            final Runnable runnable = new Runnable() {
                @Override
                public void run() {
                    mListener.onState(state);
                }
            };
            if(mHandler != null)
                mHandler.post(runnable);
            else
                runnable.run(); // for unit tests
        }
    }

    private void logi(String msg) {
        if(!isUnderTest)
            Log.i(TAG, msg); // not mocked in unit tests
        else
            System.out.println(msg);
    }

    private FailureReason mFailureReason = FailureReason.NONE;
    @Override
    public FailureReason getFailureReason() { return mFailureReason; }

    private MeasurementMode mMode = MeasurementMode.RISK;
    @Override
    public void setMode(MeasurementMode mode) {
        mMode = mode;
    }
}
