/**
  ******************************************************************************
  * File Name          : main.c
  * Description        : Main program body
  ******************************************************************************
  *
  * COPYRIGHT(c) 2015 STMicroelectronics
  *
  * Redistribution and use in source and binary forms, with or without modification,
  * are permitted provided that the following conditions are met:
  *   1. Redistributions of source code must retain the above copyright notice,
  *      this list of conditions and the following disclaimer.
  *   2. Redistributions in binary form must reproduce the above copyright notice,
  *      this list of conditions and the following disclaimer in the documentation
  *      and/or other materials provided with the distribution.
  *   3. Neither the name of STMicroelectronics nor the names of its contributors
  *      may be used to endorse or promote products derived from this software
  *      without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 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.
  *
  ******************************************************************************
  */
	
/*
		MODIFIED by Hexabitz for BitzOS (BOS) V0.1.4 - Copyright (C) 2017 Hexabitz
    All rights reserved
*/

/* Includes ------------------------------------------------------------------*/
#include "BOS.h"


extern float h08r6MaxRange;

/* Private variables ---------------------------------------------------------*/
float sensor = 0.0f;
uint32_t t0;

/* Private function prototypes -----------------------------------------------*/
uint8_t AbsDistanceToColor(float distance);
bool DetectedHorizontalSweep(float distance);
uint8_t HorizontalSweepToColor(float distance);

/* Main functions ------------------------------------------------------------*/

int main(void)
{


  /* MCU Configuration----------------------------------------------------------*/

  /* Reset all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* Configure the system clock */
  SystemClock_Config();

  /* Initialize all user peripherals */

	/* Initialize BitzOS */
	BOS_Init();

  /* Call init function for freertos objects (in freertos.c) */
  MX_FREERTOS_Init();

  /* Start scheduler */
  osKernelStart();
  
  /* We should never get here as control is now taken by the scheduler */

  /* Infinite loop */
  while (1)
  {


  }


}

/*-----------------------------------------------------------*/

/* FrontEndTask function */
void FrontEndTask(void * argument)
{
	volatile uint8_t myMode = 0; 
	uint8_t color = 0, colorOld = 0, intensity = 0;
	
	// Define BOS variables
	AddBOSvar(FMT_UINT8, (uint32_t) &myMode);
	
	// Set units to cm
	SetRangeUnit(UNIT_MEASUREMENT_CM);
	
	// Read sensor value continuously in non-blocking mode
	//Stream_ToF_Memory(100, 5000, &sensor);
	//t0 = HAL_GetTick();
	
  /* Infinite loop */
  for(;;)
  {
		sensor = Sample_ToF(0, 0);
		Delay_ms(200);
		
		switch (myMode)
		{
			case 0:								// No action
				break;
			
			case 1:								// LEDs off		
				SendMessageToModule(BOS_BROADCAST, 101, 0);		// CODE_H01R0_OFF
				myMode = 0;					// Reset the mode to execute this only once
				break;
			
			case 2:								// Sensor sweeps LED colors vertically based on absolute distance
			case 3:								// Sensor sweeps LED colors horizontally			
				if (myMode == 2)
					color = AbsDistanceToColor(sensor);
				else
					color = HorizontalSweepToColor(sensor);
			
				if (color != colorOld) {												// Execute only if color changed
					messageParams[0] = 0;													// set color name
					messageParams[1] = color;
					messageParams[2] = 100;
					SendMessageToModule(BOS_BROADCAST, 103, 3);		// CODE_H01R0_COLOR
					colorOld = color;
				}
				break;
				
			case 4:								// Toggle white LEDs with hand sweep
				if (DetectedHorizontalSweep(sensor)) {
					messageParams[0] = 100;
					SendMessageToModule(BOS_BROADCAST, 102, 1);		// CODE_H01R0_TOGGLE
				}
				break;
				
			case 5:								// Sensor controls white LED intensity

				
				messageParams[0] = 0;
				messageParams[1] = 2;													// WHITE
				messageParams[2] = intensity;
				SendMessageToModule(BOS_BROADCAST, 103, 3);		// CODE_H01R0_COLOR				
				break;
			
			default:
				break;

		}
		
		// Reset the mode
		//myMode = 0;

	}
}

// Map absolute sensor distance to basic RGB colors
uint8_t AbsDistanceToColor(float distance)
{
//	static float distanceOld;
	
//	// Only update if there's at least 1cm change 
//	if ( distance-distanceOld <= 1.0f || distanceOld-distance <= 1.0f )
//		distance = distanceOld;
//	else
//		distanceOld = distance;

	if (distance < 10.0f)
		return 5;	// YELLOW
	else if (distance < 15.0f)
		return 8;	// GREEN
	else if (distance < 20.0f)
		return 6;	// CYAN
	else if (distance < 25.0f)
		return 4;	// BLUE
	else if (distance < 30.0f)
		return 7;	// MAGENTA
	else if (distance < 35.0f)
		return 3;	// RED
	else
		return 2;	// WHITE
}

bool DetectedHorizontalSweep(float distance)
{
	static float state;
	
	if (distance < h08r6MaxRange)
		state = 1;	// Detected an object
	else if (distance == h08r6MaxRange && state == 1) {
		state = 2;	// The object cleared
		return true;
	}
		
	return false;	
}

uint8_t HorizontalSweepToColor(float distance)
{
	static float colorOld;
	uint8_t temp;
	
	if (DetectedHorizontalSweep(distance))
	{		
		if (colorOld == 2)
			temp = 5;	// YELLOW
		else if (colorOld == 5)
			temp = 8;	// GREEN
		else if (colorOld == 8)
			temp = 6;	// CYAN
		else if (colorOld == 6)
			temp = 4;	// BLUE
		else if (colorOld == 4)
			temp = 7;	// MAGENTA
		else if (colorOld == 7)
			temp = 3;	// RED
		else if (colorOld == 3)
			temp = 2;	// WHITE		
		
		colorOld = temp;
		return temp;
	}
	else
		return colorOld;
}

/*-----------------------------------------------------------*/

/************************ (C) COPYRIGHT HEXABITZ *****END OF FILE****/
