/** @file HL_sys_main.c 
*   @brief Application main file
*   @date 08-Feb-2017
*   @version 04.06.01
*
*   This file contains an empty main function,
*   which can be used for the application.
*/

/* 
* Copyright (C) 2009-2016 Texas Instruments Incorporated - www.ti.com  
* 
* 
*  Redistribution and use in source and binary forms, with or without 
*  modification, are permitted provided that the following conditions 
*  are met:
*
*    Redistributions of source code must retain the above copyright 
*    notice, this list of conditions and the following disclaimer.
*
*    Redistributions in binary form must reproduce the above copyright
*    notice, this list of conditions and the following disclaimer in the 
*    documentation and/or other materials provided with the   
*    distribution.
*
*    Neither the name of Texas Instruments Incorporated nor the names of
*    its contributors may be used to endorse or promote products derived
*    from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 
*  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 
*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/


/* USER CODE BEGIN (0) */
/* USER CODE END */

/* Include Files */

#include "HL_sys_common.h"

/* USER CODE BEGIN (1) */
/* Include FreeRTOS scheduler files */
#include "FreeRTOS.h"
#include "os_task.h"
#include "os_timer.h"
#include "HL_sci.h"
#include "HL_gio.h"
#include "HL_etpwm.h"
#include "HL_can.h"
#include "HL_system.h"

//Definitions for Watchdog and CAN
#define D_COUNT  8
#define MAX_WD_DEVIATION 20      //increments
#define MAX_DUTY_DEVIATION 5    //%
#define TIMEOUT_MS 100

/* Define Task Handles */
xTaskHandle xTask1Handle;
xTaskHandle xTask2Handle;
xTimerHandle xTask3Handle;

//Variables for PWM generation
volatile float duty = 0.0;
uint16 TBPRD_Ticks;

//Handler function for alternating light flashes
void vTaskBlinky(void *pvParameters)
{
    for(;;)
    {
        //switch LED on
        gioToggleBit(gioPORTB, 6);
        gioToggleBit(gioPORTB, 7);
        vTaskDelay(100 / portTICK_PERIOD_MS);
    }
}

//handler for error timeouts
void vTaskTimeout(void *pvParameters)
{
    //stop PWMing, assume loss with master and should disable self until recovery
    duty = 0.0;

    //Set new PWM
    etpwmSetCmpA(etpwmREG1, (uint16)(duty * TBPRD_Ticks / 100.0));
    etpwmSetCmpB(etpwmREG1, (uint16)(duty * TBPRD_Ticks / 100.0));

    //report timeout occurred
    sciSend(sciREG3, 15, (unsigned char *)"MCU timed out\r\n");
    systemREG1->SYSECR = 0x8000U;
}

//CAN transmission handler
void vTaskCANTX(void *pvParameters)
{
    uint8 tx_data[D_COUNT] = {0};
    uint8 watchdogCtr = 0;
    uint16_t buffer = 0;

    unsigned char command[8];
    uint8 NumberOfChars;

    for(;;)
    {
        //construct message
        buffer = (uint16_t)(duty * 100.0) & 0x3FFF;
        tx_data[0] = (uint8)(buffer & 0xFF);
        tx_data[1] = (uint8)((buffer >> 8) & 0x3F);
        tx_data[2] = watchdogCtr;

        //increment watchdog
        if(watchdogCtr == 255)
        {
            watchdogCtr = 0;
        } else
        {
            watchdogCtr++;
        }

        //send CAN message
        canTransmit(canREG4, canMESSAGE_BOX1, (const uint8 *) &tx_data[0]);

        //Diagnostic watchdog printout
        taskENTER_CRITICAL();
        sciSend(sciREG3, 27, "Hercules Watchdog Counter: ");
        NumberOfChars = ltoa((unsigned int)watchdogCtr, (char *)command);
        sciSend(sciREG3, NumberOfChars, command);
        sciSend(sciREG3, 2, (unsigned char *)"\r\n");
        taskEXIT_CRITICAL();

        //wait until next tx
        vTaskDelay(100 / portTICK_PERIOD_MS);
    }
}

