/**
* Fire truck with 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.
*/

/* Includes */
#include "main.h"

/* Global Variables */
// Motors - TIM2 - A0..A3 ; A3 unused ; A6 A7 ; B0 B1 ; C4 C5
libMotor drive, largePiston, smallPiston;

// Servos - TIM3 remap - C6..C9
libServo steer, pump, rotation, tilt;
libPWMpin steerPin, pumpPin, rotationPin, tiltPin;

// Lights B10..B14, C0..C2
libPin frontLight, frontLightBlue, rearLight, brakeLight;
libPin leftWinker, rightWinker, sirenLight;

// Sensors - A4, A5
libTemperature temperature;
libPhotoResistor photo;

// Siren - TIM4 - B6..B9 ;only B6 used
libPWMpin siren; // - pin, e.g. horn
libPWMbeep sirenBeep; // - reverse
libPWMsiren sirenFire; // - fire siren

// Xbee - USART1 - A8..A12 ; A9 TX + A10 RX
libXbeeRx xbeeRx; // Reciever logic

// Debug - C0..C3 C0 LSB, C3 MSB
libPin db1, db2, db3, db4;

// Helpers
// - winkers
bool leftWinkerBlink = FALSE, rightWinkerBlink = FALSE, sirenBlink = FALSE;
systemTime lightsPeviousBlinkTime = {0, 0};
BitAction lightsBlinkState = Bit_RESET;

// - analog
volatile uint16_t analogValues[ANALOG_INPUTS];
libAnalogFilter af;
libAnalog analogInputs[ANALOG_INPUTS];

uint8_t temperatureLine[REM_COMMAND_DATA_LENGTH] = {'T', 'e', 'm', 'p', ':', ' ', 'x', 'x', '\'', 'C', '|', 'F', 'i', 'r', 'e', ' '}; // Temp: xx°C
uint8_t luxLine[REM_COMMAND_DATA_LENGTH] =         {'L', 'u', 'x', ':', ' ', 'x', 'x', '%', ' ',  ' ', '|', 'T', 'r', 'u', 'c', 'k'}; // Lux: xx%

// - connection
bool connected = FALSE;

// - siren
bool sirenHornOn = FALSE, sirenBeepOn = FALSE, sirenFireOn = FALSE;
uint8_t sirenHornState = 0, sirenBeepState = 0, sirenFireState = 0; // 0 = uninitialized, 1 = playing, 2 = stopped

// - xbee
uint8_t xbeeDataType = 0; // 0 = temp, 1 = photo
systemTime xbeeDataPeviousTime = {0, 0};

/**
* Main method
*/
int main(void)
{
    // Variables
    libRemoteCommandValue cv;

    // Initialize
    system_initialize(3);
    systemTime_initialize();

    initializePeriherals();
    clearPeriherals();

    // Run
    while (1)
    {
        if (connected == FALSE) { showConnecting(); }
        // checkDebug();

        // Xbee input
        if (xbeeRx_hasRecievedCommand(&xbeeRx) == TRUE)
        {
            cv = xbeeRx_getRecievedParsedCommand(&xbeeRx);
            processCommand(cv);
        }

        // Connection state
        if (xbeeRx.remoteXbee.state == REM_XBEE_NOT_CONNECTED || xbeeRx.remoteXbee.state == REM_XBEE_CONNECTING
                || xbeeRx.remoteXbee.active == FALSE)
        {
            // Abort connected
            if (connected == TRUE)
            {
                clearPeriherals();
                connected = FALSE;
            }
        }
        else
        {
            // Set connected
            if (connected == FALSE)
            {
                clearPeriherals();
                connected = TRUE;
            }

            // Send status data
            sendStatusData();
        }

        // Peripherals state
        updatePeripheralsState();

        // Analog inputs
        if (analogFilter_isReady(&af) == TRUE)
        {
            temperature_read(&temperature);
            photoResistor_readPercent(&photo);

            analogFilter_clear(&af);
        }
    }
}

/**
* Update peripherals state
*/
void updatePeripheralsState(void)
{
    systemTime t = systemTime_getTime();

    if (connected == FALSE)
    {
        return;
    }

    // Winkers, Siren light
    if (systemTime_getTimeDiffInMs(lightsPeviousBlinkTime, t) >= BLINK_INTERVAL)
    {
        lightsPeviousBlinkTime = t;
        lightsBlinkState    = (BitAction)(1 - lightsBlinkState);

        if (leftWinkerBlink == TRUE) { gpioPin_write(&leftWinker, lightsBlinkState); }
        if (rightWinkerBlink == TRUE) { gpioPin_write(&rightWinker, lightsBlinkState); }
        if (sirenBlink == TRUE) { gpioPin_write(&sirenLight, lightsBlinkState); }
    }

    // Siren - by priority - SIREN, BEEP, HORN
    if (sirenFireState != 0 || sirenFireOn == TRUE)
    {
        processSirenFire();
    }
    else
        if (sirenBeepState != 0 || sirenBeepOn == TRUE)
        {
            processSirenBeep();
        }
        else {
            processSirenHorn();
        }
}

