/**
* Dump 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 (Drive, steer, lift) | A6 A7; B0 B1; C4 C5
libMotor drive, steer, lift;

// Servos - TIM3 remap - C6..C9
libServo shift1, shift2;
libPWMpin shift1pin, shift2pin;

// Lights C0..C3 (FW, FB, Brake, R); B10..B13 (LW, RW)
libPin frontLight, frontLightBlue, rearLight, brakeLight;
libPin leftWinker, rightWinker;
uint8_t frontLights, backLights;
bool lastLightsState;

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

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

// Debug - C10..C12 ; C10 LSB, C12 MSB
libPin db10, db11, db12;

// Sensors - A4
libPhotoResistor photo;

// A5; B14 B15;    C13 C14

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

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

// - gearbox
uint8_t gearboxSet;
uint8_t gearboxActual;
systemTime gearboxSetTime;

// - display
uint8_t gearLine[REM_COMMAND_DATA_LENGTH] = {'G', 'e', 'a', 'r', ':', ' ', 'x', 'x', ' ', ' ', '|', 'D', 'u', 'm', 'p', ' '}; // Gear: xx
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;
uint8_t sirenHornState = 0, sirenBeepState = 0; // 0 = uninitialized, 1 = playing, 2 = stopped

// - xbee
uint8_t xbeeDataType = 0; // 0 = gear, 1 = lux
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;
            }

            // gearbox
            processGearbox();

            // Send status data
            sendStatusData();
        }

        // Peripherals state
        updatePeripheralsState();

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

            analogFilter_clear(&af);

            // AUTO LIGHTS
            processAnalogInputs();
        }
    }
}

/**
* 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); }
    }

    // Siren - by priority - SIREN, BEEP, HORN
    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;
        }
    }
}

/**
* Initialize perihperals
*/
void initializePeriherals(void)
{
    // Lights C0..C3, B10..B13
    gpioPin_initialize(&frontLight, GPIOC, GPIO_Pin_0, libPin_OUTPUT);
    gpioPin_initialize(&frontLightBlue, GPIOC, GPIO_Pin_1, libPin_OUTPUT);
    gpioPin_initialize(&brakeLight, GPIOC, GPIO_Pin_2, libPin_OUTPUT);
    gpioPin_initialize(&rearLight, GPIOC, GPIO_Pin_3, libPin_OUTPUT);

    gpioPin_initialize(&leftWinker, GPIOB, GPIO_Pin_10, libPin_OUTPUT);
    gpioPin_initialize(&rightWinker, GPIOB, GPIO_Pin_11, libPin_OUTPUT);

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

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

    // Servos
    pwm_initializeTimer3Remap();
    pwm_initializePin(&shift1pin, TIM3, 1);
    pwm_initializePin(&shift2pin, TIM3, 2);

    pwmServo_inizializeSimple(&shift1, &shift1pin);
    pwmServo_inizializeSimple(&shift2, &shift2pin);

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

    // Debug
    gpioPin_initialize(&db10, GPIOC, GPIO_Pin_10, libPin_INPUT);
    gpioPin_initialize(&db11, GPIOC, GPIO_Pin_11, libPin_INPUT);
    gpioPin_initialize(&db12, GPIOC, GPIO_Pin_12, libPin_INPUT);

    // Analog
    gpio_initializeInputAnalogPin(GPIOA, GPIO_Pin_4);
    system_inicializeADC(4, 4 + ANALOG_INPUTS - 1, (uint32_t) &analogValues); // A4- 4th channel
    analogFilter_initialize(&af, ANALOG_INPUTS, &analogValues[0]);
    analog_initialize(&analogInputs[0], &af, 0);
    analogCollection_addLibAnalog(&analogInputs[0]);
    photoResistor_initializeAnalogFilter(&photo, &af, 0);
}