/* can interrupt notification */
volatile int watchdogCtrLast = 0;
void canMessageNotification(canBASE_t *node, uint32 messageBox)
{
    uint8 rx_data[D_COUNT] = {0};
    uint8 NumberOfChars;
    int buffer, watchdogCtrCurr = 0;
    float pedalDuty1, pedalDuty2;
    unsigned char command[8];
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    if(node==canREG4)
    {
        //get data
        if(canIsRxMessageArrived(canREG4, canMESSAGE_BOX2))
        {
            canGetData(canREG4, canMESSAGE_BOX2, (uint8 * )&rx_data[0]); /* copy to RAM */

            //parse data
            buffer = rx_data[1] & 0x3F;
            buffer = (buffer << 8) | rx_data[0];
            pedalDuty1 = (float)buffer * 0.01;

            buffer = rx_data[3] & 0x3F;
            buffer = (buffer << 8) | rx_data[2];
            pedalDuty2 = (float)buffer * 0.01;

            watchdogCtrCurr = rx_data[4];

            //check if data is valid then store data if valid
            buffer = watchdogCtrCurr - watchdogCtrLast;
            if(buffer < 0){
                buffer += 256;
            }

            if( buffer < MAX_WD_DEVIATION && buffer > 0)
            {
                if(abs(pedalDuty1 - pedalDuty2) < MAX_DUTY_DEVIATION)
                {
                    //take minimum of the requested values
                    duty = (pedalDuty1 < pedalDuty2) ? pedalDuty1 : pedalDuty2;

                    //Set new PWM
                    etpwmSetCmpA(etpwmREG1, (uint16)(duty * TBPRD_Ticks / 100.0));
                    etpwmSetCmpB(etpwmREG1, (uint16)(duty * TBPRD_Ticks / 100.0));

                    //report successful result on serial
                    sciSend(sciREG3, 12, "New % Duty: ");
                    NumberOfChars = ltoa((unsigned int)duty, (char *)command);
                    sciSend(sciREG3, NumberOfChars, command);
                    sciSend(sciREG3, 1, ".");
                    NumberOfChars = ltoa((unsigned int)(duty * 10) % 10, (char *)command);
                    sciSend(sciREG3, NumberOfChars, command);
                    NumberOfChars = ltoa((unsigned int)(duty * 100) % 10, (char *)command);
                    sciSend(sciREG3, NumberOfChars, command);
                    sciSend(sciREG3, 3, (unsigned char *)"%\r\n");

                    //flag successful rx in cycle
                    if( xTimerResetFromISR( xTask3Handle, &xHigherPriorityTaskWoken) != pdPASS )
                    {
                        //failed to reset timer
                        sciSend(sciREG3, 32, (unsigned char *)"Timeout timer failed to reset.\r\n");
                    }

                    //Diagnostic watchdog printout
                    sciSend(sciREG3, 22, "ECU Watchdog Counter: ");
                    NumberOfChars = ltoa((unsigned int)watchdogCtrCurr, (char *)command);
                    sciSend(sciREG3, NumberOfChars, command);
                    sciSend(sciREG3, 2, (unsigned char *)"\r\n");

//                    if(xTimerIsTimerActive(xTask3Handle) == pdTRUE)
//                    {
//                        //timer still running
//                        if( xTimerResetFromISR( xTask3Handle, &xHigherPriorityTaskWoken) != pdPASS )
//                        {
//                            //failed to reset timer
//                            sciSend(sciREG3, 32, (unsigned char *)"Timeout timer failed to reset.\r\n");
//                        }
//                    } else
//                    {
//                        //timer died before
//                        if( xTimerStartFromISR( xTask3Handle, &xHigherPriorityTaskWoken) != pdPASS )
//                        {
//                            //failed to reset timer
//                            sciSend(sciREG3, 34, (unsigned char *)"Timeout timer failed to restart.\r\n");
//                        }
//                    }
                } else
                {
                    //error duty cycles requested far apart
                    sciSend(sciREG3, 30, "Pedal requests too different: ");
                    NumberOfChars = ltoa((unsigned int)pedalDuty1, (char *)command);
                    sciSend(sciREG3, NumberOfChars, command);
                    sciSend(sciREG3, 1, ".");
                    NumberOfChars = ltoa((unsigned int)(pedalDuty1 * 10) % 10, (char *)command);
                    sciSend(sciREG3, NumberOfChars, command);
                    NumberOfChars = ltoa((unsigned int)(pedalDuty1 * 100) % 10, (char *)command);
                    sciSend(sciREG3, NumberOfChars, command);
                    sciSend(sciREG3, 5, " vs. ");
                    NumberOfChars = ltoa((unsigned int)pedalDuty2, (char *)command);
                    sciSend(sciREG3, NumberOfChars, command);
                    sciSend(sciREG3, 1, ".");
                    NumberOfChars = ltoa((unsigned int)(pedalDuty2 * 10) % 10, (char *)command);
                    sciSend(sciREG3, NumberOfChars, command);
                    NumberOfChars = ltoa((unsigned int)(pedalDuty2 * 100) % 10, (char *)command);
                    sciSend(sciREG3, NumberOfChars, command);
                    sciSend(sciREG3, 2, (unsigned char *)"\r\n");
                }
            } else
            {
                //error watchdog not incremented correctly
                sciSend(sciREG3, 30, "Watchdog incremented wrongly: ");
                NumberOfChars = ltoa((unsigned int)watchdogCtrLast, (char *)command);
                sciSend(sciREG3, NumberOfChars, command);
                sciSend(sciREG3, 4, " -> ");
                NumberOfChars = ltoa((unsigned int)watchdogCtrCurr, (char *)command);
                sciSend(sciREG3, NumberOfChars, command);
                sciSend(sciREG3, 2, (unsigned char *)"\r\n");
            }

            //update last observed watchdog
            watchdogCtrLast = watchdogCtrCurr;
        }
    }
}

