/************************************************************************************************************************
 * 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 design flow of the user from the UI
///////////////////////////////////////////////////////////////////////////////////////////
#include "include.h"
#include "Modules/General/GeneralHardware.h"
#include "Modules/AlarmHandling/AlarmHandling.h"

#include "MillisecTask.h"
#include <driverlib/timer.h>
#include <Drivers/SSI_Comm/SSI_Comm.h>
#include <Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.h>
#include <Drivers/SSI_Comm/Dancer/Dancer.h>
#include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
#include <inc/hw_ints.h>

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

#include "drivers/Motors/Motor.h"
#include "drivers/Heater/TemperatureSensor.h"
#include "drivers/FPGA/FPGA_SPI_Comm.h"
#include "drivers/FPGA/FPGA.h"
#include "drivers/Valves/Valve.h"
#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"

#include "modules/thread/thread_ex.h"
#include "modules/ids/ids_ex.h"

#include "Communication/Connection.h"

Task_Handle Millisecond_Task_Handle;
/******************** Definitions  ********************************************/
#define INVALID_MSG_ID    0xFFFF
#define MAX_TANGO_CONTROL_DEVICES 200
/******************** STRUCTURES AND ENUMs  ********************************************/
typedef struct MillisecMotorData
{
    bool WaitForData;
    bool DataRequired;
    bool SyncRequired;
    MSecFptr Callback;
    unsigned long Data;
    int Length;
    bool Active;
}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[NUM_OF_MOTORS] = {0};
//uint32_t  MotorStatus_Data[NUM_OF_MOTORS] = {0};
//uint32_t  MotorPosition_Data[NUM_OF_MOTORS] = {0};
//bool      MotorBusy_Data[NUM_OF_MOTORS] = {true};
uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};
float  Speed_Data = 0;
uint32_t DrawerFansStatus = 0;

bool watchdogCriticalAlarm = false;

uint32_t msec_millisecondCounter = 0;

MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0};
MillisecMotorDataStruc ScrewMovePending = {0};
MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0};
MillisecMotorDataStruc PT100Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
/******************** GLOBAL PARAMETERS  ********************************************/
Mailbox_Handle          MillisecMsgQ = NULL;
Mailbox_Handle          MotorsMsgQ[NUM_OF_MOTORS] = {NULL};
bool                    MillisecRestart;
static GateMutex_Handle gateMillisecDB;

uint32_t        Millisec_timerBase = TIMER1_BASE;        //Timer handle
/******************** Functions  ********************************************/
uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
//**********************************************************************
/******************** CODE  ********************************************/
//**********************************************************************
uint32_t MSBacklog[200]={0};
uint8_t Motor_Id[200]={0};
uint32_t MSTick[200]={0};
uint16_t MsecLogindex = 0;

void MillisecInit(void)
{
    Error_Block eb;
    int i;

    Error_init(&eb);

    MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 2, NULL,&eb);
    for (i=0;i<NUM_OF_MOTORS;i++)
    {
        MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 5, NULL,&eb);
    }

    MillisecRestart = 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*/);
    ROM_TimerEnable(Millisec_timerBase, TIMER_A);
    ROM_IntEnable(INT_TIMER1A);
    ROM_TimerIntEnable(Millisec_timerBase, TIMER_TIMA_TIMEOUT);
    ADCAcquireStart(0,1);
}

