/**
* Tractor with accessories controller using XBee remote controll
*
* 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.
*/

// libraries
#include "stdio.h"

#define _define_bool_ 0x01

#define DEBUG 0

#define BLINK_INTERVAL 500
#define HYDRAULICS_SPEED 90 // %
#define PUMP_DIR_1 0  //° of 180
#define PUMP_DIR_2 180 //° of 180
#define STEER_MIN 4 //° of 180
#define STEER_MAX 176 //° of 180
#define ARMS_MIN 0 //° of 180
#define ARMS_MAX 180 //° of 180
#define MIN_SHAFT_SPEED 25 // %
#define MIN_MACHINE_SPEED 25 // %
#define MIN_DRIVE_SPEED 25 // %

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

// Motors
libMotor drive, hydraulics, shaft;
libMotor machine;
uint8_t shaftDir;
uint8_t machineDir;

// Servos
libServo steer, pump, arms;

// Lights
libPin frontLightL, frontLightsBlue;
libPin frontLightR, backLights;
libPin machineBackRed, machineBackBreak, machineBackLeft, machineBackRight;
bool machineLeftBlink = FALSE, machineRightBlink = FALSE;
systemTime machinePreviousBlinkTime = {0, 0};
BitAction machineBlinkState = Bit_RESET;

// Xbee
libXbeeRx xbeeRx; // Reciever logic

// headers
void initializePeripheries(void);
void setDefaultValues(void);
void processCommand(libRemoteCommandValue cv);
void debug(void);

/**
* Main method
*/
int main(void)
{
    libRemoteCommandValue cv;
    systemTime now, previousBlinkTime = {0, 0};
    BitAction blinkState = Bit_RESET;
    bool connected = FALSE;

    // init
    initialize(3);
    initializeClock();

    // peripheries
    initializePeripheries();
    setDefaultValues();

    while (1)
    {
        now = getClock();

        // DEBUG
        if (DEBUG == 1) { debug(); delayMs(125); continue; }

        // Xbee input
        if (hasXbeeRxRecievedCommand(&xbeeRx) == TRUE)
        {
            cv = getXbeeRxRecievedParsedCommand(&xbeeRx);
            processCommand(cv);
        }

        // Connection state
        if (xbeeRx.remoteXbee.state == REM_XBEE_NOT_CONNECTED || xbeeRx.remoteXbee.state == REM_XBEE_CONNECTING
                || xbeeRx.remoteXbee.active == FALSE)
        {
            // if disconnected
            if (connected == TRUE)
            {
                connected = FALSE;
                setDefaultValues();
            }

            // blink
            if (getClockDiffInMs(previousBlinkTime, now) >= BLINK_INTERVAL)
            {
                previousBlinkTime = now;
                writePin(&frontLightL, blinkState);
                writePin(&frontLightR, blinkState);
                blinkState = (BitAction)(1 - blinkState); // negate
                writePin(&frontLightsBlue, blinkState);
                writePin(&backLights, blinkState);
            }
        }
        else {
            connected = TRUE;
            if (blinkState == Bit_SET)
            {
                blinkState = Bit_RESET;
                writePin(&frontLightsBlue, blinkState);
                writePin(&backLights, blinkState);
                writePin(&frontLightL, blinkState);
                writePin(&frontLightR, blinkState);
            }
        }

        // Machine blink
        if (getClockDiffInMs(machinePreviousBlinkTime, now) >= BLINK_INTERVAL)
        {
            machinePreviousBlinkTime = now;
            machineBlinkState = (BitAction)(1 - machineBlinkState);
            if (machineLeftBlink == TRUE) { writePin(&machineBackLeft, machineBlinkState); }
            if (machineRightBlink == TRUE) { writePin(&machineBackRight, machineBlinkState); }
        }
    }
}

