/**
* Velorex based vehicle
*
* Author: Jan Dvořák z Vozerovic
* E-mail: dvorkaman@gmail.com
* Web: dvorkaman.asp2.cz
* Created: 2015
*/

/* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements.  See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* The ASF licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License.  You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

// INCLUDES
#include "main.h"

/* MAP */
/*
MOTOR 3: Drive
MOTOR 4: Lights

SERVO 1: STEER

LIGHT 1: FRONT
LIGHT 2: LEFT
LIGHT 3: RIGHT
LIGHT 4: BACK
*/

libF4Mini f4mini;

libPWMPDC pdc;
libPWMpin audioPin;
bool hornOn;
uint8_t hornState;
bool pdcOff;

libAnalog distAnalog;
uint8_t distLine[REM_COMMAND_DATA_LENGTH] = { 'D', 'i', 's', 't', 'a', 'n', 'c', 'e', ' ', ':', '_', '_', '_', '_', 'u', '\0' };
uint16_t d, stopLimit;

// MAIN
int main(void)
{
	bool connected = FALSE;
	systemTime nextDistSent = { 0, 0 }, now;

	system_initialize(3); // A..C
	systemTime_initialize();

	F4Mini_initialize(
		&f4mini,

		XBEE_API_COORD_MSB, XBEE_API_COORD_LSB, // coordinator xbee addresses (me is slave)

		tim2, 					// A0..A3 type
		libPin_OUTPUT, 	// A0 - TONE
		libPin_OUTPUT,	// A1
		libPin_OUTPUT,	// A2
		libPin_OUTPUT,	// A3

		libPin_OUTPUT,	// A8

		libPin_OUTPUT,	// B12
		libPin_OUTPUT,	// B13

		libPin_ANALOG,	// C0 - DISTANCE
		libPin_OUTPUT,	// C1
		libPin_OUTPUT,	// C2
		libPin_OUTPUT,	// C4
		libPin_OUTPUT,	// C5
		libPin_OUTPUT	// C9
		);

	initializePeripherals();

	// update servo width
	pwmServo_setBounds(&f4mini.servo1, 600, 2400);
	pdcOff = FALSE;
	stopLimit = 0;

	//Main loop
	while (1)
	{
		// Xbee input
		if (xbeeRx_hasRecievedCommand(&f4mini.xbee) == TRUE)
		{
			processCommand(xbeeRx_getRecievedParsedCommand(&f4mini.xbee));
		}

		// Connection state
		if (xbeeRx_isConnected(&f4mini.xbee, TRUE) == FALSE) // consider if xbee is active to connection state
		{
			// Abort connected
			if (connected == TRUE)
			{
				F4Mini_clearPerihperals(&f4mini);
				clearPeripherals();
				connected = FALSE;
				gpioPin_write(&f4mini.red, Bit_RESET);
			}
		}
		else
		{
			// Set connected
			if (connected == FALSE)
			{
				connected = TRUE;
				gpioPin_write(&f4mini.red, Bit_SET);
			}

			// Tones
			toneLoop();

			// PDC
			if (analogFilter_isReady(&f4mini.af) == TRUE)
			{
				d = ANALOG_VALUE_RANGE - analogFilter_getValue(&f4mini.af, 0);
				pwmTone_pdcSetDistance(&pdc, d / 10);
				analogFilter_clear(&f4mini.af);

				if (d <= stopLimit && motor_getDirection(&f4mini.motor3) == MOTOR_FORWARD)
				{
					motor_stop(&f4mini.motor3);
				}
			}

			if (pdcOff == FALSE)
			{
				pwmTone_pdcLoop(&pdc);
			}

			// Info
			now = systemTime_getTime();
			if (systemTime_compareTimes(now, nextDistSent) >= 0)
			{
				numberToDec(d / 10, 4, &distLine[10]);
				xbeeRx_sendData(&f4mini.xbee, 4, &distLine[0], REM_COMMAND_DATA_LENGTH);
				nextDistSent = systemTime_build(now.s, now.ms + SEND_DIST_EACH);
			}
		}
	}
}

