/*
 * Copyright (c) 2014 Paul Scherrer Institute. All rights reserved.
 */

import ch.psi.pshell.device.Device;
import ch.psi.pshell.device.Readable;
import ch.psi.pshell.epics.*;
import java.io.IOException;
import java.util.ArrayList;

/**
 *
 */
public class Camtool extends ArraySource {

    final String prefix;
    final String dataPrefix;
    final boolean latch;

    final public ChannelInteger channelRun;
    final public ChannelInteger channelLatch;    
    final public ChannelDouble channelTimestamp;
    final public ChannelDoubleArray origin;
    final public ChannelDouble posX, posY;
    final public ChannelDoubleArray profileX, profileY;
    final public ChannelIntegerArray shape;
    final public ChannelInteger roiEnabled;
    final public ChannelIntegerArray roiShape;
    final public ChannelInteger bgEnable, bgCapture, bgCaptureRemain;
    final public ChannelDouble calOffX, calOffY, calScaleX, calScaleY;
    final public CamToolPosX posMeanX;
    final public CamToolPosY posMeanY;
    final public CamToolVarX posVarX;
    final public CamToolVarY posVarY;

    public Camtool(String name, String prefix) {
        this(name, prefix, false);
    }

    public Camtool(String name, String prefix, boolean latch) {
        //super(name, prefix + (latch ? ":latch" : ":pipeline") + (roi ? ".roi.output" : ".image"));
        super(name, prefix + (latch ? ":latch" : ":pipeline") + ".roi.output");
        this.prefix = prefix + ":";
        this.latch = latch;
        dataPrefix = this.prefix + (latch ? "latch" : "pipeline") + ".";

        channelRun = new ChannelInteger(name + " run", this.prefix + "camera.run");
        channelLatch = new ChannelInteger(name + " latch", this.prefix + "latch.capture");
        channelTimestamp = new ChannelDouble(name + " timestamp", dataPrefix + "timestamp");
        channelTimestamp.setMonitored(true);
        //posX = new ChannelDouble(name + " com x", dataPrefix + "x_stats.com");
        //posY = new ChannelDouble(name + " com y", dataPrefix + "y_stats.com");
        posX = new ChannelDouble(name + " com x", dataPrefix + "x_stats.com_egu");
        posY = new ChannelDouble(name + " com y", dataPrefix + "y_stats.com_egu");
        profileX = new ChannelDoubleArray(name + " profile x", dataPrefix + "profile.x");
        profileY = new ChannelDoubleArray(name + " profile y", dataPrefix + "profile.y");
        //shape = new ChannelIntegerArray(name + " shape", dataPrefix + (roi ? "roi.output.shape" : "image.shape"));
        shape = new ChannelIntegerArray(name + " shape", dataPrefix + ("image.shape"));
        roiShape = new ChannelIntegerArray(name + " roi shape", dataPrefix + "roi.roi");
        roiEnabled = new ChannelInteger(name + " roi enabled", dataPrefix + "roi.enabled");
        //origin = new ChannelDoubleArray(name + " origin X", roi ? (dataPrefix + "roi.origin_out") : (prefix + "origin"));
        origin = new ChannelDoubleArray(name + " origin X", dataPrefix + "roi.origin_out");
        bgEnable = new ChannelInteger(name + " bg enable", this.prefix + "pipeline.bg.enabled");
        bgCapture = new ChannelInteger(name + " bg capture", this.prefix + "pipeline.bg.capture");
        bgCaptureRemain = new ChannelInteger(name + " bg capture remain", this.prefix + "pipeline.bg.capture_remain");
        calOffX = new ChannelDouble(name + " cal off x", this.prefix + "pipeline.egu.eoff_x");
        calOffY = new ChannelDouble(name + " cal off y", this.prefix + "pipeline.egu.eoff_y");
        calScaleX = new ChannelDouble(name + " cal scale x", this.prefix + "pipeline.egu.eslo_x");
        calScaleY = new ChannelDouble(name + " cal scale y", this.prefix + "pipeline.egu.eslo_y");

        posMeanX = new CamToolPosX();
        posMeanY = new CamToolPosY();
        posVarX = new CamToolVarX();
        posVarY = new CamToolVarY();
    }

    @Override
    public void doSetMonitored(boolean value) {
        super.doSetMonitored(value);
        getDevice().setMonitored(value);
    }

    @Override
    public void doUpdate() throws IOException, InterruptedException {
        super.doUpdate();
        getDevice().update();
    }

    void safeInitialize(Device dev, int timeout) throws IOException, InterruptedException {
        for (int retries = 0; retries < 10; retries++) {
            try {
                dev.initialize();
                break;
            } catch (IOException ex) {
                if (retries == 9) {
                    throw ex;
                }
                Thread.sleep(timeout / 10);
            }
        }
    }
        