/**
* Initialize peripheries
*/
void initializePeripheries(void)
{
    // timers
    initializePWMRemapTimer3(); // C6 - C9
    initializePWMTimer4();        // B6 - B9

    // xbee
    initializeUSART1(); // A9 = TX, A10 = RX
    initializeXbeeRx(&xbeeRx, USART1, ROUTER, XBEE_API_COORD_MSB, XBEE_API_COORD_LSB);

    // Motors
    drive =          initializeMotorPWM(GPIOC, GPIO_Pin_0, GPIO_Pin_1, TIM3, 1); // + C6
    hydraulics = initializeMotorPWM(GPIOC, GPIO_Pin_2, GPIO_Pin_3, TIM3, 2); // + C7
    shaft =          initializeMotorPWM(GPIOC, GPIO_Pin_4, GPIO_Pin_5, TIM3, 3); // + C8

    machine =    initializeMotorPWM(GPIOC, GPIO_Pin_10, GPIO_Pin_11, TIM3, 4);// + C9

    // Servos
    arms =  initializeServo(TIM4, 1); // B6
    steer = initializeServo(TIM4, 2); // B7
    pump =  initializeServo(TIM4, 3); // B8

    // lights
    frontLightL = initializePin(GPIOA, GPIO_Pin_0, libPin_OUTPUT);
    frontLightR = initializePin(GPIOA, GPIO_Pin_1, libPin_OUTPUT);
    frontLightsBlue = initializePin(GPIOA, GPIO_Pin_2, libPin_OUTPUT);
    backLights = initializePin(GPIOA, GPIO_Pin_3, libPin_OUTPUT);

    machineBackRed = initializePin(GPIOA, GPIO_Pin_4, libPin_OUTPUT);
    machineBackBreak = initializePin(GPIOA, GPIO_Pin_5, libPin_OUTPUT);
    machineBackLeft = initializePin(GPIOA, GPIO_Pin_6, libPin_OUTPUT);
    machineBackRight = initializePin(GPIOA, GPIO_Pin_7, libPin_OUTPUT);

    // DEBUG inputs
    if (DEBUG == 1)
    {
        initializeInputPin(GPIOB, GPIO_Pin_10 | GPIO_Pin_11 | GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15);
    }
}

/**
* Set default values
*/
void setDefaultValues(void)
{
    // servos
    setServoPosition(&steer, 90);
    setServoPosition(&pump, 90);
    setServoPosition(&arms, 90);

    // motors
    stopMotor(&drive);
    stopMotor(&hydraulics);
    stopMotor(&shaft);
    shaftDir = 1;
    machineDir = 1;

    stopMotor(&machine);

    // lights
    writePin(&frontLightL, Bit_RESET);
    writePin(&frontLightR, Bit_RESET);
    writePin(&frontLightsBlue, Bit_RESET);
    writePin(&backLights, Bit_RESET);

    writePin(&machineBackRed, Bit_RESET);
    writePin(&machineBackBreak, Bit_RESET);
    writePin(&machineBackLeft, Bit_RESET);
    writePin(&machineBackRight, Bit_RESET);
}

