/**
* School bus
*
* 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 1: Drive
MOTOR 2: Hood
// - cannot be used by f4 mini

SERVO 1: STEER
SERVO 2: LOCK 4WD
SERVO 3: GEAR LEFT
SERVO 4: GEAR RIGHT

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

libF4Mini f4mini;
uint8_t gearLCD[8] = "Gear: _";
libMotor m1, m2; // STOP, DOORS
libPWMpin tone; // audio output
libPWMbeep toneBeep;

bool hornOn, beepOn;
uint8_t hornState, beepState;

// MAIN
int main(void)
{
	bool connected = FALSE;

	system_initialize(3); // A..C
	systemTime_initialize();
	gearLCD[7] = '\0'; // end string

	F4Mini_initialize(
		&f4mini,

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

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

		libPin_OUTPUT,	// A8

		libPin_OUTPUT,	// B12
		libPin_OUTPUT,	// B13

		libPin_OUTPUT,	// C0
		libPin_OUTPUT,	// C1
		libPin_OUTPUT,	// C2
		libPin_OUTPUT,	// C4
		libPin_OUTPUT,	// C5
		libPin_OUTPUT,	// C9

		0 // ADC ADRESS - NO ADC APPLIED
		);

	initializePeripherals();

	// update servo width
	pwmServo_setBounds(&f4mini.servo1, 600, 2400);
	pwmServo_setBounds(&f4mini.servo2, 600, 2400);
	pwmServo_setBounds(&f4mini.servo3, 600, 2400);
	pwmServo_setBounds(&f4mini.servo4, 600, 2400);

	//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();
		}
	}
}

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

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

			// LOCK
		case REM_COMMNAND_SWITCH5:
			if (cv.value == 0)
			{
				pwmServo_setPosition(&f4mini.servo2, LOCK_OFF);
			}
			else
			{
				pwmServo_setPosition(&f4mini.servo2, LOCK_ON);
			}

			break;

			// GEAR BOX
		case REM_COMMNAND_SWITCH1:
			setGear(cv.value == 0 ? 0 : 1);
			break;

		case REM_COMMNAND_SWITCH2:
			setGear(cv.value == 0 ? 0 : 2);
			break;

		case REM_COMMNAND_SWITCH3:
			setGear(cv.value == 0 ? 0 : 3);
			break;

		case REM_COMMNAND_SWITCH4:
			setGear(cv.value == 0 ? 0 : 4);
			break;

		case REM_COMMNAND_BUTTON1:
			if (cv.value > 0)setGear(1);
			break;

		case REM_COMMNAND_BUTTON2:
			if (cv.value > 0)setGear(2);
			break;

		case REM_COMMNAND_BUTTON3:
			if (cv.value > 0)setGear(3);
			break;

		case REM_COMMNAND_BUTTON4:
			if (cv.value > 0)setGear(4);
			break;

		case REM_COMMNAND_BUTTON5:
			if (cv.value > 0)setGear(0);
			break;

			// HOOD
		case REM_COMMNAND_BUTTON9:
			if (cv.value == 0)
			{
				motor_stop(&f4mini.motor2); // up
			}
			else
			{
				motor_runForwardsPercent(&f4mini.motor2, HOOD_SPEED);
			}
			break;

		case REM_COMMNAND_BUTTON12:
			if (cv.value == 0)
			{
				motor_stop(&f4mini.motor2); // down
			}
			else
			{
				motor_runBackwardsPercent(&f4mini.motor2, HOOD_SPEED);
			}
			break;

			// STOP SIGN
		case REM_COMMNAND_BUTTON8:
			if (cv.value == 0)
			{
				motor_stop(&m1); // show
			}
			else
			{
				motor_runForwardsPercent(&m1, 100);
			}
			break;

		case REM_COMMNAND_BUTTON11:
			if (cv.value == 0)
			{
				motor_stop(&m1); // hide
			}
			else
			{
				motor_runBackwardsPercent(&m1, 100);
			}
			break;

			// DOORS
		case REM_COMMNAND_BUTTON7:
			if (cv.value == 0)
			{
				motor_stop(&m2); // up
			}
			else
			{
				motor_runForwardsPercent(&m2, 100);
			}
			break;

		case REM_COMMNAND_BUTTON10:
			if (cv.value == 0)
			{
				motor_stop(&m2); // down
			}
			else
			{
				motor_runBackwardsPercent(&m2, 100);
			}
			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_SWITCH9:
			gpioPin_write(&f4mini.light1, cv.value == 0 ? Bit_RESET : Bit_SET); //front
			break;

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

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

		case REM_COMMNAND_SWITCH7:
			beepOn = cv.value == 0 ? FALSE : TRUE;
			break;
	}
}

// SET GEAR
void setGear(uint8_t gear)
{
	gearLCD[6] = '0' + gear;
	xbeeRx_sendData(&f4mini.xbee, 3, &gearLCD[0], 8);

	switch (gear)
	{
		case 0:
			pwmServo_setPosition(&f4mini.servo3, GEAR_LEFT_MIDDLE);
			pwmServo_setPosition(&f4mini.servo4, GEAR_RIGHT_MIDDLE);
			break;
		case 1:
			pwmServo_setPosition(&f4mini.servo3, GEAR_LEFT_MIDDLE);
			pwmServo_setPosition(&f4mini.servo4, GEAR_RIGHT_MIN);
			break;
		case 2:
			pwmServo_setPosition(&f4mini.servo3, GEAR_LEFT_MIDDLE);
			pwmServo_setPosition(&f4mini.servo4, GEAR_RIGHT_MAX);
			break;
		case 3:
			pwmServo_setPosition(&f4mini.servo3, GEAR_LEFT_MAX);
			pwmServo_setPosition(&f4mini.servo4, GEAR_RIGHT_MIDDLE);
			break;
		case 4:
			pwmServo_setPosition(&f4mini.servo3, GEAR_LEFT_MIN);
			pwmServo_setPosition(&f4mini.servo4, GEAR_RIGHT_MIDDLE);
			break;
	}
}

// INIT
void initializePeripherals()
{
	motor_initialize(&m1, GPIOC, GPIO_Pin_4, GPIO_Pin_5);
	motor_initialize(&m2, GPIOC, GPIO_Pin_1, GPIO_Pin_2);

	pwm_initializePin(&tone, TIM2, 1); // A0
	pwmTone_beepInitialize(&toneBeep, &tone);

	clearPeripherals();
}

// CLEAR
void clearPeripherals()
{
	motor_stop(&m1);
	motor_stop(&m2);

	beepOn = FALSE;
	beepState = 0;
	hornOn = FALSE;
	hornState = 0;

	// Beep
	pwmTone_hornStart(&tone);
	systemTime_delayMs(500);
	pwmTone_hornStop(&tone);
}

// TONE
void toneLoop(void)
{
	// BEEP - HIGH PRIO
	if (beepOn == TRUE)
	{
		if (beepState == 0)
		{
			pwmTone_beepStart(&toneBeep);
			beepState = 1;
		}

		if (beepState == 1)
		{
			// BEEP TONE INTERNAL LOOP
			pwmTone_beepPlay(&toneBeep);
		}
	}
	else
	{
		if (beepState == 1)
		{
			pwmTone_beepStop(&toneBeep);
			beepState = 0;
		}
	}

	// HORN
	if (hornState == 0 && hornOn == TRUE)
	{
		// on
		pwmTone_hornStart(&tone);
		hornState = 1;
		return;
	}

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