/************************************************************************************************************************
 * Millisec.c
 * Millisec module
 *
 * The millisec task is called once every 1 millisecond to gather data from the FPGA crucial modules,
 * so it will be ready for the Millisec operations. it is called 300 (TBD, configurable)
 * microsecond before the Millisec task, so that the data will be ready for the Millisec handling.
 * the ,millisecond task holds the pointer to a double buffer of results, so that the Millisec will handle the
 * most updated data, without disturbing data gathering
 *
 **************************************************************************************************************************/

////////////////////////////////State machine operation////////////////////////////////////
//the state machine operation is used to operate in runtime correct profile flow execution
//by recieved esign flow of the user from the UI
///////////////////////////////////////////////////////////////////////////////////////////
#include "include.h"
#include "Modules/General/GeneralHardware.h"

#include "MillisecTask.h"
#include <driverlib/timer.h>
#include <inc/hw_ints.h>

#include "drivers/adc_sampling/adc.h"
#include "control.h"

#include "drivers/Motors/Motor.h"
#include "drivers/Danser_SSI/SSI_Comm.h"
#include "drivers/Heater/TemperatureSensor.h"

/******************** Definitions  ********************************************/
#define INVALID_MSG_ID    0xFFFF
#define MAX_TANGO_CONTROL_DEVICES 200
/******************** STRUCTURES AND ENUMs  ********************************************/
typedef struct MillisecMotorData
{
    bool Active;
    bool WaitForData;
    bool DataRequired;
    MSecFptr Callback;
    unsigned long Data;
    int Length;
}MillisecMotorDataStruc;

typedef enum
{
    OneMillisec,
}MillisecMessages;

typedef struct MillisecMessage{
    uint16_t messageId;
    uint16_t msglen;
    uint32_t tick;
    uint8_t messageData[20];
}MillisecMessageStruc;

uint32_t  ADC_Data[MAX_ADC_DEVICES] = {0};
uint32_t  TemperatureSensor_Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
uint32_t  MotorSpeed_Data[MOTOR_SPARE1_1] = {0};
uint32_t  MotorStatus_Data[MOTOR_SPARE1_1] = {0};
uint32_t  MotorPosition_Data[MOTOR_SPARE1_1] = {0};
bool      MotorBusy_Data[MOTOR_SPARE1_1] = {true};
uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};

MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};

/******************** GLOBAL PARAMETERS  ********************************************/
Mailbox_Handle          MillisecMsgQ = NULL;
bool                    MillisecRestart;
static GateMutex_Handle gateMillisecDB;

uint32_t                MillisecDatalog[MAX_TANGO_CONTROL_DEVICES];
uint32_t        Millisec_timerBase = TIMER1_BASE;        //Timer handle
/******************** Functions  ********************************************/
uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
//**********************************************************************
/******************** CODE  ********************************************/
//**********************************************************************

void MillisecInit(void)
{
    Error_Block eb;

    MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 20, NULL,NULL);

    MillisecRestart = false;

    memset(MillisecDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES);

    int i;
    for (i=0;i<NUM_OF_MOTORS;i++)
    {
        MotorData[i].Active = false;
    }
    gateMillisecDB = GateMutex_create(NULL, &eb);
    if (gateMillisecDB == NULL)
    {
        System_abort("Could not create USB Wait gate");
    }

    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
    ROM_TimerConfigure(Millisec_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
    //TimerIntRegister(Millisec_timerBase, TIMER_A, Timer0Isr);    // Registering  isr
    ROM_TimerEnable(Millisec_timerBase, TIMER_A);
    ROM_IntEnable(INT_TIMER1A);
    ROM_TimerIntEnable(Millisec_timerBase, TIMER_TIMA_TIMEOUT);

    ADCAcquireInit();

    return;
}
void MillisecStop(void)
{
    MillisecRestart = false;
    ADCAcquireStop();
}

void MillisecStart(void)
{
    MillisecRestart = true;
    ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,120000/*one millisecond*/);
    ADCAcquireStart(0,1);
}