/**
* Process command
*/
void processCommand(libRemoteCommandValue cv)
{
    switch (cv.command)
    {
        /* STEER */
        // - drive
        case REM_COMMNAND_ANALOG1:
            if (cv.value == ANALOG_MIDDLE_POSITION) { stopMotor(&drive); }
            else
                if (cv.value > ANALOG_MIDDLE_POSITION) { runMotorForwardsPercent(&drive, map(cv.value, ANALOG_MIDDLE_POSITION, ANALOG_MAX_POSITION, MIN_DRIVE_SPEED, 100)); }
                else { runMotorBackwardsPercent(&drive, 100 - map(cv.value, 0, ANALOG_MIDDLE_POSITION, MIN_DRIVE_SPEED, 100)); }
            break;

        // - stear
        case REM_COMMNAND_ANALOG4:
            setServoPosition(&steer, map(ANALOG_MAX_POSITION - cv.value, 0, ANALOG_MAX_POSITION, STEER_MIN, STEER_MAX));
            break;

        /* ACCESSORIES */
        // - arms
        case REM_COMMNAND_ANALOG5:
            setServoPosition(&arms, map(cv.value, 0, ANALOG_MAX_POSITION, ARMS_MIN, ARMS_MAX));
            break;

        // - shaft
        case REM_COMMNAND_ANALOG6:
            if (cv.value == 0) { stopMotor(&shaft); return; }
            if (shaftDir == 1) { runMotorForwardsPercent(&shaft, map(cv.value, 0, ANALOG_MAX_POSITION, MIN_SHAFT_SPEED, 100)); }
            else { runMotorBackwardsPercent(&shaft, map(cv.value, 0, ANALOG_MAX_POSITION, MIN_SHAFT_SPEED, 100)); }
            break;

        // - machine motor
        case REM_COMMNAND_ANALOG8:
            if (cv.value == 0) { stopMotor(&machine); return; }
            if (machineDir == 1) { runMotorForwardsPercent(&machine, map(cv.value, 0, ANALOG_MAX_POSITION, MIN_MACHINE_SPEED, 100)); }
            else { runMotorBackwardsPercent(&machine, map(cv.value, 0, ANALOG_MAX_POSITION, MIN_MACHINE_SPEED, 100)); }
            break;

        // - directions
        case REM_COMMNAND_BUTTON3: shaftDir = 1; break;
        case REM_COMMNAND_BUTTON4: shaftDir = 0; break;
        case REM_COMMNAND_BUTTON5: machineDir = 1; break;
        case REM_COMMNAND_BUTTON6: machineDir = 0; break;

        /* LIGHTS */
        // - front
        case REM_COMMNAND_SWITCH1:
            writePin(&frontLightL, (BitAction)cv.value);
            writePin(&frontLightR, (BitAction)cv.value);
            break;

        // - blue
        case REM_COMMNAND_SWITCH2:
            writePin(&frontLightsBlue, (BitAction)cv.value);
            break;

        // - back
        case REM_COMMNAND_SWITCH3:
            writePin(&backLights, (BitAction)cv.value);
            break;

        // - machine L
        case REM_COMMNAND_SWITCH4:
            writePin(&machineBackLeft, (BitAction)cv.value);
            machineLeftBlink = cv.value == 1 ? TRUE : FALSE;
            machinePreviousBlinkTime = getClock();
            break;

        // - machine back
        case REM_COMMNAND_SWITCH5:
            writePin(&machineBackRed, (BitAction)cv.value);
            break;

        // - machine break
        case REM_COMMNAND_BUTTON1:
            writePin(&machineBackBreak, (BitAction)cv.value);
            break;

        // - machine R
        case REM_COMMNAND_SWITCH6:
            writePin(&machineBackRight, (BitAction)cv.value);
            machineRightBlink = cv.value == 1 ? TRUE : FALSE;
            machinePreviousBlinkTime = getClock();
            break;

        /* PUMP */
        // - start
        case REM_COMMNAND_BUTTON7:
            if (cv.value == 1) { runMotorForwardsPercent(&hydraulics, HYDRAULICS_SPEED); }
            break;

        // - stop
        case REM_COMMNAND_BUTTON8:
            if (cv.value == 1) { stopMotor(&hydraulics); }
            break;

        // - dir 1
        case REM_COMMNAND_BUTTON9:
            if (cv.value == 1) { setServoPosition(&pump, PUMP_DIR_1); }
            break;

        // - dir 2
        case REM_COMMNAND_BUTTON10:
            if (cv.value == 1) { setServoPosition(&pump, PUMP_DIR_2); }
            break;
    }
}

/**
* Debugging
*/
void debug(void)
{
    u8 steerV, armV, pumpV;

    // STEER SERVO
    steerV = getServoPosition(&steer);
    if (readInputPin(GPIOB, GPIO_Pin_10)) { setServoPosition(&steer, steerV - 4); }
    if (readInputPin(GPIOB, GPIO_Pin_11)) { setServoPosition(&steer, steerV + 4); }

    // ARM SERVO
    armV = getServoPosition(&arms);
    if (readInputPin(GPIOB, GPIO_Pin_12)) { setServoPosition(&arms, armV - 4); }
    if (readInputPin(GPIOB, GPIO_Pin_13)) { setServoPosition(&arms, armV + 4); }

    // PUMP SERVO
    pumpV = getServoPosition(&pump);
    if (readInputPin(GPIOB, GPIO_Pin_14)) { setServoPosition(&pump, pumpV - 4); }
    if (readInputPin(GPIOB, GPIO_Pin_15)) { setServoPosition(&pump, pumpV + 4); }

    // - READ VALUES
    steerV += 0;
    armV += 0;
    pumpV += 0;
}