/**
* Domino bot that tracks line and builds domino pieces
*
* 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..A3 (B0 B1 | C0 C1 | C2 C3 | C4 C5)
libMotor drive, steer, barrier, belt;
beltUnitStruct beltUnit;
barrierUnitStruct barrierUnit;
steerStruct steerUnit;
driveUnitStruct driveUnit;

// Servos - TIM3 remap - C6..C9
libServo lever;			// C6
libPWMpin leverPin;

// - TIM4 - B6..B9
libLegoPFServo selector;  // B6 B7

// Analog A4..A6
volatile uint16_t analogValues[ANALOG_INPUTS];
libAnalogFilter af;
libAnalog analogInputs[ANALOG_INPUTS];

// Debug - B10..B13 B10 LSB, B13 MSB
libPin db1, db2, db3, db4;

// Line reader
BitAction lineLeft, lineCenter, lineRight;

// Internal
selectorUnitStruct selectorUnit;

/**
* Main method
*/
int main(void)
{
	// Initialize
	system_initialize(3);
	systemTime_initialize();

	initializePeriherals();
	clearPeriherals();

	// Run
	while (1)
	{
		// Debug
		//checkDebug();

		// Analog inputs
		if (analogFilter_isReady(&af) == TRUE)
		{
			lineLeft = parseLineValue(analogFilter_getValue(&af, 0));
			lineCenter = parseLineValue(analogFilter_getValue(&af, 1));
			lineRight = parseLineValue(analogFilter_getValue(&af, 2));

			analogFilter_clear(&af);
		}

		// Button B10 - complete one domino piece
		if (gpioPin_read(&db1) == Bit_SET)
		{
			if (selectorUnit.mode == 0 && beltUnit.mode == 0 && barrierUnit.mode == 0 && driveUnit.mode == 0 && steerUnit.mode == 0)
			{
				selectorUnit.mode = 1; // set move command
			}
		}

		// Loops
		selectorLoop();
		beltLoop();
		barrierLoop();
		driveLoop();
		steerLoop();

		// debug
		selectorDebug();
	}
}

/**
* Initialize perihperals
*/
void initializePeriherals(void)
{
	// Analog
	gpio_initializeInputAnalogPin(GPIOA, GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6);
	system_inicializeADC(4, 6, (uint32_t)&analogValues); // A4 + A6 - 4th to 6th channel
	analogFilter_initialize(&af, ANALOG_INPUTS, &analogValues[0]);
	analog_initialize(&analogInputs[0], &af, 0);
	analogCollection_addLibAnalog(&analogInputs[0]);
	analog_initialize(&analogInputs[1], &af, 1);
	analogCollection_addLibAnalog(&analogInputs[1]);
	analog_initialize(&analogInputs[2], &af, 2);
	analogCollection_addLibAnalog(&analogInputs[2]);

	// Motor
	pwm_initializeTimer2();
	motor_initializePWM(&drive, GPIOB, GPIO_Pin_0, GPIO_Pin_1, TIM2, 1); 	// A0
	motor_initializePWM(&steer, GPIOC, GPIO_Pin_0, GPIO_Pin_1, TIM2, 2);  // A1
	motor_initializePWM(&barrier, GPIOC, GPIO_Pin_2, GPIO_Pin_3, TIM2, 3);// A2
	motor_initializePWM(&belt, GPIOC, GPIO_Pin_4, GPIO_Pin_5, TIM2, 4);   // A3

	// Servos
	pwm_initializeTimer3Remap();
	pwm_initializePin(&leverPin, TIM3, 1); // C6
	pwmServo_inizializeSimple(&lever, &leverPin);

	pwm_initializeTimer4();
	pwm_setPeriodFrequency(TIM4, PWM_LEGO_PF_PERIOD, PWM_SERVO_LEGO_PF_PERIOD); // 1.2 KHz
	legoPFServo_inizializeSimple(&selector, TIM4, 1, 2); // B6 B7

	// Debug
	gpioPin_initialize(&db1, GPIOB, GPIO_Pin_10, libPin_INPUT);
	gpioPin_initialize(&db2, GPIOB, GPIO_Pin_11, libPin_INPUT);
	gpioPin_initialize(&db3, GPIOB, GPIO_Pin_12, libPin_INPUT);
	gpioPin_initialize(&db4, GPIOB, GPIO_Pin_13, libPin_INPUT);
}