//CAN Error handler
void canErrorNotification(canBASE_t *node, uint32 messageBox)
{
    uint32 error = canGetLastError(canREG4);   //0 is ok, check def for other numbers

    switch(error)
    {
        case canERROR_STUFF:
            sciSend(sciREG3, 36, (unsigned char *)"Stuff error occurred on RX message\r\n");
            //flag error
            break;
        case canERROR_FORMAT:
            sciSend(sciREG3, 42, (unsigned char *)"Form/format error occurred on RX message\r\n");
            //flag error
            break;
        case canERROR_ACKNOWLEDGE:
            sciSend(sciREG3, 32, (unsigned char *)"TX message wasn't acknowledged\r\n");
            //flag error
            break;
        case canERROR_BIT1:
            sciSend(sciREG3, 65, (unsigned char *)"TX message monitored dominant level where recessive is expected\r\n");
            //flag error
            break;
        case canERROR_BIT0:
            sciSend(sciREG3, 65, (unsigned char *)"TX message monitored recessive level where dominant is expected\r\n");
            //flag error
            break;
        case canERROR_CRC:
            sciSend(sciREG3, 32, (unsigned char *)"RX message has wrong CRC value\r\n");
            //flag error
            break;
        case canERROR_OK:
            sciSend(sciREG3, 23, (unsigned char *)"No CAN error occurred\r\n");
            break;
        case canERROR_NO:
            sciSend(sciREG3, 52, (unsigned char *)"No error occurred since last call of this function\r\n");
            break;
        default:
            sciSend(sciREG3, 23, (unsigned char *)"No CAN error occurred\r\n");
            break;
    }


}

//CAN status change notifier
void canStatusChangeNotification(canBASE_t *node, uint32 messageBox)
{
    return;
}

//button press handler TODO: remove in final version
void gioNotification(gioPORT_t *port, uint32 bit){
    uint32_t NumberOfChars;
    unsigned char command[8];

    //get current PWM
    //pwmGetSignal(hetRAM1, pwm0, &sig);

    if(bit == 4){
       //increment PWM
       if(duty >= 100){
           duty = 100;
       } else {
           duty += 1;
       }

    } else if(bit == 5){
       //decrement PWM
       if(duty <= 0){
           duty = 0;
       } else {
           duty -= 1;
       }
    }

    //Set new PWM
    etpwmSetCmpA(etpwmREG1, (uint16)(duty * TBPRD_Ticks / 100.0));
    etpwmSetCmpB(etpwmREG1, (uint16)(duty * TBPRD_Ticks / 100.0));

    //Print change to PWM
    sciSend(sciREG3, 8, "% Duty: ");
    NumberOfChars = ltoa((long)duty, (char *)command);
    sciSend(sciREG3, NumberOfChars, command);
    sciSend(sciREG3, 2, (unsigned char *)"\r\n");

    sciSend(sciREG1, 8, "% Duty: ");
    NumberOfChars = ltoa((long)duty, (char *)command);
    sciSend(sciREG1, NumberOfChars, command);
    sciSend(sciREG1, 2, (unsigned char *)"\r\n");
}
/* USER CODE END */

/** @fn void main(void)
*   @brief Application main function
*   @note This function is empty by default.
*
*   This function is called after startup.
*   The user can use this function to implement the application.
*/

/* USER CODE BEGIN (2) */
/* USER CODE END */

