/**
* Pencil sharpening factory
*
* Author: Jan Dvořák z Vozerovic
* E-mail: dvorkaman@gmail.com
* Web: dvorkaman.asp2.cz
* Created: 2016
*/

/* 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 2: BELT
MOTOR 3: ? SHARPENING
MOTOR 4: ? SHARP FW

SERVO 1: BELT TILT
SERVO 2: TILT OUT
SERVO 3: SPIKE
*/

libF4Mini f4mini;
bool manualMode;

/*
1..10 belt move
11..19 put away
21..29 put next pencil in
31..39 sharpening process
41..49 sharpenning finish
*/
uint8_t state;
systemTime nextStepTime;

// MAIN
int main(void)
{
	bool connected = FALSE;
	systemTime 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)

		io_adc, 				// 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_OUTPUT,	// C1
		libPin_OUTPUT,	// C2
		libPin_OUTPUT,	// C4
		libPin_OUTPUT,	// C5
		libPin_OUTPUT		// C9
		);

	initializePeripherals();

	// update servo width
	pwmServo_setBounds(&f4mini.servo1, 600, 2400);
	pwmServo_setBounds(&f4mini.servo2, 600, 2400);
	pwmServo_setBounds(&f4mini.servo3, 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);
				clearPeripherals();
			}

			// Connected
			now = systemTime_getTime();

			// - belt stop functionality
			if (state > 0 && systemTime_compareTimes(now, nextStepTime) >= 0)
			{
				stateLoop();
			}

			pwmServo_loop(&f4mini.servo2);
		}
	}
}

// PROCESS COMMAND
void processCommand(libRemoteCommandValue cv)
{
	switch (cv.command)
	{
		// MANUAL
		/*
		case REM_COMMNAND_SWITCH1: // manual mode
		manualMode = cv.value > 0 ? TRUE : FALSE;
		break;

		case REM_COMMNAND_ANALOG3: // belt tilt
		if(manualMode==TRUE) pwmServo_setPositionByAnalogValue(&f4mini.servo1, cv.value, IN_DOWN, IN_UP);
		break;

		case REM_COMMNAND_ANALOG4: // out tilt
		if(manualMode==TRUE) pwmServo_setPositionByAnalogValue(&f4mini.servo2, cv.value, OUT_DOWN, OUT_UP);
		break;

		case REM_COMMNAND_ANALOG5: // spike
		if(manualMode==TRUE) pwmServo_setPositionByAnalogValue(&f4mini.servo3, cv.value, SPIKE_DOWN, SPIKE_UP);
		break;
		*/

		// - semi manual
		case REM_COMMNAND_BUTTON13: // Sharp IN - FW
			if (cv.value == 0)
			{
				motor_stop(&f4mini.motor4);
			}
			else
			{
				motor_runForwardsPercent(&f4mini.motor4, SHARPEN_FW_SPEED);
			}
			break;

		case REM_COMMNAND_BUTTON14: // Sharp OUT - BW
			if (cv.value == 0)
			{
				motor_stop(&f4mini.motor4);
			}
			else
			{
				motor_runBackwardsPercent(&f4mini.motor4, SHARPEN_FW_SPEED);
			}
			break;

			// BELT
		case REM_COMMNAND_BUTTON1: // insert next
			if (cv.value == 1)
			{
				state = 21;
			}
			break;

		case REM_COMMNAND_BUTTON2: // 1 belt turn
			if (cv.value == 1)
			{
				motor_runForwardsPercent(&f4mini.motor2, BELT_SPEED);
				state = 1;
				nextStepTime = systemTime_AddMs(systemTime_getTime(), BELT_TIME);
			}
			break;

		case REM_COMMNAND_BUTTON11: // belt manual
			if (cv.value == 0)
			{
				motor_stop(&f4mini.motor2);
			}
			else
			{
				motor_runForwardsPercent(&f4mini.motor2, BELT_SPEED);
			}
			break;

			// BELT TILT
		case REM_COMMNAND_SWITCH11: // MID - UP
			if (cv.value == 1)
			{
				pwmServo_setPosition(&f4mini.servo1, IN_UP);
			}
			else
			{
				pwmServo_setPosition(&f4mini.servo1, IN_MID);
			}
			break;

		case REM_COMMNAND_SWITCH12: // DOWN - MID
			if (cv.value == 1)
			{
				pwmServo_setPosition(&f4mini.servo1, IN_DOWN);
			}
			else
			{
				pwmServo_setPosition(&f4mini.servo1, IN_MID);
			}
			break;

			// SHARPENING		
		case REM_COMMNAND_BUTTON4: // Sharp ON
			if (cv.value == 1)
			{
				motor_runBackwardsPercent(&f4mini.motor3, SHARPEN_SPEED);
				state = 31;
			}
			break;

		case REM_COMMNAND_BUTTON7: // Sharp OFF
			if (cv.value == 1)
			{
				motor_stop(&f4mini.motor3); // sharp
				motor_stop(&f4mini.motor4); // fw
				state = 41;
			}
			break;

		case REM_COMMNAND_BUTTON8: // Sharp ON			
			if (cv.value == 1) motor_runBackwardsPercent(&f4mini.motor3, SHARPEN_SPEED);
			break;

		case REM_COMMNAND_BUTTON9: // Sharp OFF
			if (cv.value == 1) motor_stop(&f4mini.motor3);
			break;

		case REM_COMMNAND_BUTTON5: // SPIKE DOWN
			if (cv.value == 1)
			{
				pwmServo_setPosition(&f4mini.servo3, SPIKE_DOWN);
			}
			break;

		case REM_COMMNAND_BUTTON6: // SPIKE UP
			if (cv.value == 1)
			{
				pwmServo_setPosition(&f4mini.servo3, SPIKE_UP);
			}
			break;

			// OUT
		case REM_COMMNAND_BUTTON3: // OUT TILT - up and down
			if (cv.value == 1)
			{
				pwmServo_moveToPosition(&f4mini.servo2, OUT_UP);
			}
			else
			{
				pwmServo_setPosition(&f4mini.servo2, OUT_DOWN);
			}
			break;

		case REM_COMMNAND_BUTTON10: // OUT TILT - up and down
			if (cv.value == 1)
			{
				state = 11;
			}
			break;

			// ANALOG_MAX_POSITION - 
			// pwmServo_setPositionByAnalogValue(&f4mini.servo1, ANALOG_MAX_POSITION - cv.value, STEER_MIN, STEER_MAX);
			// motor_runByAnalogValue(&f4mini.motor3, cv.value, DRIVE_MIN);		
	}
}