void OneMilliSecondMillisecInterrupt(UArg arg0)
{
    MillisecMessageStruc Message;
    bool retcode = false;
    ROM_IntMasterDisable();
    if (MillisecRestart == true)
    {
        ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,120000/*one millisecond*/);
    }
    else
        ROM_TimerDisable(Millisec_timerBase,TIMER_A);

    //send message to the Millisec task
    Message.messageId = OneMillisec;
    Message.tick = UsersysTickGet();
    Message.msglen = sizeof(MillisecMessageStruc);
    if (MillisecMsgQ != NULL)
        retcode = Mailbox_post(MillisecMsgQ , &Message, BIOS_NO_WAIT);

    ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
    //
    // Enable all interrupts.
    //
    ROM_IntMasterEnable();
    return ;
}
//typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
{
    if (MotorId >= NUM_OF_MOTORS) return -1;
    if (MotorData[MotorId].Active == true) return -2;
    MotorData[MotorId].Callback = Callback;
    MotorData[MotorId].Data = Data;
    MotorData[MotorId].Length = Length;
    MotorData[MotorId].Active = true;
    MotorData[MotorId].DataRequired = false;
    return OK;
}
int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
{
    if (MotorId >= NUM_OF_MOTORS) return -1;
    if (MotorData[MotorId].Active == true) return -2;
    MotorData[MotorId].Callback = Callback;
    MotorData[MotorId].Data = Data;
    MotorData[MotorId].Length = Length;
    MotorData[MotorId].Active = true;
    MotorData[MotorId].DataRequired = true;
    return OK;
}
uint32_t MillisecLoop(uint32_t tick)
{
    int Motor_i;
    unsigned int MotorInfo = 0;
    //call all modules Millisec functions
    //test dancers and speed encoders
    //check all callback units (state machine waiting for completion of a change)
    bool Ten_msTick, Hundred_msTick, Onesecond_Tick;
    Ten_msTick      =   (tick%eTenMilliSecond == 0)      ?true:false;
    Hundred_msTick  =   (tick%eHunderdMillisecond == 0)  ?true:false;
    Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;

    //gather Motor data from FPGA
    for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
    {
        if (MotorData[Motor_i].WaitForData == true) //Read request sent, data is waiting
        {
            if (MotorGetFPGAResponse(Motor_i,&MotorInfo) == OK) //got the data from the FPGA
            {
                MotorData[Motor_i].WaitForData = false;
                MotorData[Motor_i].Callback(Motor_i,MotorInfo);
            }
        }
        if (MotorData[Motor_i].Active == true) //new data to send
        {
            if (MotorSendFPGARequest(Motor_i,MotorData[Motor_i].Data,MotorData[Motor_i].Length) == OK) //sent the data to the FPGA
            {
                MotorData[Motor_i].Active = false; //set the Active to false first, because the callback might send a new request immediately
                if (MotorData[Motor_i].DataRequired == true)
                {
                    MotorData[Motor_i].WaitForData = true; // mark the motor for data request next round
                }
                else
                {
                    MotorData[Motor_i].Callback(Motor_i,0); // call the callback to report execution
                }
            }
        }
    }
    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);

