/**
 * Knuckle arm truck
 *
 * 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.
 */

#include "stm32f30x.h"

#include "libInit.c"
#include "libUtils.c"
#include "libTime.c"
#include "libGPIO.c"
#include "libSwitch.c"
#include "libPWM.c"
#include "libUSART.c"
#include "libAnalog.c"
#include "libMotor.c"
#include "libXbee.c"
#include "libRemote.c"

// Headers
void processCommand(libRemoteCommandValue* commandValue);

// Constants
#define FRONT_LIGHT_NORMAL 40 	// percent
#define FRONT_LIGHT_MAX 	 100	// percent

#define WINKER_LIGHT_PERIOD 500	// ms

#define PISTON_SPEED 90 // percent
#define ARM_SPEED 	 75 // percent
#define MIN_MOTOR_SPEED (MOTOR_SPEED_RANGE >> 3 ) // divide by 8
#define ARM_HALF_BLIND_SPOT (ANALOG_VALUE_RANGE >> 2 ) // divide by 4; for each side

// Xbee
libXbee xbee;
u8 command[2];
libRemoteCommandValue cv;

// Peripheries
libMotor drive; // fw bw
libMotor largePiston, smallPiston;
libMotor arm; // rotate

libServo steer;
libServo pliers;

libPin backL, backR;
libPin backBrightL, backBrightR;
libPin backWinkerL, backWinkerR;
libPWMpin frontL, frontR;

bool backWinkerLOn, backWinkerROn;
systemTime lastWinkerTime;
BitAction lastWinkerState;

/**
* Main function
*/
int main(void)
{
    systemTime st;

    // Initialization
    initialize(6);
    initializeClock();

    // - xbee
    initializeUSART1atGPIOA();
    xbee = initializeXBee(USART1, FALSE);

    // - timers for pwm
    initializePWMTimer2();
    initializePWMTimer3();

    // - motors
    drive = initializeMotorPWM(GPIOA, GPIO_Pin_4, GPIO_Pin_5, TIM2, 1);
    arm = initializeMotorPWM(GPIOA, GPIO_Pin_6, GPIO_Pin_7, TIM2, 2);
    largePiston = initializeMotorPWM(GPIOB, GPIO_Pin_0, GPIO_Pin_1, TIM2, 3);
    smallPiston = initializeMotorPWM(GPIOB, GPIO_Pin_4, GPIO_Pin_5, TIM2, 4);

    // - servos
    steer = initializeServo(TIM3, 1);
    pliers = initializeServo(TIM3, 2);

    // - diodes
    backWinkerL = initializePin(GPIOB, GPIO_Pin_6, libPin_OUTPUT);
    backWinkerR = initializePin(GPIOB, GPIO_Pin_7, libPin_OUTPUT);
    backL = initializePin(GPIOB, GPIO_Pin_8, libPin_OUTPUT);
    backR = initializePin(GPIOB, GPIO_Pin_9, libPin_OUTPUT);
    backBrightL = initializePin(GPIOB, GPIO_Pin_10, libPin_OUTPUT);
    backBrightR = initializePin(GPIOB, GPIO_Pin_11, libPin_OUTPUT);
    frontL = initializePWMpin(TIM3, 3);
    frontR = initializePWMpin(TIM3, 4);

    // Default state
    stopMotor(&drive);
    stopMotor(&arm);
    stopMotor(&largePiston);
    stopMotor(&smallPiston);
    setServoPosition(&steer, 90);
    setServoPosition(&pliers, 90);
    writePin(&backL, Bit_RESET);
    writePin(&backR, Bit_RESET);
    writePin(&backBrightL, Bit_RESET);
    writePin(&backBrightR, Bit_RESET);
    writePin(&backWinkerL, Bit_RESET);
    writePin(&backWinkerR, Bit_RESET);
    backWinkerLOn = FALSE;
    backWinkerROn = FALSE;
    lastWinkerState = Bit_RESET;
    lastWinkerTime = getClock();
    writePWMpin(&frontL, 0);
    writePWMpin(&frontR, 0);

    // Main loop
    while (1)
    {
        st = getClock();

        if (isXbeeConnected(&xbee) == FALSE)
        {
            // blink front lights
            if (st.ms < 500)
            {
                if (getPWMpinValuePercent(&frontL) <= 10)
                {
                    writePWMpinPercent(&frontL, 100);
                    writePWMpinPercent(&frontR, 100);
                }
            } else {
                if (getPWMpinValuePercent(&frontL) >= 90)
                {
                    writePWMpinPercent(&frontL, 0);
                    writePWMpinPercent(&frontR, 0);
                }
            }

            continue;
        }

        // CONNECTED
        if (hasXbeeRecievedCommand(&xbee) == TRUE)
        {
            getXbeeCommand(&xbee, command);
            cv = parseRemoteRecievedCommandData(command);
            processCommand(&cv);
        }

        // - winkers
        if ((backWinkerLOn == TRUE || backWinkerROn == TRUE) && getClockDiffInMs(lastWinkerTime, st) >= WINKER_LIGHT_PERIOD)
        {
            lastWinkerState = (lastWinkerState == Bit_SET ? Bit_RESET : Bit_SET); // negate value
            if (backWinkerLOn == TRUE) {
                writePin(&backWinkerL, lastWinkerState);
            }
            if (backWinkerROn == TRUE) {
                writePin(&backWinkerR, lastWinkerState);
            }
            lastWinkerTime = st;
        }
    }
}