void OneMilliSecondMillisecInterrupt(UArg arg0)
{
    MillisecMessageStruc Message;
    ROM_IntMasterDisable();
    ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
    if (MillisecRestart == true)
    {
        ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,120000/*one millisecond*/);
    }
    else
	{
        ROM_TimerDisable(Millisec_timerBase,TIMER_A);
	    ROM_IntMasterEnable();
		return;
	}
    //send message to the Millisec task
    Message.messageId = OneMillisec;
    Message.tick = msec_millisecondCounter++;;
    Message.msglen = sizeof(MillisecMessageStruc);
    if (MillisecMsgQ != NULL)
        Mailbox_post(MillisecMsgQ , &Message, BIOS_NO_WAIT);

    //
    // Enable all interrupts.
    //
    ROM_IntMasterEnable();
    return ;
}
uint32_t PT100Activity = 0;
int32_t MillisecReadFromTempSensor(uint32_t TempSensorId,  MSecFptr Callback)
{
    if (TempSensorId >= MAX_TEMPERATURE_SENSOR_ID) return -1;
    PT100Activity++; //read request
    PT100Data[TempSensorId].Callback = Callback;
    PT100Data[TempSensorId].Active = true;

    return OK;
}
//typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
uint32_t MotorActivity = 0;

int32_t MillisecMoveScrew(unsigned long Data, int Length, MSecFptr Callback)
{
    //==========================
    MSBacklog[MsecLogindex]=Data;
    Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
    MSTick[MsecLogindex]=msec_millisecondCounter;
    MsecLogindex++;
    if (MsecLogindex>=199)
        MsecLogindex = 0;
    LOG_ERROR(Data, "MillisecMoveScrew");

    //==========================

    ScrewMovePending.Callback = Callback;
    ScrewMovePending.Data = Data;
    ScrewMovePending.Length = Length;
    ScrewMovePending.DataRequired = false;
    if (ScrewMovePending.Active == false)
    {
        MotorActivity++;
        ScrewMovePending.Active = true;
    }

    return OK;
}