/**
* Siren sounds
*/
void processSirenHorn(void)
{
    if (sirenHornOn == TRUE)
    {
        if (sirenHornState == 0)
        {
            pwmTone_hornStart(&siren);
            sirenHornState = 1;
        }
    }
    else
    {
        if (sirenHornState == 1)
        {
            pwmTone_hornStop(&siren);
            sirenHornState = 0;
        }
    }
}

void processSirenBeep(void)
{
    if (sirenBeepOn == TRUE)
    {
        if (sirenBeepState == 0)
        {
            pwmTone_beepStart(&sirenBeep);
            sirenBeepState = 1;
        }

        if (sirenBeepState == 1)
        {
            pwmTone_beepPlay(&sirenBeep);
        }
    }
    else
    {
        if (sirenBeepState == 1)
        {
            pwmTone_beepStop(&sirenBeep);
            sirenBeepState = 0;
        }
    }
}

void processSirenFire(void)
{
    if (sirenFireOn == TRUE)
    {
        if (sirenFireState == 0)
        {
            pwmTone_sirenStart(&sirenFire);
            sirenFireState = 1;
        }

        if (sirenFireState == 1)
        {
            pwmTone_sirenPlay(&sirenFire);
        }
    }
    else
    {
        if (sirenFireState == 1)
        {
            pwmTone_sirenStop(&sirenFire);
            sirenFireState = 0;
        }
    }
}

/**
* Initialize perihperals
*/
void initializePeriherals(void)
{
    // Lights B10..B14, C0..C2
    gpioPin_initialize(&frontLight, GPIOB, GPIO_Pin_10, libPin_OUTPUT);
    gpioPin_initialize(&frontLightBlue, GPIOB, GPIO_Pin_11, libPin_OUTPUT);
    gpioPin_initialize(&rearLight, GPIOB, GPIO_Pin_12, libPin_OUTPUT);
    gpioPin_initialize(&brakeLight, GPIOB, GPIO_Pin_13, libPin_OUTPUT);

    gpioPin_initialize(&leftWinker, GPIOC, GPIO_Pin_0, libPin_OUTPUT);
    gpioPin_initialize(&rightWinker, GPIOC, GPIO_Pin_1, libPin_OUTPUT);
    gpioPin_initialize(&sirenLight, GPIOC, GPIO_Pin_2, libPin_OUTPUT);

    // Motor
    pwm_initializeTimer2();
    motor_initializePWM(&drive, GPIOA, GPIO_Pin_6, GPIO_Pin_7, TIM2, 1);
    motor_initializePWM(&largePiston, GPIOB, GPIO_Pin_0, GPIO_Pin_1, TIM2, 2);
    motor_initializePWM(&smallPiston, GPIOC, GPIO_Pin_4, GPIO_Pin_5, TIM2, 3);

    // Horn
    pwm_initializeTimer4();
    pwm_initializePin(&siren, TIM4, 1);
    pwmTone_sirenInitialize(&sirenFire, &siren);
    pwmTone_beepInitialize(&sirenBeep, &siren);

    // Servos
    pwm_initializeTimer3Remap();
    pwm_initializePin(&steerPin, TIM3, 1);
    pwm_initializePin(&pumpPin, TIM3, 2);
    pwm_initializePin(&rotationPin, TIM3, 3);
    pwm_initializePin(&tiltPin, TIM3, 4);
    pwmServo_inizializeSimple(&rotation, &rotationPin);
    pwmServo_inizializeSimple(&tilt, &tiltPin);
    pwmServo_inizializeSimple(&pump, &pumpPin);
    pwmServo_inizializeSimple(&steer, &steerPin);

    // Analog
    gpio_initializeInputAnalogPin(GPIOA, GPIO_Pin_4 | GPIO_Pin_5);
    system_inicializeADC(4, 5, (uint32_t) &analogValues); // A4 + A5 - 4th and 5th 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]);
    temperature_initializeAnalogFilter(&temperature, &af, 0);
    photoResistor_initializeAnalogFilter(&photo, &af, 1);

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

    // Debug
    gpioPin_initialize(&db1, GPIOC, GPIO_Pin_0, libPin_INPUT);
    gpioPin_initialize(&db2, GPIOC, GPIO_Pin_1, libPin_INPUT);
    gpioPin_initialize(&db3, GPIOC, GPIO_Pin_2, libPin_INPUT);
    gpioPin_initialize(&db4, GPIOC, GPIO_Pin_3, libPin_INPUT);
}

