/**
 * @file        comm.c
 */

/* Includes *******************************************************************/
#include "comm.h"
#include "commserial.h"
#include "encoder.h"
#include "Timer.h"
#include "motor.h"
#include "log.h"

/* Public variables ***********************************************************/
static uint16_t encoderPulse = 0;
/* Private variables **********************************************************/
/* Exported variables *********************************************************/
extern volatile uint16_t uiMotorTimeout;
/* Constants ******************************************************************/
#define MSB16(a) ((uint8_t)(((a) & 0xFF00) >> 8))
#define LSB16(a) ((uint8_t)(((a) & 0x00FF) >> 0))

/* Private functions **********************************************************/

/* Public functions ***********************************************************/
/**
 * @brief   COMM_Init()
 *          Initialization of the comm interface module.
 */
void COMM_Init(void)
{
    COMMSeriald_Init();
}

/**
 * @brief   Diag_Handler()
 *          This functions must be called from the super-loop. It handles the
 *          commnose communication.
 */
void COMM_Handler(void)
{
    static commserial_msg_t msg;

    if (COMMSeriald_LoadMsg(&msg)){
        COMM_StateMachine(&msg);
        COMMSeriald_SendMsg(&msg);
    }
}
void COMM_SetPulseError(uint16_t pulse)
{
    encoderPulse = pulse;
}
/**
 * @brief   COMM_StateMachine()
 *          Contains all commnostic functions
 */
void COMM_StateMachine(commserial_msg_t* msg)
{
    volatile uint16_t value;
    volatile uint8_t valueMSB;
    volatile uint8_t valueLSB;
    if(0 == msg->Valid) {
        msg->Command = COMM_RESPONSE_NACK;
        msg->DataLength = 0;
    }
    else  {
        switch (msg->Command) {
            /* COMMON */
            /* ------ */
            case COMM_COMMON_SYSTEM: {
                 msg->Command = COMM_RESPONSE_ACK;
                 msg->DataLength = 3u;

                 msg->Data[0] = VERSION_1;
                 msg->Data[1] = VERSION_2;
                 msg->Data[2] = VERSION_3;

                 break;
            }
            case COMM_ENCODER_STATUS: {
                msg->Command = COMM_RESPONSE_STATUS;
                msg->DataLength = 9;
                uint16_t count      = Encoder_GetCount();
                int16_t revolutions = Encoder_GetRevolutions();
                uint16_t freq       = Motor_GetFreq();
                msg->Data[0] = MSB16(count);
                msg->Data[1] = LSB16(count);
                msg->Data[2] = MSB16(revolutions);
                msg->Data[3] = LSB16(revolutions);
                msg->Data[4] = MSB16(freq);
                msg->Data[5] = LSB16(freq);
                msg->Data[6] = 0x00; // TODO log status - LOG_GetStatus
                msg->Data[7] = 0x00; // TODO pps status
                msg->Data[8] = 0x00; // TODO UTC status
                uiMotorTimeout = COMM_MOTOR_TIMEOUT;
                break;
            }
            case COMM_MOTOR_SET_RPM: {
                msg->Command = COMM_RESPONSE_ACK;
                msg->DataLength = 0;
                valueMSB = msg->Data[0];
                valueLSB = msg->Data[1];
                value = (valueMSB << 8) | valueLSB;
                Motor_SetRPM(value);
                break;
            }
            case COMM_MOTOR_STOP: {
                msg->Command = COMM_RESPONSE_ACK;
                msg->DataLength = 0;
                Motor_Stop();
                break;
            }
            case COMM_LOG_START: {
                msg->Command = COMM_RESPONSE_ACK;
                msg->DataLength = 0;
                LOG_Start();
                break;
            }
            case COMM_LOG_SEGMENT: {
                msg->Command = COMM_RESPONSE_ACK;
                msg->DataLength = 0;
                LOG_NewSegment();
                break;
            }
            case COMM_LOG_STOP: {
                msg->Command = COMM_RESPONSE_ACK;
                msg->DataLength = 0;
                LOG_Stop();
                break;
            }
            case COMM_LOG_RESET: {
                msg->Command = COMM_RESPONSE_ACK;
                msg->DataLength = 0;
                LOG_Reset();
                break;
            }
            default: {
                msg->Command = COMM_RESPONSE_NACK;
                msg->DataLength = 0u;
            }
        }
    }
}