// INIT
void initializePeripherals()
{
	f4mini.servo2.servo.changeEachMs = 125;
	f4mini.servo2.servo.changeIncrement = 4;

	clearPeripherals();
}

// CLEAR
void clearPeripherals()
{
	manualMode = FALSE;
	state = FALSE;

	F4Mini_clearPerihperals(&f4mini);

	pwmServo_setPosition(&f4mini.servo1, IN_DOWN);
	pwmServo_setPosition(&f4mini.servo2, OUT_DOWN);
	pwmServo_setPosition(&f4mini.servo3, SPIKE_UP);
}

// STATE
void stateLoop(void)
{
	// - belt move
	if (state == 1){ motor_stop(&f4mini.motor2); state = 0; }

	// - out tilting
	if (state >= 11 && state <= 20)
	{
		switch (state)
		{
			case 11: // belt in move down
				pwmServo_setPosition(&f4mini.servo1, IN_DOWN); // belt int
				pwmServo_setPosition(&f4mini.servo3, SPIKE_UP); // spike
				nextStepTime = systemTime_AddMs(systemTime_getTime(), IN_TIME);
				state = 12;
				break;

			case 12: // put OUT up
				pwmServo_moveToPosition(&f4mini.servo2, OUT_UP);
				nextStepTime = systemTime_AddMs(systemTime_getTime(), OUT_TIME);
				state = 13;
				break;

			case 13: // put OUT down, leave belt in down, does not matter
				pwmServo_setPosition(&f4mini.servo2, OUT_DOWN);
				state = 0;
				break;
		}
	}

	// - insert into belt
	if (state >= 21 && state <= 30)
	{
		switch (state)
		{
			case 21: // put holder middle
				pwmServo_setPosition(&f4mini.servo1, IN_MID);
				nextStepTime = systemTime_AddMs(systemTime_getTime(), IN_TIME);
				state = 22;
				break;

			case 22: // move belt
				motor_runForwardsPercent(&f4mini.motor2, BELT_SPEED);
				state = 23;
				nextStepTime = systemTime_AddMs(systemTime_getTime(), BELT_TIME);
				break;

			case 23: // stop belt, wait a bit
				motor_stop(&f4mini.motor2);
				nextStepTime = systemTime_AddMs(systemTime_getTime(), BELT_FALL_TIME);
				state = 24;
				break;

			case 24: // move belt holder up
				pwmServo_setPosition(&f4mini.servo1, IN_UP);
				state = 0;
		}
	}

	// - starpening
	if (state >= 31 && state <= 50)
	{
		switch (state)
		{
			case 31: // move a bit closer
				motor_runForwardsPercent(&f4mini.motor4, SHARPEN_FW_SPEED);
				nextStepTime = systemTime_AddMs(systemTime_getTime(), SHARPEN_FW_TIME);
				state = 32;
				break;

			case 32: // wait a bit
				motor_stop(&f4mini.motor4);
				nextStepTime = systemTime_AddMs(systemTime_getTime(), SHARPEN_FW_EACH);
				state = 31;
				break;

				// - finishing
			case 41: // reverse sharpen
				motor_runForwardsPercent(&f4mini.motor3, SHARPEN_SPEED);
				nextStepTime = systemTime_AddMs(systemTime_getTime(), SHARPEN_BW_TIME);
				state = 42;
				break;

			case 42: // stop sharpen, move spike down
				motor_stop(&f4mini.motor3);
				state = 0;
				pwmServo_setPosition(&f4mini.servo3, SPIKE_DOWN);
				break;
		}
	}
}