/**
* Bulldozer with XBee remote controll
*
* Author: Jan Dvořák z Vozerovic
* E-mail: dvorkaman@gmail.com
* Web: dvorkaman.asp2.cz
* Created: 2014
*/

/* 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"

/* Global Variables */
// Motors - TIM2 - A0..A1 (Drive left, Drive right) | A6 A7 | B0 B1
libMotor driveLeft, driveRight;

// Servos - TIM3 remap - C6..C9
libServo liftLeft, liftRight, tilt;
libPWMpin liftLeftPin, liftRightPin, tiltPin;

// Lights C0..C2 (FW, FB, R);
libPin frontLight, frontLightBlue, rearLight;
BitAction lastLightsState;
systemTime lightsPeviousBlinkTime;

// Xbee - USART1 - A8..A12 ; A9 TX + A10 RX
libXbeeRx xbeeRx; // Reciever logic

// - connection
bool connected = FALSE;

/**
* Main method
*/
int main(void)
{
	// Variables
	libRemoteCommandValue cv;

	// Initialize
	system_initialize(3);
	systemTime_initialize();

	initializePeriherals();
	clearPeriherals();

	// Run
	while (1)
	{
		if (connected == FALSE) { showConnecting(); }

		// Xbee input
		if (xbeeRx_hasRecievedCommand(&xbeeRx) == TRUE)
		{
			cv = xbeeRx_getRecievedParsedCommand(&xbeeRx);
			processCommand(cv);
		}

		// Connection state
		if (xbeeRx.remoteXbee.state == REM_XBEE_NOT_CONNECTED || xbeeRx.remoteXbee.state == REM_XBEE_CONNECTING || xbeeRx.remoteXbee.active == FALSE)
		{
			// Abort connected
			if (connected == TRUE)
			{
				clearPeriherals();
				connected = FALSE;
			}
		}
		else
		{
			// Set connected
			if (connected == FALSE)
			{
				clearPeriherals();
				connected = TRUE;
			}
		}
	}
}

/**
* Initialize perihperals
*/
void initializePeriherals(void)
{
	// Lights C0..C2
	gpioPin_initialize(&frontLight, GPIOC, GPIO_Pin_0, libPin_OUTPUT);
	gpioPin_initialize(&frontLightBlue, GPIOC, GPIO_Pin_1, libPin_OUTPUT);
	gpioPin_initialize(&rearLight, GPIOC, GPIO_Pin_2, libPin_OUTPUT);

	// Motor
	pwm_initializeTimer2();
	motor_initializePWM(&driveLeft, GPIOA, GPIO_Pin_6, GPIO_Pin_7, TIM2, 1);
	motor_initializePWM(&driveRight, GPIOB, GPIO_Pin_0, GPIO_Pin_1, TIM2, 2);

	// Servos
	pwm_initializeTimer3Remap();
	pwm_initializePin(&liftLeftPin, TIM3, 1);
	pwm_initializePin(&liftRightPin, TIM3, 2);
	pwm_initializePin(&tiltPin, TIM3, 3);

	pwmServo_inizializeSimple(&liftLeft, &liftLeftPin);
	pwmServo_inizializeSimple(&liftRight, &liftRightPin);
	pwmServo_inizializeSimple(&tilt, &tiltPin);

	// Xbee
	initializeUSART1(); // A9 = TX, A10 = RX
	xbeeRx_initialize(&xbeeRx, USART1, ROUTER, XBEE_API_COORD_MSB, XBEE_API_COORD_LSB);
}

/**
* Clear perihperals
*/
void clearPeriherals(void)
{
	// Lights
	gpioPin_write(&frontLight, Bit_RESET);
	gpioPin_write(&frontLightBlue, Bit_RESET);
	gpioPin_write(&rearLight, Bit_RESET);

	lastLightsState = Bit_RESET;

	// Morors
	motor_stop(&driveLeft);
	motor_stop(&driveRight);

	// Servos
	pwmServo_setPosition(&liftLeft, LIFT_MIDDLE);
	pwmServo_setPosition(&liftRight, LIFT_MIDDLE);
	pwmServo_setPosition(&tilt, TILT_MIDDLE);
}

/**
* Show connecting
*/
void showConnecting(void)
{
	systemTime t = systemTime_getTime();
	BitAction negated = lastLightsState;
	if (systemTime_getTimeDiffInMs(lightsPeviousBlinkTime, t) < BLINK_INTERVAL) { return; }

	// show, negate value
	lightsPeviousBlinkTime = t;
	lastLightsState = (BitAction)(1 - lastLightsState);

	// Leds
	gpioPin_write(&frontLight, lastLightsState);
	gpioPin_write(&frontLightBlue, negated);
	gpioPin_write(&rearLight, lastLightsState);
}

/**
* Process input command
*/
void processCommand(libRemoteCommandValue cv)
{
	uint16_t val;
	switch (cv.command)
	{
		// - MOTORS -
		// Drive LEFT
		case REM_COMMNAND_ANALOG1:
			if (cv.value == ANALOG_MIDDLE_POSITION)
			{
				motor_stop(&driveLeft);
			}
			else
			{
				if (cv.value < ANALOG_MIDDLE_POSITION)
				{
					motor_runForwardsPercent(&driveLeft, 100 - map(cv.value, 0, ANALOG_MIDDLE_POSITION, DRIVE_MIN, 100));
				}
				else
				{
					motor_runBackwardsPercent(&driveLeft, map(cv.value, ANALOG_MIDDLE_POSITION, ANALOG_MAX_POSITION, DRIVE_MIN, 100));
				}
			}

			break;

		// Drive RIGHT
		case REM_COMMNAND_ANALOG3:
			if (cv.value == ANALOG_MIDDLE_POSITION)
			{
				motor_stop(&driveRight);
			}
			else
			{
				if (cv.value < ANALOG_MIDDLE_POSITION)
				{
					motor_runForwardsPercent(&driveRight, 100 - map(cv.value, 0, ANALOG_MIDDLE_POSITION, DRIVE_MIN, 100));
				}
				else
				{
					motor_runBackwardsPercent(&driveRight, map(cv.value, ANALOG_MIDDLE_POSITION, ANALOG_MAX_POSITION, DRIVE_MIN, 100));
				}
			}

			break;

		// - SERVOS -
		// Lift
		case REM_COMMNAND_ANALOG8:
			val = map(cv.value, 0, ANALOG_MAX_POSITION, TILT_MIN, TILT_MAX);
			pwmServo_setPosition(&liftLeft, val);
			pwmServo_setPosition(&liftRight, TILT_MIDDLE + ((200 + TILT_MIDDLE) - val) - 200); // protect overflow
			break;

		// Tilt
		case REM_COMMNAND_ANALOG7:
			pwmServo_setPosition(&tilt, map(cv.value, 0, ANALOG_MAX_POSITION, TILT_MIN, TILT_MAX));
			break;

		// - LIGHTS -
		// Front
		case REM_COMMNAND_SWITCH1:
			gpioPin_write(&frontLight, (BitAction)cv.value);
			break;

		// Front / blue
		case REM_COMMNAND_SWITCH2:
			gpioPin_write(&frontLightBlue, (BitAction)cv.value);
			break;

		// Back
		case REM_COMMNAND_SWITCH3:
			gpioPin_write(&rearLight, (BitAction)cv.value);
			break;
	}
}