/**
* Clear perihperals
*/
void clearPeriherals(void)
{
    // Lights
    gpioPin_write(&frontLight, Bit_RESET);
    gpioPin_write(&frontLightBlue, Bit_RESET);
    gpioPin_write(&rearLight, Bit_RESET);
    gpioPin_write(&brakeLight, Bit_RESET);

    gpioPin_write(&leftWinker, Bit_RESET);
    gpioPin_write(&rightWinker, Bit_RESET);
    gpioPin_write(&sirenLight, Bit_RESET);

    // Morors
    motor_stop(&drive);
    motor_stop(&largePiston);
    motor_stop(&smallPiston);

    // Siren
    pwmTone_playTone(&siren, PWM_NOTE_C4);
    systemTime_delayMs(10);
    pwmTone_playTone(&siren, PWM_NOTE_NONE);

    // Servos
    pwmServo_setPosition(&rotation, 90);
    pwmServo_setPosition(&tilt, 90);
    pwmServo_setPosition(&pump, PUMP_STOP);
    pwmServo_setPosition(&steer, 90);
}

/**
* Show connecting
*/
void showConnecting(void)
{
    systemTime t = systemTime_getTime();
    BitAction negated = lightsBlinkState;
    if (systemTime_getTimeDiffInMs(lightsPeviousBlinkTime, t) < BLINK_INTERVAL) { return; }

    // show, negate value
    lightsPeviousBlinkTime = t;
    lightsBlinkState    = (BitAction)(1 - lightsBlinkState);

    // Leds
    gpioPin_write(&frontLight, lightsBlinkState);
    gpioPin_write(&frontLightBlue, negated);
    gpioPin_write(&rearLight, lightsBlinkState);
    gpioPin_write(&brakeLight, negated);

    gpioPin_write(&leftWinker, lightsBlinkState);
    gpioPin_write(&rightWinker, lightsBlinkState);
    gpioPin_write(&sirenLight, negated);
}

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

    choice = (a4 << 2) | (a3 << 1) | a2;
    if (DEBUG != 1) { return; }

    switch (choice)
    {
        // Drive
        case 1: // 001
            if (value == Bit_SET) { motor_runForwards(&drive); }
            else { motor_stop(&drive); }
            break;

        //  - Drive
        case 2: // 010
            if (value == Bit_SET) { motor_runBackwards(&drive); }
            else { motor_stop(&drive); }
            break;

        // Large Piston
        case 3: // 011
            if (value == Bit_SET) { motor_runForwards(&largePiston); }
            else { motor_stop(&largePiston); }
            break;

        //  - Large Piston
        case 4: // 100
            if (value == Bit_SET) { motor_runBackwards(&largePiston); }
            else { motor_stop(&largePiston); }
            break;

        // Small Piston
        case 5: // 101
            if (value == Bit_SET) { motor_runForwards(&smallPiston); }
            else { motor_stop(&smallPiston); }
            break;

        //  - Small Piston
        case 6: // 110
            if (value == Bit_SET) { motor_runBackwards(&smallPiston); }
            else { motor_stop(&smallPiston); }
            break;

        // All Servos
        case 7:
            if (value == Bit_SET)
            {
                pwmServo_setPosition(&steer, 180);
                pwmServo_setPosition(&pump, 180);
                pwmServo_setPosition(&rotation, ROTATION_MAX);
                pwmServo_setPosition(&tilt, TILT_MAX);
            }
            else
            {
                pwmServo_setPosition(&steer, 0);
                pwmServo_setPosition(&pump, PUMP_STOP);
                pwmServo_setPosition(&rotation, ROTATION_MIN);
                pwmServo_setPosition(&tilt, TILT_MIN);
            }
            break;
    }

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