// PROCESS COMMAND
void processCommand(libRemoteCommandValue cv)
{
	switch (cv.command)
	{
		// DRIVE
		case REM_COMMNAND_ANALOG1:
			motor_runByAnalogValue(&f4mini.motor3, cv.value, DRIVE_MIN);
			break;

		case REM_COMMNAND_ANALOG8:
			pwmServo_setPositionByAnalogValue(&f4mini.servo1, ANALOG_MAX_POSITION - cv.value, STEER_MIN, STEER_MAX);
			break;

			// Lights motor
		case REM_COMMNAND_BUTTON1:
			if (cv.value == 0)
			{
				motor_stop(&f4mini.motor4);
			}
			else
			{
				motor_runForwardsPercent(&f4mini.motor4, LIGHT_SPEED);
			}
			break;

		case REM_COMMNAND_BUTTON2:
			if (cv.value == 0)
			{
				motor_stop(&f4mini.motor4);
			}
			else
			{
				motor_runBackwardsPercent(&f4mini.motor4, LIGHT_SPEED);
			}
			break;

			// LIGHTS
		case REM_COMMNAND_BUTTON13:
			gpioPin_write(&f4mini.light2, cv.value == 0 ? Bit_RESET : Bit_SET); //left
			break;

		case REM_COMMNAND_BUTTON14:
			gpioPin_write(&f4mini.light3, cv.value == 0 ? Bit_RESET : Bit_SET); //right
			break;

		case REM_COMMNAND_SWITCH1:
			gpioPin_write(&f4mini.light1, cv.value == 0 ? Bit_RESET : Bit_SET); //front
			break;

		case REM_COMMNAND_SWITCH3:
			gpioPin_write(&f4mini.light4, cv.value == 0 ? Bit_RESET : Bit_SET); //rear
			break;

			// AUDIO
		case REM_COMMNAND_BUTTON12:
			hornOn = cv.value == 0 ? FALSE : TRUE;
			break;

		case REM_COMMNAND_SWITCH11:
			pdcOff = cv.value == 0 ? FALSE : TRUE;
			break;

		case REM_COMMNAND_ANALOG3:
			pdc.firstToneDistance = cv.value / 10;
			break;

		case REM_COMMNAND_ANALOG4:
			pdc.fullToneDistance = cv.value / 10;
			stopLimit = cv.value;
			break;
	}
}

// INIT
void initializePeripherals()
{
	pwm_initializePin(&audioPin, TIM2, 1); // A0

	pwmTone_pdcInitialize(&pdc, &audioPin, 0, ANALOG_VALUE_RANGE / 10);
	// sharpDistance_initializeAnalogFilter(&dist, &f4mini.af, 0); // C0 at 0 index

	// Turn on lights for a moment
	gpioPin_write(&f4mini.light1, Bit_SET);
	gpioPin_write(&f4mini.light2, Bit_SET);
	gpioPin_write(&f4mini.light3, Bit_SET);
	gpioPin_write(&f4mini.light4, Bit_SET);
	pwmTone_hornStart(&audioPin);
	systemTime_delayMs(1000);

	clearPeripherals();
}

// CLEAR
void clearPeripherals()
{
	hornOn = FALSE;
	hornState = 0;
	pwmTone_hornStop(&audioPin);
	F4Mini_clearPerihperals(&f4mini);
}

// Tone loop
void toneLoop()
{
	// HORN
	if (hornState == 0 && hornOn == TRUE)
	{
		// on
		pwmTone_hornStart(&audioPin);
		hornState = 1;
		return;
	}

	if (hornState == 1 && hornOn == FALSE)
	{
		// off
		pwmTone_hornStop(&audioPin);
		hornState = 0;
		return;
	}
}