#ifndef EVALUATION_BOARD
    /* this cannot be done within one millisecond, and not needed
     * instead, check if there is a motor waiting with data to send or read request
     * MotorSpeed_Data[MOTOR_DRYER_DRIVING] = MotorGetSpeedFromFPGA(MOTOR_DRYER_DRIVING);
    MotorStatus_Data[MOTOR_DRYER_DRIVING] = MotorGetStatusFromFPGA(MOTOR_DRYER_DRIVING);
    MotorSpeed_Data[MOTOR_SCREW] = MotorGetSpeedFromFPGA(MOTOR_SCREW);
    MotorStatus_Data[MOTOR_SCREW] = MotorGetStatusFromFPGA(MOTOR_SCREW);
    MotorSpeed_Data[MOTOR_WINDER] = MotorGetSpeedFromFPGA(MOTOR_WINDER);
    MotorStatus_Data[MOTOR_WINDER] = MotorGetStatusFromFPGA(MOTOR_WINDER);
    MotorSpeed_Data[MOTOR_LDRIVING] = MotorGetSpeedFromFPGA(MOTOR_LDRIVING);
    MotorStatus_Data[MOTOR_LDRIVING] = MotorGetStatusFromFPGA(MOTOR_LDRIVING);
    MotorSpeed_Data[MOTOR_RDRIVING] = MotorGetSpeedFromFPGA(MOTOR_RDRIVING);
    MotorStatus_Data[MOTOR_RDRIVING] = MotorGetStatusFromFPGA(MOTOR_RDRIVING);*/
    //gather Dancer data from FPGA
    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER,0);
    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER,0);
    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER,0);
    //gather data from FPGA
    if (Ten_msTick)
    {
        MotorPosition_Data[MOTOR_SCREW] = MotorGetPositionFromFPGA(MOTOR_SCREW);
        MotorPosition_Data[MOTOR_RDRIVING] = MotorGetPositionFromFPGA(MOTOR_RDRIVING);
    }
    if (Hundred_msTick)
    {
        int adc_i;
        for (adc_i = 0; adc_i < MAX_ADC_DEVICES ; adc_i++)
            ADC_Data[adc_i] = ADC_GetReading(adc_i);
        //trigger the ADC collection - check and set priorities to make sure handling timing is correct.
        //we might want to call it from the task, after execution of other tasks!!!
        ADC_TriggerCollection();

        TEMPERATURE_SENSOR_ID_ENUM pt100_i;
        for (pt100_i = 0; pt100_i < (int)MAX_TEMPERATURE_SENSOR_ID ; pt100_i++)
            TemperatureSensor_Data[pt100_i] = TemperatureSensorReadFromFPGA(pt100_i);
        MotorGetnBusyFromFPGA(); // get all motors nBusy bit status from the FPGAs
        TimerMotors_t motor_i;
        for (motor_i = 0; motor_i < MOTOR_SPARE1_1 ; motor_i++)
        {
            MotorBusy_Data[motor_i] = MotorGetnBusyState(motor_i);
            if (MotorBusy_Data[motor_i] == false) //can get data
            {
                MotorSpeed_Data[motor_i] = MotorGetSpeedFromFPGA(motor_i);
                MotorStatus_Data[motor_i] = MotorGetStatusFromFPGA(motor_i);
            }
        }
    }
#endif
    //gather data from FPGA

    return OK;
}
/******************************************************************************
 *  ======== messageTsk ========
 *  Task for this function is created statically. See the project's .cfg file.
 *  this message task is created statically in system initialization,
 ******************************************************************************/
void MillisecTask(UArg arg0, UArg arg1)
{
    MillisecMessageStruc Message;
    //char str[60];
    //uint16_t length;
    //Clock_setTimeout(HostKAClock, 1000);
    //Clock_start(HostKAClock);
    MillisecInit();

    while(1)
    {
        Mailbox_pend(MillisecMsgQ , &Message, BIOS_WAIT_FOREVER);
        switch (Message.messageId)
        {
            case OneMillisec:
                MillisecLoop(Message.tick);
                break;
            default:
                break;
        }
    }
}


uint32_t getMotorStatusData(int MotorId)
{
    assert (MotorId < MOTOR_SPARE1_1);
    return MotorStatus_Data[MotorId];
}
uint32_t getMotorSpeedData(int MotorId)
{
    assert (MotorId < MOTOR_SPARE1_1);
    return MotorSpeed_Data[MotorId];
}

uint32_t getTemperatureSensorData(int SensorId)
{
    assert (SensorId < MAX_TEMPERATURE_SENSOR_ID);
    return TemperatureSensor_Data[SensorId];
}

uint32_t getADCData(int DeviceId)
{
    assert (DeviceId < MAX_ADC_DEVICES);
    return ADC_Data[DeviceId];
}

/********************************************************************
*
*    Name        : GTIME_Delta_Time_Pass
*
*    Parameters  : start_time.
*
*    Return      : time pass from start time
*
*    Description :
*
*********************************************************************/

uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read)
{
    uint32_t Time_Pass;
  #define   MAX_COUNTER 0x3FFF  //14 bits


  if (Current_Read < Previous_Read)
    Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1;
  else
    Time_Pass = Current_Read - Previous_Read;

  return (Time_Pass);
}