/**
* Crawler Tractor
*
* 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 1
MOTOR 4: Drive 2

SERVO 2: lego pf servo
SERVO 3: lego pf servo

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

libF4Mini f4mini;

uint8_t distLine[REM_COMMAND_DATA_LENGTH] = { '|', '_', '|', '_', '|', '_', '|', '_', '|', '_', '|', '\0' };
libPWMDigital left, right; // winkers
libAnalogToDigital lineSensors[ANALOG_INPUTS];
BitAction lineInputs[ANALOG_INPUTS];
bool lineAutoSteerFlag;
libLegoPFServo steerServo;
libServo hitch;
libPWMpin hitchPin;

uint8_t lastSteerAutomaticAngle;

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

	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
		libPin_OUTPUT,	// A1
		libPin_OUTPUT,	// A2
		libPin_OUTPUT,	// A3

		libPin_OUTPUT,	// A8

		libPin_OUTPUT,	// B12
		libPin_OUTPUT,	// B13

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

	initializePeripherals();

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

			// Analog
			if (analogFilter_isReady(&f4mini.af) == TRUE)
			{
				for (i = 0; i < ANALOG_INPUTS; i++)
				{
					lineInputs[i] = analogToDigital_read(&lineSensors[i]);
				}

				analogFilter_clear(&f4mini.af);
			}

			// Info
			now = systemTime_getTime();
			if (systemTime_compareTimes(now, nextDistSent) >= 0)
			{
				for (i = 0; i < ANALOG_INPUTS; i++)
				{
					if (lineInputs[i] == Bit_SET)
					{
						distLine[(i * 2) + 1] = 'X';
					}
					else
					{
						distLine[(i * 2) + 1] = ' ';
					}
				}

				xbeeRx_sendData(&f4mini.xbee, 4, &distLine[0], REM_COMMAND_DATA_LENGTH);
				nextDistSent = systemTime_build(now.s, now.ms + SEND_SENSORS_EACH);
			}

			// Leds
			pwmDigital_loop(&left);
			pwmDigital_loop(&right);

			// Line
			if (systemTime_compareTimes(now, nextLineLogic) >= 0)
			{
				readLine();
				nextLineLogic = systemTime_build(now.s, now.ms + LINE_LOGIC_EACH);
			}
		}
	}
}

// PROCESS COMMAND
void processCommand(libRemoteCommandValue cv)
{
	uint8_t i;

	switch (cv.command)
	{
		// DRIVE
		case REM_COMMNAND_ANALOG1:
			motor_runByAnalogValue(&f4mini.motor3, cv.value, DRIVE_MIN);
			motor_runByAnalogValue(&f4mini.motor4, cv.value, DRIVE_MIN);
			break;

		case REM_COMMNAND_ANALOG8:
			if (lineAutoSteerFlag == TRUE){ return; }
			legoPFServo_setPositionByAnalogValue(&steerServo, cv.value, STEER_MIN, STEER_MAX);
			break;

			// HITCH
		case REM_COMMNAND_ANALOG3:
			pwmServo_setPositionByAnalogValue(&hitch, ANALOG_VALUE_RANGE - cv.value, HITCH_MIN, HITCH_MAX);
			break;

			// LIGHTS
		case REM_COMMNAND_BUTTON13:
			pwmDigital_setDutyMs(&left, cv.value > 0 ? LIGHT_PWM_PERIOD / 2 : 0);
			break;

		case REM_COMMNAND_BUTTON14:
			pwmDigital_setDutyMs(&right, cv.value > 0 ? LIGHT_PWM_PERIOD / 2 : 0);
			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.light2, cv.value == 0 ? Bit_RESET : Bit_SET); //rear
			break;

			// LINE DETECTION		
		case REM_COMMNAND_SWITCH11:
			lineAutoSteerFlag = cv.value > 0 ? TRUE : FALSE;
			if (cv.value == 0)
			{
				// turned off
				motor_stop(&f4mini.motor3);
				motor_stop(&f4mini.motor4);
			}
			break;

			// LINE LIMIT
		case REM_COMMNAND_ANALOG6:
			for (i = 0; i < ANALOG_INPUTS; i++)
			{
				lineSensors[i].threshold = cv.value;
			}

			break;
	}
}

// INIT
void initializePeripherals()
{
	uint8_t i;

	// Winkers
	pwmDigital_initialize(&left, &f4mini.light3, LIGHT_PWM_PERIOD);
	pwmDigital_initialize(&right, &f4mini.light4, LIGHT_PWM_PERIOD);

	// Line
	for (i = 0; i < ANALOG_INPUTS; i++)
	{
		analogToDigital_initializeAnalogFilter(&lineSensors[i], &f4mini.af, i);
	}

	// PF servo
	legoPFServo_inizializeSimple(&steerServo, TIM4, 2, 3);
	pwm_setPeriodFrequency(TIM4, PWM_LEGO_PF_PERIOD, PWM_SERVO_LEGO_PF_PERIOD);

	// hitch servo
	pwm_initializePin(&hitchPin, TIM2, 1); // A0
	pwmServo_inizialize(&hitch, &hitchPin);
	pwmServo_setBounds(&hitch, 600, 2400);

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

	clearPeripherals();
}

// CLEAR
void clearPeripherals()
{
	pwmDigital_setDutyMs(&left, 0);
	pwmDigital_setDutyMs(&right, 0);
	pwmServo_setPosition(&hitch, 90);

	lineAutoSteerFlag = FALSE;
	F4Mini_clearPerihperals(&f4mini);
	lastSteerAutomaticAngle = 90;
}

// READ LINE
void readLine(void)
{
	uint8_t i, count = 0, angle = 90;
	uint8_t step1 = 45, step2 = 90;

	if (lineAutoSteerFlag == FALSE){ return; }

	// Logic
	for (i = 0; i < ANALOG_INPUTS; i++)
	{
		if (lineInputs[i] == Bit_SET){ count++; }
	}

	if (count != 1){ return; } // to much sensors are one -> propably wrong measurement; OR no line detected

	// 0 1 | 2 | 3 4
	if (lineInputs[0] == Bit_SET && lineInputs[4] == Bit_RESET) { angle = 90 + step2; }
	if (lineInputs[4] == Bit_SET && lineInputs[0] == Bit_RESET) { angle = 90 - step2; }

	if (lineInputs[1] == Bit_SET && lineInputs[3] == Bit_RESET) { angle = 90 + step1; }
	if (lineInputs[3] == Bit_SET && lineInputs[1] == Bit_RESET) { angle = 90 - step1; }

	if (lineInputs[2] == Bit_SET) { angle = 90; }

	// set
	if (lastSteerAutomaticAngle != angle)
	{
		legoPFServo_setPosition(&steerServo, angle);
		lastSteerAutomaticAngle = angle;
	}
}