/**
* Clear perihperals
*/
void clearPeriherals(void)
{
	// Morors
	motor_stop(&drive);
	motor_stop(&steer);
	motor_stop(&belt);
	motor_stop(&barrier);

	// Servos
	pwmServo_setPosition(&lever, LEVER_OUT);
	legoPFServo_setPosition(&selector, SELECTOR_1);

	// Selector unit
	selectorUnit.mode = 0;
	selectorUnit.direction = 0;
	selectorUnit.selected = 1;

	beltUnit.mode = 0;
	barrierUnit.mode = 0;
	steerUnit.mode = 0;
	steerUnit.lastDir = 0;
	driveUnit.mode = 0;
}

/**
* Parse line value
*/
BitAction parseLineValue(uint16_t value)
{
	if (value > LINE_TRESHOLD)
	{
		return Bit_SET;
	}

	return Bit_RESET;
}

/**
* Steer loop
*/
void steerLoop(void)
{
	bool applied = FALSE;
	systemTime now = systemTime_getTime();

	// set dir due to values
	if (lineLeft == Bit_SET) steerUnit.dir = 1; // left
	if (lineRight == Bit_SET) steerUnit.dir = 2; // right
	if (lineCenter == Bit_SET) steerUnit.dir = 0; // keep

	// check invalid states
	if (lineLeft == Bit_SET == Bit_SET && lineRight == Bit_SET) steerUnit.dir = 0;

	// Start condition
	if (steerUnit.mode == 0 && driveUnit.mode == 1)
	{
		// if last left and not center, return back steer using 1 right cycle
		if (steerUnit.lastDir == 1 && steerUnit.dir == 0)
		{
			steerUnit.lastDir = 0;
			steerUnit.dir = 2;
			applied = TRUE;
		}

		// if last right and not center, return back steer using 1 left cycle
		if (steerUnit.lastDir == 2 && steerUnit.dir == 0)
		{
			steerUnit.lastDir = 0;
			steerUnit.dir = 1;
			applied = TRUE;
		}

		// if last left and now left, do nothing - clear dir
		if (steerUnit.lastDir == 1 && steerUnit.dir == 1){ steerUnit.dir = 0; applied = TRUE; }
		if (steerUnit.lastDir == 2 && steerUnit.dir == 2){ steerUnit.dir = 0; applied = TRUE; }

		// keep forwards, or initiate steering
		if (applied == FALSE)
		{
			// dir set, set last for the next cycle
			steerUnit.lastDir = steerUnit.dir;
		}

		// apply
		if (steerUnit.dir == 1)
		{
			motor_runForwardsPercent(&steer, STEER_SPEED); // Left
		}
		if (steerUnit.dir == 2)
		{
			motor_runBackwardsPercent(&steer, STEER_SPEED); // Right
		}

		steerUnit.mode = 1;
		steerUnit.offTime = systemTime_build(now.s, now.ms + STEER_TIME);
	}

	// check if make change
	if (steerUnit.mode == 0 || systemTime_compareTimes(now, steerUnit.offTime) == -1)
	{
		return;
	}

	// change mode
	switch (steerUnit.mode)
	{
		// moving -> waiting
		case 1:
			motor_stop(&steer);
			steerUnit.mode = 2;
			steerUnit.offTime = systemTime_build(now.s, now.ms + STEER_AVAILABLE_TIME);
			break;

			// available
		case 2:
			steerUnit.mode = 0;
			steerUnit.dir = 0;
			break;
	}
}