/**
* Process input command
*/
void processCommand(libRemoteCommandValue cv)
{
    uint8_t v;
    switch (cv.command)
    {
        /* MOTORS */
        // Drive
        case REM_COMMNAND_ANALOG1:
            if (cv.value == ANALOG_MIDDLE_POSITION)
            {
                sirenBeepOn = FALSE;
                motor_stop(&drive);
            }
            else
                if (cv.value < ANALOG_MIDDLE_POSITION)
                {
                    motor_runForwardsPercent(&drive, map(cv.value, ANALOG_MIDDLE_POSITION, ANALOG_MAX_POSITION, 30, 100));
                }
                else
                {
                    motor_runBackwardsPercent(&drive, 100 - map(cv.value, 0, ANALOG_MIDDLE_POSITION, 30, 100));
                    sirenBeepOn = TRUE;
                }
            break;

        // Large - UP
        case REM_COMMNAND_BUTTON3:
            if (cv.value == 1)
            {
                motor_runBackwards(&largePiston);
            }
            else
            {
                motor_stop(&largePiston);
            }
            break;

        // Large - DOWN
        case REM_COMMNAND_BUTTON4:
            if (cv.value == 1)
            {
                motor_runForwards(&largePiston);
            }
            else
            {
                motor_stop(&largePiston);
            }
            break;

        // Small - UP
        case REM_COMMNAND_BUTTON5:
            if (cv.value == 1)
            {
                motor_runBackwards(&smallPiston);
            }
            else
            {
                motor_stop(&smallPiston);
            }
            break;

        // Small - DOWN
        case REM_COMMNAND_BUTTON6:
            if (cv.value == 1)
            {
                motor_runForwards(&smallPiston);
            }
            else
            {
                motor_stop(&smallPiston);
            }
            break;

        /* SERVOS */
        // Steer
        case REM_COMMNAND_ANALOG4:
            v = map(cv.value, 0, ANALOG_RESOLUTION, 0, 180);
            pwmServo_setPosition(&steer, v);

            // winkers
            rightWinkerBlink = (v <= 45) ? TRUE : FALSE;
            leftWinkerBlink = (v >= 135) ? TRUE : FALSE;

            // turn off light
            if (rightWinkerBlink == FALSE)
            {
                gpioPin_write(&rightWinker, Bit_RESET);
            }

            if (leftWinkerBlink == FALSE)
            {
                gpioPin_write(&leftWinker, Bit_RESET);
            }

            break;

        // Rotation
        case REM_COMMNAND_ANALOG5:
            pwmServo_setPosition(&rotation, map(ANALOG_RESOLUTION - cv.value, 0, ANALOG_RESOLUTION, ROTATION_MIN, ROTATION_MAX));
            break;

        // Tilt
        case REM_COMMNAND_ANALOG8:
            pwmServo_setPosition(&tilt, map(ANALOG_RESOLUTION - cv.value, 0, ANALOG_RESOLUTION, TILT_MIN, TILT_MAX));
            break;

        // Pump - on
        case REM_COMMNAND_BUTTON9:
            pwmServo_setPosition(&pump, PUMP_ON);
            break;

        // Pump -off
        case REM_COMMNAND_BUTTON10:
            pwmServo_setPosition(&pump, PUMP_STOP);
            break;

        /* LIGHTS */
        // Brake
        case REM_COMMNAND_BUTTON1:
            gpioPin_write(&brakeLight, (BitAction)cv.value);
            break;

        // Front
        case REM_COMMNAND_SWITCH1:
            gpioPin_write(&frontLight, (BitAction)cv.value);
            break;

        // Front / blue
        case REM_COMMNAND_SWITCH2:
            gpioPin_write(&frontLightBlue, (BitAction)cv.value);
            break;

        // Back
        case REM_COMMNAND_SWITCH3:
            gpioPin_write(&rearLight, (BitAction)cv.value);
            break;

        // Siren
        case REM_COMMNAND_SWITCH4:
            sirenBlink = cv.value > 0 ? TRUE : FALSE;
            break;

        /* SIREN */
        // Horn
        case REM_COMMNAND_BUTTON2:
            sirenHornOn = cv.value > 0 ? TRUE : FALSE;
            break;

        // Beep
        case REM_COMMNAND_SWITCH5:
            sirenBeepOn = cv.value > 0 ? TRUE : FALSE;
            break;

        // Beep
        case REM_COMMNAND_SWITCH6:
            sirenFireOn = cv.value > 0 ? TRUE : FALSE;
            break;
    }
}

/**
* Send status data
*/
void sendStatusData(void)
{
    systemTime t = systemTime_getTime();

    if (systemTime_getTimeDiffInMs(xbeeDataPeviousTime, t) <= XBEE_SEND_DATA)
    {
        return;
    }

    // set data and send
    if (xbeeDataType == 0)
    {
        numberToDec(temperature.temperature, 2, &temperatureLine[6]);
        xbeeRx_sendData(&xbeeRx, 1, &temperatureLine[0], REM_COMMAND_DATA_LENGTH);
    }
    else
        if (xbeeDataType == 1)
        {
            numberToDec(photo.luxp, 2, &luxLine[5]);
            xbeeRx_sendData(&xbeeRx, 2, &luxLine[0], REM_COMMAND_DATA_LENGTH);
        }

    // set next state
    xbeeDataPeviousTime = t;
    xbeeDataType = 1 - xbeeDataType; // 0 or 1
}