/**
* 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);
    frontLights = 0;
    backLights = 0;
    lastLightsState = FALSE; // OFF

    // Morors
    motor_stop(&drive);
    motor_stop(&steer);
    motor_stop(&lift);

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

    // Servos
    gearboxSet = 0;
    pwmServo_setPosition(&shift1, SHIFT_1_MIDDLE);
    pwmServo_setPosition(&shift2, SHIFT_2_MIDDLE);
}

/**
* 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);
}

/**
* Check debug
*/
void checkDebug(void)
{

    BitAction d10, d11, d12;
    uint8_t choice;

    if (DEBUG != 1) { return; }

    d12 = gpioPin_read(&db12);
    d11 = gpioPin_read(&db11);
    d10 = gpioPin_read(&db10);

    choice = (d12 << 2) | (d11 << 1) | d10;

    switch (choice)
    {
        case 0x0:
            motor_stop(&drive);
            motor_stop(&steer);
            motor_stop(&lift);
            pwmTone_playTone(&siren, PWM_NOTE_NONE);

            break;

        case 0x1:
            //pwmServo_setPosition(&shift1, pwmServo_getPosition(&shift1) + 1);
            motor_runForwardsPercent(&drive, 100);
            break;

        case 0x2:
            //pwmServo_setPosition(&shift1, pwmServo_getPosition(&shift1) - 1);
            motor_runBackwardsPercent(&drive, 100);
            break;

        case 0x3:
            //pwmServo_setPosition(&shift2, pwmServo_getPosition(&shift2) + 1);
            motor_runForwardsPercent(&steer, 100);
            break;

        case 0x4:
            //pwmServo_setPosition(&shift2, pwmServo_getPosition(&shift2) - 1);
            motor_runBackwardsPercent(&steer, 100);
            break;

        case 0x5:
            motor_runForwardsPercent(&lift, 100);
            break;

        case 0x6:
            motor_runBackwardsPercent(&lift, 100);
            break;

        case 0x7:
            pwmTone_playTone(&siren, PWM_NOTE_C4);
            break;
    }

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

/**
* Process input command
*/
void processCommand(libRemoteCommandValue cv)
{
    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, 100 - map(cv.value, 0, ANALOG_MIDDLE_POSITION, DRIVE_MIN, 100));
                }
                else
                {
                    motor_runBackwardsPercent(&drive, map(cv.value, ANALOG_MIDDLE_POSITION, ANALOG_MAX_POSITION, DRIVE_MIN, 100));
                    sirenBeepOn = TRUE;
                }
            }

            break;

        // Steer
        case REM_COMMNAND_ANALOG4:
            if (cv.value == ANALOG_MIDDLE_POSITION)
            {
                // turn off light
                gpioPin_write(&rightWinker, Bit_RESET);
                gpioPin_write(&leftWinker, Bit_RESET);

                leftWinkerBlink = FALSE;
                rightWinkerBlink = FALSE;

                motor_stop(&steer);
            }
            else
            {
                if (cv.value < ANALOG_MIDDLE_POSITION)
                {
                    rightWinkerBlink = TRUE;

                    motor_runForwardsPercent(&steer, 100 - map(cv.value, 0, ANALOG_MIDDLE_POSITION, DRIVE_MIN, 100));
                }
                else
                {
                    leftWinkerBlink = TRUE;
                    motor_runBackwardsPercent(&steer, map(cv.value, ANALOG_MIDDLE_POSITION, ANALOG_MAX_POSITION, DRIVE_MIN, 100));
                }
            }

            break;

        // Lift
        case REM_COMMNAND_BUTTON10:
            if (cv.value > 0) { motor_runForwards(&lift); }
            else { motor_stop(&lift); }
            break;

        case REM_COMMNAND_BUTTON9:
            if (cv.value > 0) { motor_runBackwards(&lift); }
            else { motor_stop(&lift); }
            break;

        // - GEARBOX
        case REM_COMMNAND_BUTTON1:
            setGearbox(0);
            break;

        case REM_COMMNAND_BUTTON3:
            setGearbox(1);
            break;

        case REM_COMMNAND_BUTTON4:
            setGearbox(2);
            break;

        case REM_COMMNAND_BUTTON5:
            setGearbox(3);
            break;

        case REM_COMMNAND_BUTTON6:
            setGearbox(4);
            break;

        // - HORN -
        case REM_COMMNAND_BUTTON2:
            sirenHornOn = cv.value > 0 ? TRUE : FALSE;
            break;

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

        // Front
        case REM_COMMNAND_SWITCH1:
            if (cv.value > 0) { frontLights = setNthBit(frontLights, 0); }
            else { frontLights = resetNthBit(frontLights, 0); }

            gpioPin_write(&frontLight, (BitAction)frontLights);

            break;

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

        // Back
        case REM_COMMNAND_SWITCH6:
            if (cv.value > 0) { backLights = setNthBit(backLights, 0); }
            else { backLights = resetNthBit(backLights, 0); }

            gpioPin_write(&rearLight, (BitAction)backLights);
            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(gearboxActual, 2, &gearLine[6]);
        xbeeRx_sendData(&xbeeRx, 1, &gearLine[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
}

/**
* Set gearbox
*/
void setGearbox(uint8_t gear)
{
    systemTime now = systemTime_getTime();

    // clear
    pwmServo_setPosition(&shift1, SHIFT_1_MIDDLE);
    pwmServo_setPosition(&shift2, SHIFT_2_MIDDLE);

    // set for next iteration
    gearboxSet = gear; // cleared after set
    gearboxActual = gear;
    gearboxSetTime = systemTime_build(now.s, now.ms + GEARBOX_SET_AFTER_N);
}

/**
* Process gearbox
*/
void processGearbox(void)
{
    systemTime now = systemTime_getTime();
    if (gearboxSet == 0)
    {
        return; // NEUTRAL
    }

    // check time
    if (systemTime_compareTimes(gearboxSetTime, now) <= 0)
    {
        return; // wait
    }

    // set
    switch (gearboxSet)
    {
        case 2:
            pwmServo_setPosition(&shift1, SHIFT_1_MIN);
            break;

        case 1:
            pwmServo_setPosition(&shift1, SHIFT_1_MAX);
            break;

        case 3:
            pwmServo_setPosition(&shift2, SHIFT_2_MIN);
            break;

        case 4:
            pwmServo_setPosition(&shift2, SHIFT_2_MAX);
            break;
    }

    gearboxSet = 0;
}

/**
* Process analog inpus
*/
void processAnalogInputs(void)
{
    bool light = photo.luxp <= LIGHTS_SWITCH_LEVEL ? TRUE : FALSE;
    if (light == lastLightsState) { return; } // nothing to do

    // set variables
    lastLightsState = light;
    if (light == TRUE)
    {
        frontLights = setNthBit(frontLights, 1);
        backLights = setNthBit(backLights, 1);

    }
    else
    {
        frontLights = resetNthBit(frontLights, 1);
        backLights = resetNthBit(backLights, 1);
    }

    // light up
    gpioPin_write(&frontLight, (BitAction)frontLights);
    gpioPin_write(&rearLight, (BitAction)backLights);
}