/* Copyright (c) 2008 The Samraksh Company.
 * All rights reserved.
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose, without fee, and without written
 * agreement is hereby granted, provided that the above copyright
 * notice, the following paragraph and any author information appear in
 * all copies of this source code.
 *
 * THIS SOFTWARE IS PROVIDED BY THE SAMRAKSH COMPANY `AS IS' AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN 
 * NO EVENT SHALL THE SAMRAKSH COMPANY BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, LOSS OF USE, DATA, OR PROFITS) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * Version: 1.0, Modified May 10, 2009
 * Author: Sandip Bapat, <sandip.bapat@samraksh.com>
*/

// This sample application uses the BumbleBeeITS400 library to detect motion using a BumbleBee radar
// connected to an Imote2 .Net device via the ITS400 board

// The detector application measures difference in instantaneous radar values over the mean noise 
// and compares it to a threshold.
// Also uses m-out-of-n hysteresis logic to rule out spurious samples triggering a detection and to prevent 
// spurious misses from breaking up detections for same target.

using Crossbow.platform.imote2;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using Samraksh.BumbleBeeITS400;

enum SAMPLING
{
    RATE = 166, //in Hz
    INTERSAMPTIME = 1000/RATE,
}

enum DETECTOR
{
    DC_EST_SECS = 10,       // No. of seconds to estimate mean DC level
    M = 8,                  // M out of N Hysterisis Filter
    N = 1*SAMPLING.RATE,    // keep detections alive if 8 hits per second
    THRESH = 500,           // Threshold for deviation over DC level to declare hit
};

namespace SimpleDetector
{
    public class SimpleDetector
    {
        public static void Main()
        {
            Leds myleds = new Leds();

            ushort chI, chQ;
            int meanI = 0;
            int meanQ = 0;
            int numSamples = 0;
            
            int devI, devQ, dev;
            int sampNum = 0;
            ushort[] compdata = new ushort[2];

            int[] hitTime = new int[(int)DETECTOR.M];
            int i;
            int index = 0;
            int detected = 0;
            int counter = 0;

            for (i = 0; i < (int)DETECTOR.M; i++)
                hitTime[i] = -(int)DETECTOR.N;

            BumbleBeeSensor newSensor = new BumbleBeeSensor();

            Debug.Print("Starting Detector");
            for (; ; )
            {
                newSensor.readPair(compdata);
                
                chI = compdata[0];
                chQ = compdata[1];

                // use initial samples for DC level estimation
                if (numSamples < (int)DETECTOR.DC_EST_SECS * (int)SAMPLING.RATE)
                {
                    meanI += chI;
                    meanQ += chQ;
                    numSamples++;
                }
                // estimate DC level on both channels
                else if (numSamples == (int)DETECTOR.DC_EST_SECS * (int)SAMPLING.RATE)
                {
                    meanI = meanI / ((int)DETECTOR.DC_EST_SECS * (int)SAMPLING.RATE);
                    meanQ = meanQ / ((int)DETECTOR.DC_EST_SECS * (int)SAMPLING.RATE);
                    numSamples++;
                    Debug.Print("DC Estimation Complete");
                    myleds.blueOn();
                }
                // start detecting using estimated DC levels
                else
                {
                    sampNum++;

                    // Threshold logic: if (Ival-meanNoise)^2 + (Qval-meanNoise)^2 > Thresh^2, sample is a hit
                    devI = (int)chI - meanI;
                    devQ = (int)chQ - meanQ;
                    dev = (devI*devI) + (devQ*devQ);
                              

                    if (dev > (int)DETECTOR.THRESH * (int)DETECTOR.THRESH)
                    {
                        hitTime[index] = sampNum;
                		index = (index + 1) % (int)DETECTOR.M;

		                if ((sampNum - hitTime[index] <= (int)DETECTOR.N) && (detected == 0)) // Detector criteria are satisfied
                        {
				            detected = 1;						// Trigger a new detection 
				            counter++;							// count number of detection events
				            Debug.Print("Detection #" + counter + " Start");
				            myleds.redOn();     				// Red led On for visual indication to user
		                }
	                }
                    else if ((sampNum - hitTime[index] > (int)DETECTOR.N) && (detected == 1)) // Detector criteria are not satisfied
                    {
                        detected = 0;						// End active detections
                        Debug.Print("Detection Stop");
                        myleds.redOff();					// Red led Off for visual indication to user
                    }
                        
                    //Debug.Print(sampNum.ToString() + ": " + chI + " " + chQ + " " + devI + " " + devQ);
                }

                System.Threading.Thread.Sleep((int)SAMPLING.INTERSAMPTIME);
            }
        }
    }
}