/**
* Process command with value
*/
void processCommand(libRemoteCommandValue* commandValue)
{
    u16 v = commandValue->value;

    switch (commandValue->command)
    {
        // Front light normal
        case REM_COMMNAND_SWITCH2:
            writePWMpinPercent(&frontL, v == 0 ? 0 : FRONT_LIGHT_NORMAL);
            writePWMpinPercent(&frontR, v == 0 ? 0 : FRONT_LIGHT_NORMAL);
            break;

        // Front light bright
        case REM_COMMNAND_SWITCH4:
            writePWMpinPercent(&frontL, v == 0 ? 0 : FRONT_LIGHT_MAX);
            writePWMpinPercent(&frontR, v == 0 ? 0 : FRONT_LIGHT_MAX);
            break;

        // Back red
        case REM_COMMNAND_SWITCH3:
            writePin(&backL, v == 0 ? Bit_RESET : Bit_SET);
            writePin(&backR, v == 0 ? Bit_RESET : Bit_SET);
            break;

        // Back bright red
        case REM_COMMNAND_SWITCH5:
            writePin(&backBrightL, v == 0 ? Bit_RESET : Bit_SET);
            writePin(&backBrightR, v == 0 ? Bit_RESET : Bit_SET);
            break;

        // Back Winker L
        case REM_COMMNAND_SWITCH1:
            backWinkerLOn = (v == 0 ? FALSE : TRUE);
            if (v == 0) {
                writePin(&backWinkerL, Bit_RESET);
            }
            lastWinkerState = Bit_RESET;
            break;

        // Back Winker R
        case REM_COMMNAND_SWITCH6:
            backWinkerROn = (v == 0 ? FALSE : TRUE);
            if (v == 0) {
                writePin(&backWinkerR, Bit_RESET);
            }
            lastWinkerState = Bit_RESET;
            break;

        // Steering
        case REM_COMMNAND_TRIMMER6:
            setServoPosition(&steer, 180 - map(v, 0, ANALOG_RESOLUTION, 0, 180));
            break;

        // Pliers
        case REM_COMMNAND_TRIMMER5:
            setServoPosition(&pliers, map(v, 0, ANALOG_RESOLUTION, 0, 180));
            break;

        // Small piston UP
        case REM_COMMNAND_BUTTON1:
            if (v == 0) {
                stopMotor(&smallPiston);
            }
            else {
                runMotorForwardsPercent(&smallPiston, PISTON_SPEED);
            }
            break;

        // Small piston DOWN
        case REM_COMMNAND_BUTTON2:
            if (v == 0) {
                stopMotor(&smallPiston);
            }
            else {
                runMotorBackwardsPercent(&smallPiston, PISTON_SPEED);
            }
            break;

        // Large piston UP
        case REM_COMMNAND_BUTTON3:
            if (v == 0) {
                stopMotor(&largePiston);
            }
            else {
                runMotorForwardsPercent(&largePiston, PISTON_SPEED);
            }
            break;

        // Large piston DOWN
        case REM_COMMNAND_BUTTON4:
            if (v == 0) {
                stopMotor(&largePiston);
            }
            else {
                runMotorBackwardsPercent(&largePiston, PISTON_SPEED);
            }
            break;

        // Drive
        case REM_COMMNAND_TRIMMER1:
            if (v == ANALOG_MIDDLE_POSITION) {
                stopMotor(&drive);
            }
            else if (v > ANALOG_MIDDLE_POSITION) {
                runMotorForwardsSpeed(&drive, map(v, ANALOG_MIDDLE_POSITION, ANALOG_MAX_POSITION, MIN_MOTOR_SPEED, MOTOR_SPEED_RANGE));
            }
            else {
                runMotorBackwardsSpeed(&drive, MOTOR_SPEED_RANGE - map(v, 0, ANALOG_MIDDLE_POSITION, 0, MOTOR_SPEED_RANGE - MIN_MOTOR_SPEED));
            }
            break;

        // Arm
        case REM_COMMNAND_TRIMMER2:
            if (v >= ANALOG_MIDDLE_POSITION - ARM_HALF_BLIND_SPOT && v <= ANALOG_MIDDLE_POSITION + ARM_HALF_BLIND_SPOT) {
                stopMotor(&arm);
            }
            else if (v >= ANALOG_MIDDLE_POSITION) {
                runMotorForwardsPercent(&arm, ARM_SPEED);
            }
            else {
                runMotorBackwardsPercent(&arm, ARM_SPEED);
            }
            break;
    }
}