int32_t MillisecSetScrewSpeed(unsigned long Data, int Length, MSecFptr Callback)
{
    //==========================
    MSBacklog[MsecLogindex]=Data;
    Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
    MSTick[MsecLogindex]=msec_millisecondCounter;
    MsecLogindex++;
    if (MsecLogindex>=199)
        MsecLogindex = 0;
    //==========================
    LOG_ERROR(Data, "MillisecSetScrewSpeed");

    ScrewSetMaxSpeedPending.Callback = Callback;
    ScrewSetMaxSpeedPending.Data = Data;
    ScrewSetMaxSpeedPending.Length = Length;
    ScrewSetMaxSpeedPending.DataRequired = false;
    if (ScrewSetMaxSpeedPending.Active == false)
    {
        MotorActivity++;
        ScrewSetMaxSpeedPending.Active = true;
    }

    return OK;
}
int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
{
    if (MotorId >= NUM_OF_MOTORS) return -1;
    //==========================
    MSBacklog[MsecLogindex]=Data;
    MSTick[MsecLogindex]=msec_millisecondCounter;
    Motor_Id[MsecLogindex]=MotorId;
    MsecLogindex++;
    if (MsecLogindex>=199)
        MsecLogindex = 0;
    //==========================

    SpeedSetPending[MotorId].Callback = Callback;
    SpeedSetPending[MotorId].Data = Data;
    SpeedSetPending[MotorId].Length = Length;
    SpeedSetPending[MotorId].DataRequired = false;
    if (SpeedSetPending[MotorId].Active == false)
    {
        MotorActivity++;
        SpeedSetPending[MotorId].Active = true;
    }

    return OK;
}
int MillisecFlushMsgQ(TimerMotors_t MotorId)
{
    MillisecMotorDataStruc MotorData = {0};
    int pend = Mailbox_getNumPendingMsgs(MotorsMsgQ[MotorId]);
    int i;
    if (pend)
    {
        for (i=0;i<pend;i++)
            Mailbox_pend(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
    }
   return pend;
}
int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
{
    MillisecMotorDataStruc MotorData = {0};
    //==========================
    MSBacklog[MsecLogindex]=Data;
    MSTick[MsecLogindex]=msec_millisecondCounter;
    Motor_Id[MsecLogindex]=MotorId;
    MsecLogindex++;
    if (MsecLogindex>=199)
        MsecLogindex = 0;
    //==========================
    if (MotorId >= NUM_OF_MOTORS) return -1;
    MotorActivity++;
    MotorData.Callback = Callback;
    MotorData.Data = Data;
    MotorData.Length = Length;
    MotorData.DataRequired = false;
    if (MotorsMsgQ[MotorId] != NULL)
        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
    else return false;
}
int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
{
    MillisecMotorDataStruc MotorData = {0};
    if (MotorId >= NUM_OF_MOTORS) return -1;
//==========================
    MSBacklog[MsecLogindex]=Data;
    Motor_Id[MsecLogindex]=MotorId;
    MSTick[MsecLogindex]=msec_millisecondCounter;
    MsecLogindex++;
    if (MsecLogindex>=199)
        MsecLogindex = 0;
    //==========================
    MotorActivity++;
    MotorActivity++;
    MotorData.Callback = Callback;
    MotorData.Data = Data;
    MotorData.Length = Length;
    MotorData.DataRequired = true;
    if (MotorsMsgQ[MotorId] != NULL)
        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
    else return false;
}

//TEMPERATURE_SENSOR_ID_ENUM Sensor_Read = 0;
uint32_t MillisecLoop(uint32_t tick)
{
    uint8_t Motor_i,Disp_i;
    TEMPERATURE_SENSOR_ID_ENUM Sensor_i;
    unsigned int MotorInfo = 0;
    static int temp=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,O900Millisecond_Tick,Tick98,OneMinute_Tick;
    Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
    Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
    O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
    Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
    OneMinute_Tick  =   (tick%eOneMinute == 0)           ?true:false;
    Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
    //gather Motor data from FPGA
    //ROM_IntMasterDisable();

#ifndef EVALUATION_BOARD
        FPGA_GetBusy();  //load the busy motor information to all motors
        FPGA_Read_limit_Switches_Registers();
        //Read_FPGA_GPI_Rgisters();//FPGA_Read_limit_Switches();
#endif

        temp += MotorActivity;
   if (MotorActivity)
   {
        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
        {
            if (MotorDriverResponse[Motor_i].Busy == true)
            {
                temp++;
                continue;
            }
            if (MotorData[Motor_i].WaitForData == true) //Read request sent, data is waiting
            {
                if (MotorGetFPGAResponse((HardwareMotorType)Motor_i,&MotorInfo) == OK) //got the data from the FPGA
                {
                    MotorData[Motor_i].WaitForData = false;
                    if (MotorData[Motor_i].Callback)
                        MotorData[Motor_i].Callback(Motor_i,MotorInfo);
                }
                MotorActivity--;
            }

            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
            {
                if (ScrewSetMaxSpeedPending.Active == true)
                {
                    MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSetMaxSpeedPending.Data,ScrewSetMaxSpeedPending.Length);
                    MotorActivity--;
                    ScrewSetMaxSpeedPending.Active = false;
                    if (ScrewSetMaxSpeedPending.Callback)
                        ScrewSetMaxSpeedPending.Callback(Motor_i,0);
                    continue;
                }
                else if (ScrewMovePending.Active == true)
                {
                    MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewMovePending.Data,ScrewMovePending.Length);
                    MotorActivity--;
                    /*if (Screwindex>=50)
                        Screwindex = 0;*/
                    ScrewMovePending.Active = false;
                    if (ScrewMovePending.Callback)
                        ScrewMovePending.Callback(Motor_i,0);
                    continue;
                }
            }
            if (SpeedSetPending[Motor_i].Active == true)
            {
                MotorSendFPGARequest((HardwareMotorType)Motor_i,SpeedSetPending[Motor_i].Data,SpeedSetPending[Motor_i].Length);
                MotorActivity--;
                SpeedSetPending[Motor_i].Active = false;
                if (SpeedSetPending[Motor_i].Callback)
                    SpeedSetPending[Motor_i].Callback(Motor_i,0);
            }
            else if (Mailbox_pend(MotorsMsgQ[Motor_i] , &MotorData[Motor_i], BIOS_NO_WAIT)==true)
            {
                if (MotorSendFPGARequest((HardwareMotorType)Motor_i,MotorData[Motor_i].Data,MotorData[Motor_i].Length) == OK) //sent the data to the FPGA
                {
                    if (MotorData[Motor_i].DataRequired == true)
                    {
                        MotorData[Motor_i].WaitForData = true; // mark the motor for data request next round
                    }
                    else
                    {
                        if (MotorData[Motor_i].Callback)
                            MotorData[Motor_i].Callback(Motor_i,0); // call the callback to report execution
                    }
                }
                MotorActivity--;
            }
        }
   }
    //FPGA_GetTempSensorBusy();
   if (PT100Activity)
   {
       for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
       {
           //if (TempDriverDriverResponse[Sensor_i].Busy == true)
           //    continue;
           if (PT100Data[Sensor_i].Active == true)
           {
               TemperatureSendSensorDummyClk(Sensor_i);
               PT100Data[Sensor_i].Active = false;
               PT100Data[Sensor_i].WaitForData = true; // mark the motor for data request next round
               break; //  one PT100 activitiy per MS 
           }
           else if (PT100Data[Sensor_i].WaitForData == true) //Read request sent, data is waiting
           {
               TemperatureSensorReadFromFPGA_Res(Sensor_i); //got the data from the FPGA
               PT100Data[Sensor_i].WaitForData = false;
               PT100Data[Sensor_i].SyncRequired = true;
               if (PT100Data[Sensor_i].Callback)
                       PT100Data[Sensor_i].Callback(Sensor_i,MotorInfo);
               break; // one PT100 activitiy per MS 
           }
           else if (PT100Data[Sensor_i].SyncRequired == true)
           {
               TemperatureSensorSync(Sensor_i);
               PT100Data[Sensor_i].SyncRequired = false;
               PT100Activity--;
               break; // one PT100 activitiy per MS 
           }
       }
   }
    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
    if (Ten_msTick)
    {
        //Speed_Data = Calculate_Speed_Sensor_Velocity();
        //MillisecReadFromTempSensor(Sensor_Read, NULL);
        //if (Sensor_Read++ >= MAX_TEMPERATURE_SENSOR_ID) Sensor_Read = 0;
    }
    if (Hundred_msTick)
    {
        Speed_Data = Calculate_Speed_Sensor_Velocity();
        for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
        {
            MillisecReadFromTempSensor(Sensor_i, NULL);
        }
    }

    if (O900Millisecond_Tick)
    {
        ADC_TriggerCollection();
    }
    if (Onesecond_Tick)
    {
        for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
        {
            CalculateDispenserPressure(Disp_i);
        }
        FPGA_GetAllDispensersValveBusyOCD();
        if (watchdogCriticalAlarm == false)
        {
                Control_WD(ENABLE,50);  //activate heaters/dispenser watchdog, 5 seconds
        }
        DrawerFansStatus = Read_Fans_Tacho();
        KeepAliveOneSecondCall();
    }
    if (OneMinute_Tick)
    {
        for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
        {
            Read_MidTank_Pressure_Sensor(Disp_i);
        }
        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
        {
            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
                continue;  //
            if (isMotorConfigured(Motor_i))
                MotorGetStatusFromFPGA(Motor_i);
        }
    }
    //ROM_IntMasterEnable();


    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();
    Millisecond_Task_Handle = Task_self();
    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 < NUM_OF_MOTORS);
    return MotorStatus_Data[MotorId];
}*/
/*uint32_t getMotorSpeedData(int MotorId)
{
    assert (MotorId < NUM_OF_MOTORS);
    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];
}*/
float getSensorSpeedData(void)
{
    return Speed_Data;
}

uint32_t getDrawerFansStatus(void)
{
    return DrawerFansStatus;
}