int main(void)
{
/* USER CODE BEGIN (3) */
    //flashing LEDs
    gioInit();

    gioEnableNotification(gioPORTB, 4);
    gioEnableNotification(gioPORTB, 5);

    gioSetBit(gioPORTB, 6, 1);
    gioSetBit(gioPORTB, 7, 0);

    //PWM setup
    //configure phase shifted pwms, find output pins and test
    etpwm_config_reg_t etpwm1Conf;
    etpwmStopTBCLK();

    etpwmInit();

    //Set period of all pwms
    //etpwm1GetConfigValue(&etpwm1Conf, CurrentValue);
    etpwm1GetConfigValue(&etpwm1Conf, InitialValue);
    TBPRD_Ticks = etpwm1Conf.CONFIG_TBPRD;
    //etpwmSetTimebasePeriod(etpwmREG1, TBPRD_Ticks);

    //setting clock divisions
    //etpwmSetClkDiv(etpwmREG1, ClkDiv_by_128, HspClkDiv_by_1);

    //set counter behavior
    etpwmSetCount(etpwmREG1, 0);
    etpwmSetCounterMode(etpwmREG1, CounterMode_Up);

    //set sync behavior
    etpwmSetSyncOut(etpwmREG1, SyncOut_CtrEqZero);
    etpwmDisableCounterLoadOnSync(etpwmREG1);

    //enable shadow register modes
    etpwmEnableTimebasePeriodShadowMode(etpwmREG1);
    etpwmEnableCmpAShadowMode(etpwmREG1, LoadMode_CtrEqZero);
    etpwmEnableCmpBShadowMode(etpwmREG1, LoadMode_CtrEqZero);   //etpwmSetCmpA/B value must be greater than 1

    //set qualifier module
    etpwmActionQualConfig_t aqCfg;

    //pwmA behavior
    aqCfg.CtrEqCmpADown_Action  = ActionQual_Disabled;
    aqCfg.CtrEqCmpAUp_Action    = ActionQual_Clear;
    aqCfg.CtrEqCmpBDown_Action  = ActionQual_Disabled;
    aqCfg.CtrEqCmpBUp_Action    = ActionQual_Disabled;
    aqCfg.CtrEqPeriod_Action    = ActionQual_Set;
    aqCfg.CtrEqZero_Action      = ActionQual_Set;
    etpwmSetActionQualPwmA(etpwmREG1, aqCfg);

    //pwmB behavior
    aqCfg.CtrEqCmpADown_Action  = ActionQual_Disabled;
    aqCfg.CtrEqCmpAUp_Action    = ActionQual_Disabled;
    aqCfg.CtrEqCmpBDown_Action  = ActionQual_Disabled;
    aqCfg.CtrEqCmpBUp_Action    = ActionQual_Clear;
    aqCfg.CtrEqPeriod_Action    = ActionQual_Set;
    aqCfg.CtrEqZero_Action      = ActionQual_Set;
    etpwmSetActionQualPwmA(etpwmREG1, aqCfg);

    //set duty cycle via compare registers
    etpwmSetCmpA(etpwmREG1, (uint16)(duty * TBPRD_Ticks / 100.0));
    etpwmSetCmpB(etpwmREG1, (uint16)(duty * TBPRD_Ticks / 100.0));

    //restart clock
    etpwmStartTBCLK();

    //Serial setup
    sciInit();

    //setup CAN
    _enable_IRQ_interrupt_();
    canInit();
    //canEnableloopback(canREG4, External_Lbk);   //TODO: comment out when actually testing with HW!
    //canDisableloopback(canREG4);
    //canEnableErrorNotification(canREG4);

    /* Create Task 1 */
    if (xTaskCreate(vTaskBlinky,"Task1", configMINIMAL_STACK_SIZE, NULL, 1, &xTask1Handle) != pdTRUE)
    {
        /* Task could not be created */
        sciSend(sciREG3, 24, (unsigned char *)"Blinky failed to start\r\n");
        while(1);
    }

    /* Create Task 2 */
    if (xTaskCreate(vTaskCANTX,"Task2", configMINIMAL_STACK_SIZE, NULL, 1, &xTask2Handle) != pdTRUE)
    {
        /* Task could not be created */
        sciSend(sciREG3, 23, (unsigned char *)"CANTX failed to start\r\n");
        while(1);
    }

    /* Create Task 3 */
    xTask3Handle = xTimerCreate("Task3", (TIMEOUT_MS / portTICK_RATE_MS), pdFALSE, (void*)0, vTaskTimeout);       //note (500 / portTICK_PERIOD_MS) calculates number of ticks for timer to delay, same for vTaskDelay
    if (xTask3Handle == NULL)
    {
        /* Task could not be created */
        sciSend(sciREG3, 25, (unsigned char *)"Timeout failed to start\r\n");
        while(1);
    } else
    {
        if( xTimerStart( xTask3Handle, (TIMEOUT_MS / portTICK_RATE_MS) ) != pdPASS )
        {
            //failed to reset timer
            sciSend(sciREG3, 32, (unsigned char *)"Timeout timer failed to start.\r\n");
            while(1);
        }
    }

    sciSend(sciREG3, 37, (unsigned char *)"\r\nAll systems running, Hello World!\r\n");

    /* Start Scheduler */
    vTaskStartScheduler();  //should never go past this line!

    /* Run forever */
    while(1);
/* USER CODE END */

    return 0;
}


/* USER CODE BEGIN (4) */
/* USER CODE END */