    @Override
    protected void doInitialize() throws IOException, InterruptedException {
        try {
            channelRun.initialize();
            channelLatch.initialize();
            if (latch) {
                start();
                latch();
            }
            safeInitialize(channelTimestamp, 2000);
            posX.initialize();
            posY.initialize();
            profileX.initialize();
            profileY.initialize();
            shape.initialize();
            roiShape.initialize();
            roiEnabled.initialize();
            origin.initialize();
            bgEnable.initialize();
            bgCapture.initialize();
            bgCaptureRemain.initialize();
            try {
                calOffX.initialize();
                calOffY.initialize();
                calScaleX.initialize();
                calScaleY.initialize();
            } catch (Exception ex) {
                ex.printStackTrace();
            }
             
            if (roiEnabled.read()>0){
                int[] s = roiShape.read();
                //for (int x : s) System.out.println(x);
                getConfig().imageHeight = s[3];
                getConfig().imageWidth =  s[2];
            } else {
                int[] s = shape.read();
                //for (int x : s){ System.out.println(x);}
                getConfig().imageHeight = s[0];
                getConfig().imageWidth =  s[1];                
            }
                   
            getConfig().save();
            getDevice().setSize(getConfig().imageHeight * getConfig().imageWidth);
            super.doInitialize();            
            //System.out.println(((int[])(getDevice().read())).length);
        } catch (InterruptedException ex) {
            throw ex;
        } catch (Exception ex) {
            ex.printStackTrace();
            throw new IOException(ex);
        }
    }

    int numImages = 1;

    public int getNumImages() {
        return numImages;
    }

    public void setNumImages(int value) {
        numImages = value;
    }

    double grabTimeout = 3.0;

    public double getGrabTimeout() {
        return grabTimeout;
    }

    public void setGrabTimeou(double value) {
        grabTimeout = value;
    }

    public void capture() throws IOException, InterruptedException {
        int retries = 3;
        while (true) {
            try {
                double timestamp = channelTimestamp.read();
                if (latch) {
                    channelLatch.write(1);
                } else {
                    channelRun.write(1);
                }
                long start = System.currentTimeMillis();
                while (true) {
                    double val = channelTimestamp.read();
                    if (timestamp != val) {
                        return;
                    }
                    if ((System.currentTimeMillis() - start) > grabTimeout) {
                        throw new IOException("Frame timeout");
                    }
                    Thread.sleep(5);
                }
            } catch (IOException ex) {
                retries--;
                if (--retries <= 0) {
                    throw ex;
                }
            }
        }
    }

    public void start() throws IOException, InterruptedException {
        channelRun.write(-1);
    }

    public void stop() throws IOException, InterruptedException {
        channelRun.write(0);
    }

    public void grabSingle() throws IOException, InterruptedException {
        channelRun.write(1);
    }

    public void latch() throws IOException, InterruptedException {
        channelLatch.write(1);
    }

    public void enableBackground(boolean value) throws IOException, InterruptedException {
        bgEnable.write(value ? 1 : 0);
    }

    public void captureBackground(int images) throws IOException, InterruptedException {
        start();
        bgCapture.write(images);
        Thread.sleep(200);
        while (bgCaptureRemain.read() > 0) {
            Thread.sleep(10);
        }
    }

    //Statisticss pseudo devices
    ArrayList<Double> posXSamples;
    ArrayList<Double> posYSamples;

    public void updateStats() throws IOException, InterruptedException {
        posXSamples.clear();
        posYSamples.clear();
        for (int i = 0; i < getNumImages(); i++) {
            capture();
            posXSamples.add(posX.read());
            posXSamples.add(posY.read());
        }
    }

    class CamToolPosX implements Readable {

        @Override
        public Object read() throws IOException, InterruptedException {
            return mean(posXSamples);
        }

    }

    class CamToolPosY implements Readable {

        @Override
        public Object read() throws IOException, InterruptedException {
            return mean(posYSamples);
        }
    }

    class CamToolVarX implements Readable {

        @Override
        public Object read() throws IOException, InterruptedException {
            return stdev(posXSamples);
        }

    }

    class CamToolVarY implements Readable {

        @Override
        public Object read() throws IOException, InterruptedException {
            return stdev(posYSamples);
        }
    }

    public double mean(ArrayList<Double> samples) {
        int count = 0;
        double temp = 0;
        for (Double n : samples) {
            if (!Double.isNaN(n)) {
                temp += n.doubleValue();
                count++;
            }
        }
        return count == 0 ? Double.NaN : temp / count;
    }

    public double stdev(ArrayList<Double> samples) {
        int count = 0;
        double temp = 0;
        double mean = mean(samples);
        for (Double n : samples) {
            if (!Double.isNaN(n)) {
                temp += Math.pow((mean - n), 2);
            }
        }
        return count == 0 ? Double.NaN : temp / count;
    }

}