// this function returns the bit value of the current sample
    // possible results:
    // 1900 Hz      ->  4 (leader tone)
    // 1500 Hz      ->  3 (sync porch, separator pulse)
    // 1300 Hz      ->  2 (VIS bit 0)
    // 1200 Hz      ->  1 (break, VIS start&stop bit)
    // 1100 Hz      ->  0 (VIS bit 1)
    public int demodulator(double sample) {
        double[] lines = new double[5];

        lines[0] = bandPassFreq0(sample);
        lines[1] = bandPassFreq1(sample);
        lines[2] = bandPassFreq2(sample);
        lines[3] = bandPassFreq3(sample);
        lines[4] = bandPassFreq4(sample);

        // calculating the RMS of the lines (squaring them)
        lines[0] *= lines[0];
        lines[1] *= lines[1];
        lines[2] *= lines[2];
        lines[3] *= lines[3];
        lines[4] *= lines[4];

        // lowpass filtering the lines
        lines[0] = lowPass0(lines[0]);
        lines[1] = lowPass1(lines[1]);
        lines[2] = lowPass2(lines[2]);
        lines[3] = lowPass3(lines[3]);
        lines[4] = lowPass4(lines[4]);

        // writing output wav for debugging purposes
        wav0.write(getBytesFromDouble(lines[0]), 0, 2);
        wav1.write(getBytesFromDouble(lines[1]), 0, 2);
        wav2.write(getBytesFromDouble(lines[2]), 0, 2);
        wav3.write(getBytesFromDouble(lines[3]), 0, 2);
        wav4.write(getBytesFromDouble(lines[4]), 0, 2);

        // deciding which line is the highest and returning it's index
        int maxIndex = 0;
        double maxVal = lines[0];
        for (int i = 1; i < lines.length; i++) {
            if (lines[i] > maxVal) {
                maxVal = lines[i];
                maxIndex = i;
            }
        }
        return maxIndex;
    }

    // this function returns at the half of a bit with the bit's value
    public int getVisBit() {
        int val = 0;
        for (int i = 0; i < oneBitSampleCount; i++) {
            val = demodulator(getSample());
            wavFM.write(getBytesFromDouble(0), 0, 2);
        }

        // putting a tick to the output wav signing the moment when the function returned
        wav0.write(100); wav0.write(100); wav0.write(100); wav0.write(100); wav0.write(100); wav0.write(100);

        return val;
    }

    public void startVISReceiving() {
        int bitResult;

        System.out.print("Header received, starting receiving VIS code: ");
        oneBitSampleCount = SAMPLERATE*0.030;
        // waiting half bit time (15ms)
        for (int i = 0; i < oneBitSampleCount/2; i++) {
            demodulator(getSample());
            wavFM.write(getBytesFromDouble(0), 0, 2);
        }

        int VIS = 0;
        int parity = 0;
        boolean parityError = false;
        for (int i = 0; i < 9; i++) {
            bitResult = getVisBit();
            int bit = bitResult == 0 ? 1 : 0;
            switch (i) {
                case 7:
                    if (parity != bit)
                        parityError = true;
                    break;
                case 8: break; // stop bit
                default:
                    if (bitResult == 0)
                        VIS |= bit << (i);
                    parity ^= bit;
                    System.out.print(bit);
            }
        }
        System.out.println();
        System.out.print("VIS: " + VIS);
        if (parityError)
            System.out.print(" parity ERROR");
        else
            System.out.print(" parity OK");

        if (VIS == 44) // Martin M1 VIS code
            System.out.println(" VIS MARTIN OK");
        else
            System.out.println(" VIS UNKNOWN ERROR");

        // waiting to reach the end of the stop bit
        for (int i = 0; i < oneBitSampleCount/4+5; i++) {
            demodulator(getSample());
            wavFM.write(getBytesFromDouble(0), 0, 2);
        }
    }

    public int[] waitForEdge(int[] result) {
        result[0] = result[1];
        while (!Thread.interrupted()) {
            result[1] = demodulator(getSample());
            wavFM.write(getBytesFromDouble(0), 0, 2);
            if (result[0] != result[1] || Thread.interrupted())
                break;
            result[0] = result[1];
        };
        return result;
    }

    public void waitForStart() {
        int[] result = new int[2];
        int edge41Count = 0;

        result[0] = result[1] = 0;
        while (!Thread.interrupted()) {
            result = waitForEdge(result);

            // this part starts VIS receiving if we receive 41 -> 14 -> 41 transitions (bits 4141) in sequence
            switch (edge41Count) {
                case 0:
                    if (result[0] == 4 && result[1] == 1) {
                        edge41Count = 1;
                        System.out.println("Got edge: 41");
                        continue;
                    }
                    break;
                case 1:
                    if (result[0] == 1 && result[1] == 4) { // got 41 -> 14
                        edge41Count = 2;
                        System.out.println("Got edge: 14");
                        continue;
                    }
                    break;
                case 2:
                    if (result[0] == 4 && result[1] == 1) { // got 41 -> 14 -> 41
                        startVISReceiving();
                        receiveImage();
                    }
                    break;
            }
            edge41Count = 0;
        }
    }

    // 800Hz, beta 0.5, impulse length 50
    private double[] FIRLPCoeffs = { +0.0001082461, +0.0034041195, +0.0063570207, +0.0078081648,
            +0.0060550614, -0.0002142384, -0.0104500335, -0.0211855480,
            -0.0264527776, -0.0201269304, +0.0004419626, +0.0312014771,
            +0.0606261038, +0.0727491887, +0.0537028370, -0.0004362161,
            -0.0779387981, -0.1511168919, -0.1829049634, -0.1390189257,
            -0.0017097774, +0.2201896764, +0.4894395006, +0.7485289338,
            +0.9357596142, +1.0040320616, +0.9357596142, +0.7485289338,
            +0.4894395006, +0.2201896764, -0.0017097774, -0.1390189257,
            -0.1829049634, -0.1511168919, -0.0779387981, -0.0004362161,
            +0.0537028370, +0.0727491887, +0.0606261038, +0.0312014771,
            +0.0004419626, -0.0201269304, -0.0264527776, -0.0211855480,
            -0.0104500335, -0.0002142384, +0.0060550614, +0.0078081648,
            +0.0063570207, +0.0034041195, +0.0001082461,
          };
    private double[] xvFIRLP1 = new double[FIRLPCoeffs.length];
    public double FIRLowPass1(double sampleIn) {
        for (int i = 0; i < xvFIRLP1.length-1; i++)
            xvFIRLP1[i] = xvFIRLP1[i+1];
        xvFIRLP1[xvFIRLP1.length-1] = sampleIn / 5.013665674e+00;
        double sum = 0;
        for (int i = 0; i <= xvFIRLP1.length-1; i++)
            sum += (FIRLPCoeffs[i] * xvFIRLP1[i]);
        return sum;
    }
   
    private double[] xvFIRLP2 = new double[FIRLPCoeffs.length];
    public double FIRLowPass2(double sampleIn) {
        for (int i = 0; i < xvFIRLP2.length-1; i++)
            xvFIRLP2[i] = xvFIRLP2[i+1];
        xvFIRLP2[xvFIRLP2.length-1] = sampleIn / 5.013665674e+00;
        double sum = 0;
        for (int i = 0; i <= xvFIRLP2.length-1; i++)
            sum += (FIRLPCoeffs[i] * xvFIRLP2[i]);
        return sum;
    }

    // moving average
    private double[] xvMA1 = new double[9];
    private double yvMA1prev = 0;
    public double noiseReductionFilter1(double sampleIn) {
        for (int i = 0; i < xvMA1.length-1; i++)
            xvMA1[i] = xvMA1[i+1];
        xvMA1[xvMA1.length-1] = sampleIn;
        yvMA1prev = yvMA1prev+xvMA1[xvMA1.length-1]-xvMA1[0];
        return yvMA1prev;
    }

    private double[] xvMA2 = new double[xvMA1.length];
    private double yvMA2prev = 0;
    public double noiseReductionFilter2(double sampleIn) {
        for (int i = 0; i < xvMA2.length-1; i++)
            xvMA2[i] = xvMA2[i+1];
        xvMA2[xvMA2.length-1] = sampleIn;
        yvMA2prev = yvMA2prev+xvMA2[xvMA2.length-1]-xvMA2[0];
        return yvMA2prev;
    }

    //private double fmDemodMinVal = 0, fmDemodMaxVal = 0;
    private double oscPhase = 0;
    private double realPartPrev = 0, imaginaryPartPrev = 0;
    public int fmDemodulateLuminance(double sample) {
        oscPhase += (2 * Math.PI * 2000) / SAMPLERATE;
       
        double realPart = Math.cos(oscPhase) * sample;
        double imaginaryPart = Math.sin(oscPhase) * sample;
           
        if (oscPhase >= 2 * Math.PI)
            oscPhase -= 2 * Math.PI;

        realPart = FIRLowPass1(realPart);
        imaginaryPart = FIRLowPass2(imaginaryPart);

        realPart = noiseReductionFilter1(realPart);
        imaginaryPart = noiseReductionFilter2(imaginaryPart);

        sample = (imaginaryPart*realPartPrev-realPart*imaginaryPartPrev)/(realPart*realPart+imaginaryPart*imaginaryPart);

        realPartPrev = realPart;
        imaginaryPartPrev = imaginaryPart;

        sample += 0.2335; // bring the value above 0

        /*if (fmDemodMinVal > sample) { // these min/max values were used to calibrate DC demodulator
            fmDemodMinVal = sample;
            System.out.println("fmDemodMinVal: " + sample);
        }
        if (fmDemodMaxVal < sample) {
            fmDemodMaxVal = sample;
            System.out.println("fmDemodMaxVal: " + sample);
        }*/
       
        wavFM.write(getBytesFromDouble(sample), 0, 2);
        int luminance = (int)Math.round((sample/0.617)*255);
        luminance = 255-luminance;
        //System.out.println("lum: " + luminance);
        if (luminance > 255)
            luminance = 255;
        if (luminance < 0)
            luminance = 0;

        return luminance;
    }

    public void receiveImage() {
        double pixelLengthInS = 0.0004576;
        double syncLengthInS = 0.004862;
        double porchLengthInS = 0.000572;
        double separatorLengthInS = 0.000572;
        double channelLengthInS = pixelLengthInS*320;
        double channelGStartInS = syncLengthInS + porchLengthInS;
        double channelBStartInS = channelGStartInS + channelLengthInS + separatorLengthInS;
        double channelRStartInS = channelBStartInS + channelLengthInS + separatorLengthInS;
        double lineLengthInS = syncLengthInS + porchLengthInS + channelLengthInS + separatorLengthInS + channelLengthInS + separatorLengthInS + channelLengthInS + separatorLengthInS;
        double imageLengthInSamples = (lineLengthInS*256)*SAMPLERATE;
       
        double t, linet;
        double nextSyncTime = 0;

        // clearing displayed image
        for (int y = 0; y < 256; y++) {
            for (int x = 0; x < 320; x++)
                displayedImage[x][y] = new Color(0,0,0,0);
        }

        wav2.write(100); wav2.write(100); wav2.write(100); wav2.write(100); wav2.write(100); wav2.write(100);

        double sample;
        for (int s = 0; s < imageLengthInSamples && !Thread.interrupted(); s++) {
            t = s/(double)SAMPLERATE;
            linet = t % lineLengthInS;

            sample = getSample();
            demodulator(sample);
            int lum = fmDemodulateLuminance(sample);
           
            if (t >= nextSyncTime) {
                nextSyncTime += lineLengthInS;
                wav1.write(getBytesFromDouble(1), 0, 2);
                repaint();
            }

            if (linet >= channelGStartInS && linet < channelGStartInS + channelLengthInS ||
                linet >= channelBStartInS && linet < channelBStartInS + channelLengthInS ||
                linet >= channelRStartInS && linet < channelRStartInS + channelLengthInS) {

                int y = (int)Math.floor(t/lineLengthInS);
                if (linet >= channelGStartInS && linet < channelGStartInS + channelLengthInS) {
                    int x = (int)Math.floor(((linet-channelGStartInS)/channelLengthInS)*320);
                    displayedImage[x][y] = new Color(0, lum, 0);
                }
                if (linet >= channelBStartInS && linet < channelBStartInS + channelLengthInS) {
                    int x = (int)Math.floor(((linet-channelBStartInS)/channelLengthInS)*320);
                    displayedImage[x][y] = new Color(0, displayedImage[x][y].getGreen(), lum);
                }
                if (linet >= channelRStartInS && linet < channelRStartInS + channelLengthInS) {
                    int x = (int)Math.floor(((linet-channelRStartInS)/channelLengthInS)*320);
                    displayedImage[x][y] = new Color(lum, displayedImage[x][y].getGreen(), displayedImage[x][y].getBlue());
                    //displayedImage[x][y] = new Color(lum, lum, lum);
                }
            }
        }
        repaint();
        System.out.println("Finished rx.");
    }