/* 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 application demonstrates how to acquire BumbleBee radar data from a client mote like TelosB over the I2C bus
// This enables BumbleBee-Imote2 connections without requiring an ITS400 board.
// A corresponding program runs on the client mote and converts radar data using its onboard ADC. 
// 2 modes of data acquisition are supported:
//   a. Single sample - each sample initiates a request over I2C
//   b. 1-second snippet - client mote collects 1-second snippet of data using its own timer
// GPIO based interrupt signals readDone from client mote to Imote2, simulating split-phase semantics.

// This functionality s demonstrated via a simple motion detector using the BumbleBee radar. 
// To demonstrate each data acquisition mode, the application initially acquires radar data in 1-second snippets
// and uses this initial data to estimate the DC level of the sensor.
// Once DC-estimation is complete, the application switches to the single sample mode and periodically acquires a new 
// sensor reading from the I2C client mote.
// The detector measures the 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 System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using Crossbow.platform.imote2;

// Imote2-Client mote interaction settings.
// Must have same values in both nodes.
enum I2C_SENSOR                        
{
    CLIENT_ADDRESS = 0x63,             // Client mote I2C address
    INITIALIZE = 0xaa,                 // Handshaking values
    SINGLE_SAMPLE = 0xbb,               
    ONE_SECOND_SNIPPET = 0xcc,
    SAMPLING_RATE = 166,    
    INTER_SAMPLE_TIME = 1000 / SAMPLING_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 * I2C_SENSOR.SAMPLING_RATE,  // keep detections alive if 8 hits per second
    THRESH = 500,                      // Threshold for deviation over DC level to declare hit
};

namespace Samraksh.TelosSensor
{
    public class TelosSensor
    {
        //complex data type to store I and Q radar data values
        public struct Comp
        {
            public int I, Q;

            public Comp(int p1, int p2)
            {
                I = p1;
                Q = p2;
            }
        }

        // shared LED class
        public static class myLeds
        {
            public static Leds newLeds;

            public static void init()
            {
                newLeds = new Leds();
                newLeds.greenOn();
            }

            public static void blueOn()
            {
                newLeds.blueOn();
            }

            public static void blueOff()
            {
                newLeds.blueOff();
            }

            public static void redOn()
            {
                newLeds.redOn();
            }

            public static void redOff()
            {
                newLeds.redOff();
            }
        }

        public static class ImoteState
        {
            public static byte mode = 0x00;
            public static int busy = 0;
            public static int numSnippets = 0;
        }

        // initialize client mote over I2C
        public static int Initialize()
        {
            I2CDevice I2CInitDevice = new I2CDevice(new I2CDevice.Configuration((ushort)I2C_SENSOR.CLIENT_ADDRESS, 100));
            byte[] wrByte = new byte[1];
            int y=1;  
            
            I2CDevice.I2CWriteTransaction command = I2CInitDevice.CreateWriteTransaction(wrByte);
            I2CDevice.I2CTransaction[] xAction = { command };

            Debug.Print("Starting Init");
           
            wrByte[0] = (byte)I2C_SENSOR.INITIALIZE;
            y = I2CInitDevice.Execute(xAction, 100);

            I2CInitDevice.Dispose();

            Debug.Print("Init Result = " + y.ToString());

            return y;
        }

        // send command to change data acquisition mode to client mote
        public static int SendModeCommand(byte newmode)
        {
            ImoteState.mode = newmode;

            I2CDevice I2CModeDevice = new I2CDevice(new I2CDevice.Configuration((ushort)I2C_SENSOR.CLIENT_ADDRESS, 100));
            byte[] wrByte = new byte[1];
            int y;

            wrByte[0] = newmode;

            I2CDevice.I2CWriteTransaction command = I2CModeDevice.CreateWriteTransaction(wrByte);
            I2CDevice.I2CTransaction[] xAction = { command };

            //Debug.Print("Sending Mode Command");

            y = I2CModeDevice.Execute(xAction, 100);

            I2CModeDevice.Dispose();

            //Debug.Print("Mode command Result = " + y.ToString());

            return y;
        }

        // main estimate of DC level of BumbleBee sensor
        public static class DCEstimate
        {
            public static Comp mean, sum;
            private static int count;

            public static void Init()
            {
                sum.I = sum.Q = 0;
                mean.I = mean.Q = 0;
                count = 0;
            }

            public static void Update(Comp curravg)
            {
                count++;
                sum.I += curravg.I;
                sum.Q += curravg.Q;
                mean.I = sum.I / count;
                mean.Q = sum.Q / count;
            }
        }

        // M-out-of-N Hysterisis logic for robust detection
        // Detected = TRUE iff last N samples contain M hits
        public static class MoutOfNDetector
        {
            public static int state = 0;
            public static int prevstate = 0;

            private static int M, N, End;
            private static int[] Buff;
            private static int sampNum;

            public static void Init(int x, int y)
            {
                M = x;
                N = y;
                sampNum = 0;
                End = 0;
                int i;

                Buff = new int[M];
                for (i = 0; i < M; i++)
                    Buff[i] = -N;
            }

            public static void Update(int detect)
            {
                int next;
                prevstate = state;

                sampNum++;
                next = (End + 1) % M;

                if (sampNum - Buff[next] < N)
                    state = 1;
                else
                    state = 0;

                if (detect == 1)
                {
                    Buff[End] = sampNum;
                    End = next;
                }
            }

        }

        public static void Main()
        {
            DCEstimate.Init();
            MoutOfNDetector.Init((int)DETECTOR.M, (int)DETECTOR.N);

            // configure GPIO port 93 to receive data ready interrupt from client mote
            InterruptPort dataReadyGPIO = new InterruptPort((Cpu.Pin)93, false, Port.ResistorMode.Disabled, Port.InterruptMode.InterruptEdgeHigh);
            dataReadyGPIO.OnInterrupt += new GPIOInterruptEventHandler(dataReadyGPIO_OnInterrupt);

            // initialize application and start in 1-second snippet mode for DC estimation
            Initialize();
            SendModeCommand((byte)I2C_SENSOR.ONE_SECOND_SNIPPET);

            myLeds.init();

            for (; ; )
                System.Threading.Thread.Sleep(1000);
        }

        // Data ready interrupt handler
        static void dataReadyGPIO_OnInterrupt(Cpu.Pin port, bool state, TimeSpan time)
        {
            Comp temp;
            int y;

            //Debug.Print("Received data interrupt");
                               
            //In the first 10 seconds, read snippets and estimate DC level
            if (ImoteState.numSnippets < (int)DETECTOR.DC_EST_SECS)
            {
                byte[] readByte = new byte[4*(int)I2C_SENSOR.SAMPLING_RATE];
                int i;
                
                Comp sum, mean;
                
                sum.I = sum.Q = 0;
                mean.I = mean.Q = 0;
                ImoteState.numSnippets++;

                I2CDevice I2CReadDevice = new I2CDevice(new I2CDevice.Configuration((ushort)I2C_SENSOR.CLIENT_ADDRESS, 100));
                I2CDevice.I2CReadTransaction readcommand = I2CReadDevice.CreateReadTransaction(readByte);
                I2CDevice.I2CTransaction[] yAction = { readcommand };

                y = I2CReadDevice.Execute(yAction, 500);
                Debug.Print("Read Snippet Result = " + y);
                I2CReadDevice.Dispose();
                
                for (i = 0; i < 4*(int)I2C_SENSOR.SAMPLING_RATE; i = i + 4)
                {
                    temp.I = readByte[i] * 256 + readByte[i + 1];
                    temp.Q = readByte[i + 2] * 256 + readByte[i + 3];

                    sum.I += temp.I;
                    sum.Q += temp.Q;
                 }
                
                mean.I = sum.I / (int)I2C_SENSOR.SAMPLING_RATE;
                mean.Q = sum.Q / (int)I2C_SENSOR.SAMPLING_RATE;

                DCEstimate.Update(mean);
            }
            
            // Change from snippet mode to single sample mode
            if (ImoteState.numSnippets == (int)DETECTOR.DC_EST_SECS)
            {
                myLeds.blueOn();
                ImoteState.numSnippets++;
                SendModeCommand((byte)I2C_SENSOR.SINGLE_SAMPLE);
            }

            //Now reading on a per-sample basis
            if (ImoteState.numSnippets > (int)DETECTOR.DC_EST_SECS)
            {
                byte[] readByte = new byte[4];
                int devI, devQ, dev;

                I2CDevice I2CReadDevice = new I2CDevice(new I2CDevice.Configuration((ushort)I2C_SENSOR.CLIENT_ADDRESS, 100));
                I2CDevice.I2CReadTransaction readcommand = I2CReadDevice.CreateReadTransaction(readByte);
                I2CDevice.I2CTransaction[] yAction = { readcommand };

                y = I2CReadDevice.Execute(yAction, 10);

                I2CReadDevice.Dispose();

                temp.I = readByte[0] * 256 + readByte[1];
                temp.Q = readByte[2] * 256 + readByte[3];
                
                
                devI = temp.I - DCEstimate.mean.I;
                devQ = temp.Q - DCEstimate.mean.Q;

                dev = (devI * devI) + (devQ * devQ);

                if (dev > (int)DETECTOR.THRESH * (int)DETECTOR.THRESH)
                    MoutOfNDetector.Update(1);
                else
                    MoutOfNDetector.Update(0);

                if (MoutOfNDetector.state == 1 && MoutOfNDetector.prevstate == 0)
                    myLeds.redOn();
                else if (MoutOfNDetector.prevstate == 1 && MoutOfNDetector.state == 0)
                    myLeds.redOff();

                System.Threading.Thread.Sleep((int)I2C_SENSOR.INTER_SAMPLE_TIME);
                SendModeCommand((byte)I2C_SENSOR.SINGLE_SAMPLE);
                                                 
            }

        }

    }
}