/**
* Drive loop
*/
void driveLoop(void)
{
	systemTime now = systemTime_getTime();

	// start condition
	if (driveUnit.mode == 0 && barrierUnit.mode == 3)
	{
		driveUnit.mode = 1;
		motor_runForwardsPercent(&drive, DRIVE_SPEED);
		driveUnit.offTime = systemTime_build(now.s, now.ms + DRIVE_TIME);
	}

	// check if make change
	if (driveUnit.mode == 0 || systemTime_compareTimes(now, driveUnit.offTime) == -1)
	{
		return;
	}

	// decide
	switch (driveUnit.mode)
	{
		// stop
		case 1:
			motor_stop(&drive);
			driveUnit.mode = 0;
			break;
	}
}

/**
* Belt loop
*/
void beltLoop(void)
{
	systemTime now = systemTime_getTime();

	// ON
	if (beltUnit.mode == 0 && selectorUnit.mode > 0 && barrierUnit.mode == 0)
	{
		motor_runForwardsPercent(&belt, BELT_SPEED);

		beltUnit.offTime = systemTime_build(now.s, now.ms + BELT_MOVE_TIME);
		beltUnit.mode = 1;
		return;
	}

	// OFF
	if (beltUnit.mode == 1 && systemTime_compareTimes(now, beltUnit.offTime) >= 0)
	{
		motor_stop(&belt);
		beltUnit.mode = 0;
	}
}

/**
* Barrier loop
*/
void barrierLoop(void)
{
	systemTime now = systemTime_getTime();

	// check start
	if (beltUnit.mode > 0 && barrierUnit.mode == 0)
	{
		barrierUnit.mode = 1;
		barrierUnit.nextStepTime = systemTime_build(now.s, now.ms + BARRIER_MOVE_AFTER_BELT);
		return;
	}

	// check if make change
	if (barrierUnit.mode == 0 || systemTime_compareTimes(now, barrierUnit.nextStepTime) == -1)
	{
		return;
	}

	// change mode
	switch (barrierUnit.mode)
	{
		// waiting -> move UP
		case 1:
			motor_runBackwardsPercent(&barrier, BARRIER_SPEED);
			barrierUnit.mode = 2;
			barrierUnit.nextStepTime = systemTime_build(now.s, now.ms + BARRIER_UP_TIME);
			break;

			// wait up
		case 2:
			motor_stop(&barrier);
			barrierUnit.mode = 3;
			barrierUnit.nextStepTime = systemTime_build(now.s, now.ms + BARRIER_KEEP_UP_TIME);
			break;

			// up -> down
		case 3:
			motor_runForwardsPercent(&barrier, BARRIER_SPEED);
			barrierUnit.mode = 4;
			barrierUnit.nextStepTime = systemTime_build(now.s, now.ms + BARRIER_DOWN);
			break;

			// stop
		case 4:
			motor_stop(&barrier);
			barrierUnit.mode = 0;
			break;
	}
}

