/*************************************************************

   m a i n L o o p . c

 Source file for the low priority main loop functions

 It is recommended for the main loop functions to
 include a delay to prevent using too much CPU cycles

 *************************************************************/

#include "arm_math.h"       // Include for the DSP lib
#include "base.h"           // Basic header file
#include "chprintf.h"       // Serial printf

/****** Variables ********************************************/

// Counters for the saturation detection
// Set to SAT_COUNT when an input saturates or is near saturation
volatile int32_t satCounter1 = 0;
volatile int32_t satCounter2 = 0;

/******************  Basic low priority loop ********************

 Just implements the saturation detection actions

   LED Usage:
     Red    : Lights if ch1 input is near or at saturation
     Orange : Lights if ch2 input is near or at saturation

 ****************************************************************/

void basicLoop(void)
 {
 while (1)
    {
	// Check if we have detected input saturation
	if (satCounter1)
	    {
		RED_ON;          // Turn ON red LED
		satCounter1--;   // Decrease counter
	    }
	    else
	    RED_OFF;         // Turn OFF red LED

	// Check if we have detected input saturation on CH2
	if (satCounter2)
	    {
		ORANGE_ON;        // Turn ON orange LED
		satCounter2--;    // Decrease counter
	    }
	    else
	    ORANGE_OFF;         // Turn OFF orange LED

	// Wait 10ms to loop again
	SLEEP_MS(10);
    }

 SLEEP_MS(10);
 }

/**********************  Loop for P3 *****************************

 Loop for P3

   LED Usage:
     Red    : Lights if ch1 input is near or at saturation
     Orange : Lights if ch2 input is near or at saturation

 ****************************************************************/

extern volatile int32_t windowCount;     // Total number of windows
extern volatile int32_t detections;      // Total number of correct detections
extern volatile int32_t outSamples;      // Number of samples out of window
extern volatile int32_t outFalses;       // Number of samples out of window that give detection

void p3loop(void)
 {
 int32_t cycle = 0;
 int32_t value;
 while (1)
    {
	PRO2_SET;  // Activate PRO2 Line

	// Show counters
	if (SW4_ACTIVE)
	    if (!(cycle % 50)) // Each 0.5s
	        {
	        chprintf(SERIAL,"Count = %d%s",count,BREAK);
	        chprintf(SERIAL,"goodCount = %d%s",goodCount,BREAK);
	        chprintf(SERIAL,"realCount = %d%s",realCount,BREAK);
	        //Falsos Positivos = count - min(goodCount , realCount)
	        value = goodCount;
	        if (realCount < value) value = realCount;
	        value = count - value;
	        chprintf(SERIAL,"False Positives = %d%s",value,BREAK);
	        value = realCount - goodCount;
	        if (value < 0) value = 0;
	        chprintf(SERIAL,"False Negatives = %d%s",value,BREAK);
	        chprintf(SERIAL,"Max Detect = %d%s",maxDetect,BREAK);
	        chprintf(SERIAL,"Max Detect (No pulse) = %d%s",maxDetectNs,BREAK);
	        chprintf(SERIAL,"Max Aux = %d%s",maxAux,BREAK);
	        chprintf(SERIAL,"Mean Vsq = %d (%d samples)%s",Vsq/Nsq,Nsq,BREAK);
	        chprintf(SERIAL,"%s",BREAK);
	        }
	cycle++;

	// Check if we have detected input saturation
	if (satCounter1)
	    {
		RED_ON;          // Turn ON red LED
		satCounter1--;   // Decrease counter
	    }
	    else
	    RED_OFF;         // Turn OFF red LED

	// Check if we have detected input saturation on CH2
	if (satCounter2)
	    {
		ORANGE_ON;        // Turn ON orange LED
		satCounter2--;    // Decrease counter
	    }
	    else
	    ORANGE_OFF;         // Turn OFF orange LED

	// Wait 10ms to loop again
	SLEEP_MS(10);
    }

 PRO2_CLEAR;  // Clear PRO2 Line

 SLEEP_MS(10);
 }