/**
* Selector loop
*/
void selectorLoop(void)
{
	systemTime now = systemTime_getTime();

	// check time
	if (selectorUnit.mode != 1 &&
		(selectorUnit.mode == 0 || systemTime_compareTimes(now, selectorUnit.nextStepTime) == -1))
	{
		return;
	}

	// change mode
	switch (selectorUnit.mode)
	{
		// 1->2 move lever IN
		case 1:
			pwmServo_setPosition(&lever, LEVER_IN);

			selectorUnit.mode = 2;
			selectorUnit.nextStepTime = systemTime_build(now.s, now.ms + LEVER_MOVE_TIME);
			break;

			// 2->3 move lever OUT
		case 2:
			pwmServo_setPosition(&lever, LEVER_OUT);

			selectorUnit.mode = 3;
			selectorUnit.nextStepTime = systemTime_build(now.s, now.ms + LEVER_MOVE_TIME);
			break;

			// 3->4 move selector
		case 3:
			// NEXT POSITION SELECTION
			if (selectorUnit.direction == 0)
			{
				// INC
				selectorUnit.selected++;
				if (selectorUnit.selected > 3)
				{
					// reverse, but keep current position
					selectorUnit.selected = 3;
					selectorUnit.direction = 1;
				}
			}
			else{
				// DEC
				selectorUnit.selected--;
				if (selectorUnit.selected == 0)
				{
					// reverse, but keep current position
					selectorUnit.selected = 1;
					selectorUnit.direction = 0;
				}
			}

			// APPLY
			if (selectorUnit.selected == 1){ legoPFServo_setPosition(&selector, SELECTOR_1); }
			if (selectorUnit.selected == 2){ legoPFServo_setPosition(&selector, SELECTOR_2); }
			if (selectorUnit.selected == 3){ legoPFServo_setPosition(&selector, SELECTOR_3); }

			selectorUnit.mode = 4;
			selectorUnit.nextStepTime = systemTime_build(now.s, now.ms + SELECTOR_MOVE_TIME);
			break;

			// 4->0 make available
		case 4:
			selectorUnit.mode = 0;
			break;
	}
}

/**
* Check debug
*/
void checkDebug(void)
{
	BitAction a4, a3, a2, a1;
	uint8_t choice;
	a4 = gpioPin_read(&db4);
	a3 = gpioPin_read(&db3);
	a2 = gpioPin_read(&db2);
	a1 = gpioPin_read(&db1);

	choice = (a4 << 3) | (a3 << 2) | (a2 << 1) | a1;

	switch (choice)
	{
		// stop
		case 0x0:
			motor_stop(&belt);
			motor_stop(&steer);
			motor_stop(&barrier);
			motor_stop(&drive);
			break;

			// selector +=2
		case 0x1:
			legoPFServo_setPosition(&selector, selector.servo.value + 2);
			break;

			// selector -=2
		case 0x2:
			legoPFServo_setPosition(&selector, selector.servo.value - 2);
			break;

			// lever +=2
		case 0x3:
			pwmServo_setPosition(&lever, lever.servo.value + 2);
			break;

			// lever -=2
		case 0x4:
			pwmServo_setPosition(&lever, lever.servo.value - 2);
			break;

			// belt start
		case 0x5:
			motor_runForwardsPercent(&belt, BELT_SPEED);
			break;

			// barrier DOWN
		case 0x6:
			motor_runForwardsPercent(&barrier, BARRIER_SPEED);
			break;

			// barrier UP
		case 0x7:
			motor_runBackwardsPercent(&barrier, BARRIER_SPEED);
			break;

			// drive
		case 0x8:
			motor_runForwardsPercent(&drive, DRIVE_SPEED);
			break;

			// steer left
		case 0x9:
			motor_runForwardsPercent(&steer, STEER_SPEED);
			break;

			// steer right
		case 0xA:
			motor_runBackwardsPercent(&steer, STEER_SPEED);
			break;
	}

	systemTime_delayMs(125); // only in debug phase ! - prevent to much changes
}

/**
* Selector debug
*/
void selectorDebug(void)
{
	BitAction a4, a3, a2;
	a4 = gpioPin_read(&db4);
	a3 = gpioPin_read(&db3);
	a2 = gpioPin_read(&db2);

	if (a2 == Bit_SET)
	{
		legoPFServo_setPosition(&selector, SELECTOR_1);
		systemTime_delayMs(500);
	}

	if (a3 == Bit_SET)
	{
		legoPFServo_setPosition(&selector, SELECTOR_2);
		systemTime_delayMs(500);
	}

	if (a4 == Bit_SET)
	{
		legoPFServo_setPosition(&selector, SELECTOR_3);
		systemTime_delayMs(500);
	}
}