/*
 * AlarmHandlingDiagnostics.c
 *
 *  Created on: 26 june 2018
 *      Author: shlomo
 */
#include <DataDef.h>
#include "include.h"
#include <driverlib/timer.h>
#include <inc/hw_ints.h>

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

#include <Container.h>

#include "drivers/Motors/Motor.h"
#include "drivers/Heater/TemperatureSensor.h"
#include "drivers/SSI_Comm/Dancer/Dancer.h"
#include "drivers/FPGA/FPGA_SPI_Comm.h"
#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
#include "drivers/FPGA/FPGA.h"
#include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
#include "drivers/I2C_Communication/DAC/Blower.h"
#include "drivers/valves/valve.h"

#include "modules/ids/ids_ex.h"
#include "Modules/Control/MillisecTask.h"
#include "Modules/General/GeneralHardware.h"
#include "Modules/AlarmHandling/AlarmHandling.h"
#include "Modules/heaters/heaters_ex.h"
#include "modules/thread/thread_ex.h"
#include "modules/waste/waste.h"

#include "PMR/Hardware/HardwareDancerType.pb-c.h"
#include  <PMR/Diagnostics/StartDiagnosticsRequest.pb-c.h>
#include  <PMR/Diagnostics/StartDiagnosticsResponse.pb-c.h>
#include  <PMR/Diagnostics/StopDiagnosticsRequest.pb-c.h>
#include  <PMR/Diagnostics/StopDiagnosticsResponse.pb-c.h>
#include  <PMR/Diagnostics/Event.pb-c.h>
#include  <PMR/Diagnostics/EventType.pb-c.h>
#include  <PMR/Diagnostics/DigitalInterfaceState.pb-c.h>
#include  <PMR/Diagnostics/InterfaceIOs.pb-c.h>
#include "PMR/Diagnostics/HeaterType.pb-c.h"
#include "PMR/Diagnostics/HeaterState.pb-c.h"
#include  <PMR/debugging/DebugLogCategory.pb-c.h>

#include "StateMachines/Printing/PrintingSTM.h"

#include <drivers/FPGA/FPGA_Comm.h>

#include "diagnostics.h"

extern F2_CTRL_REG F2_CTRL_Reg;

char DiagnosticsToken[36+1] = {0};
bool DiagnosticsActive = false;

void SendDiagnostics(void);
uint32_t DiagnosticsStart(void);
uint32_t DiagnosticsStop(void);

uint32_t Diagnostics_ControlTrigger(uint32_t IfIndex, uint32_t ReadValue);
uint32_t Diagnostics_TenMiliControlTrigger(uint32_t IfIndex, uint32_t ReadValue);
uint32_t Diagnostics_OneSecControlTrigger(uint32_t IfIndex, uint32_t ReadValue);
uint32_t Diagnostics_OneMinControlTrigger(uint32_t IfIndex, uint32_t ReadValue);


DiagnosticsMonitors DiagnosticsMonitor = DIAGNOSTICS_MONITORS__INIT;
#define DIAGNOSTICS_LIMIT 3
#define DIAGNOSTICS_DANCER_LIMIT 30
int DiagnosticsIndex = 0;
int DiagnosticCollectionLimit = 2;  //number of data samples to collect before sending to the host
//int DiagnosticLimit = eHundredMillisecond; //frequency of data collection
//int DiagnosticFastLimit = eTenMillisecond; //frequency of data collection
int DiagnosticLimit =eHundredMillisecond; //frequency of data collection

int DiagnosticFastLimit = eTenMillisecond; //frequency of data collection
//DigitalPin DigitalPinArray[1][DIAGNOSTICS_LIMIT+1];
double dancer1angle[DIAGNOSTICS_LIMIT+1];
double dancer2angle[DIAGNOSTICS_LIMIT+1];
double dancer3angle[DIAGNOSTICS_LIMIT+1];

double feedermotorfrequency[DIAGNOSTICS_LIMIT+1];
double dryermotor[DIAGNOSTICS_LIMIT+1];
double pollermotor[DIAGNOSTICS_LIMIT+1];
double windermotor[DIAGNOSTICS_LIMIT+1];
double screwmotor[DIAGNOSTICS_LIMIT+1];
double mixertemperature[DIAGNOSTICS_LIMIT+1];
double headzone1temperature[DIAGNOSTICS_LIMIT+1];
double headzone2temperature[DIAGNOSTICS_LIMIT+1];
double headzone3temperature[DIAGNOSTICS_LIMIT+1];
double headzone4temperature[DIAGNOSTICS_LIMIT+1];
double headzone5temperature[DIAGNOSTICS_LIMIT+1];
double headzone6temperature[DIAGNOSTICS_LIMIT+1];
double dryerzone1temperature[DIAGNOSTICS_LIMIT+1];
double dryerzone2temperature[DIAGNOSTICS_LIMIT+1];
double dryerzone3temperature[DIAGNOSTICS_LIMIT+1];


double dispensermotorfrequency[MAX_SYSTEM_DISPENSERS][DIAGNOSTICS_LIMIT+1];
double dispenserspressure[MAX_SYSTEM_DISPENSERS][DIAGNOSTICS_LIMIT+1];
double dispenserinklevel[MAX_SYSTEM_DISPENSERS][1];
double midtankinklevel[MAX_SYSTEM_DISPENSERS][1];
double MidTankpressure[MAX_SYSTEM_DISPENSERS][1];
double HeatersCurrent[NUM_OF_CURRENT_HEATERS][1];
double VOC_Sensor;

DoubleArray DispenserFreq[MAX_SYSTEM_DISPENSERS];
DoubleArray DiagnosticsDispenserPressure[MAX_SYSTEM_DISPENSERS];
DoubleArray DispenserInkLevel[MAX_SYSTEM_DISPENSERS];
DoubleArray DiagnosticsMidTankInkLevel[MAX_SYSTEM_DISPENSERS];

HeaterState **heatersstates;
HeaterState  HeaterInfo[HEATER_TYPE__MixerHeater+1];

#define NUM_OF_INTERFACE_IOS 21
DigitalInterfaceState **digitalinterfacestates;
DigitalInterfaceState DigitalOutputState[NUM_OF_INTERFACE_IOS];
/*double dispenser1motorfrequency[DIAGNOSTICS_LIMIT+1];
double dispenser2motorfrequency[DIAGNOSTICS_LIMIT+1];
double dispenser3motorfrequency[DIAGNOSTICS_LIMIT+1];
double dispenser4motorfrequency[DIAGNOSTICS_LIMIT+1];
double dispenser5motorfrequency[DIAGNOSTICS_LIMIT+1];
double dispenser6motorfrequency[DIAGNOSTICS_LIMIT+1];
double dispenser7motorfrequency[DIAGNOSTICS_LIMIT+1];
double dispenser8motorfrequency[DIAGNOSTICS_LIMIT+1];
*/
Task_Handle Diagnostics_Task_Handle;
Mailbox_Handle          DiagnosticsMsgQ = NULL;

bool blowervolatgedisplay = false;
bool midtankDisplay = false;
double diagvoltage = 0;
/******************** Functions  ********************************************/
//uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
//**********************************************************************
typedef enum
{
    DiagnosticsTrigger,
    DiagnosticsTenMiliTrigger,
    DiagnosticsOneSecTrigger,
    DiagnosticsOneMinuteTrigger
}DiagnosticsMessages;

typedef struct DiagnosticsMessage{
    uint16_t messageId;
    uint16_t msglen;
    uint32_t tick;
    uint8_t messageData[20];
}DiagnosticsMessageStruc;

#define MAX_DIAG_LOG 100
int diag_index=0;
uint32_t diag_size[MAX_DIAG_LOG];
uint32_t diag_time[MAX_DIAG_LOG];

Diagnostic_Mode DiagnosticMode = Diagnostic_Normal_Mode;

/******************** CODE  ********************************************/
//**********************************************************************

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

    Error_init(&eb);

    DiagnosticsMsgQ = Mailbox_create(sizeof(DiagnosticsMessageStruc), 40, NULL,&eb);

    DiagnosticsMonitor.dispensersmotorsfrequency = (DoubleArray**)my_malloc(sizeof(DoubleArray *)*MAX_SYSTEM_DISPENSERS);
    DiagnosticsMonitor.dispenserspressure = (DoubleArray**)my_malloc(sizeof(DoubleArray)*MAX_SYSTEM_DISPENSERS);
    DiagnosticsMonitor.midtanksinklevel = (DoubleArray**)my_malloc(sizeof(DoubleArray)*MAX_SYSTEM_DISPENSERS);
    DiagnosticsMonitor.dispensersinklevel = (DoubleArray**)my_malloc(sizeof(DoubleArray)*MAX_SYSTEM_DISPENSERS);
    for (i = 0; i<MAX_SYSTEM_DISPENSERS;i++)
    {
        double_array__init(&DispenserFreq[i]);
        double_array__init(&DiagnosticsDispenserPressure[i]);
        double_array__init(&DiagnosticsMidTankInkLevel[i]);
        double_array__init(&DispenserInkLevel[i]);
    }
    heatersstates = (HeaterState**)my_malloc(sizeof(HeaterState *)*(HEATER_TYPE__MixerHeater+1));
    for(i=0;i<=HEATER_TYPE__MixerHeater;i++)
    {
        heater_state__init(&HeaterInfo[i]);
        heatersstates[i] = &HeaterInfo[i];
    }
    digitalinterfacestates = (DigitalInterfaceState**)my_malloc(sizeof(DigitalInterfaceState *)*(MAX_HEATERS_NUM+8));
    for (i = 0;i<NUM_OF_INTERFACE_IOS;i++ )
    {
        digital_interface_state__init(&DigitalOutputState[i]);
        DigitalOutputState[i].has_interfaceio = true;
        DigitalOutputState[i].has_value = true;
        digitalinterfacestates[i] = &DigitalOutputState[i];
    }

    return;
}
/*typedef enum
{
    Diagnostic_Minimal_Mode,
    Diagnostic_Normal_Mode,
    Diagnostic_Extended_Mode,
    Diagnostic_Extreme_Mode
}Diagnostic_Mode;
if (DiagnosticMode == Diagnostic_Minimal_Mode)
if (DiagnosticMode == Diagnostic_Normal_Mode)
if (DiagnosticMode == Diagnostic_Extended_Mode)
if (DiagnosticMode == Diagnostic_Extreme_Mode)
*/
void SetDiagnosticMode(Diagnostic_Mode Mode)
{
    DiagnosticMode = Mode;
    REPORT_MSG(Mode,"Diagnostic Mode set");
}

void  SetDiagnosticCollectionLimit(int limit)
{
    if ((limit)&&(limit<= DIAGNOSTICS_LIMIT))
        DiagnosticCollectionLimit = limit;
    DiagnosticLimit =eHundredMillisecond; //frequency of data collection

    if (DiagnosticMode <= Diagnostic_Normal_Mode)
    {
        DiagnosticCollectionLimit = 1;  //overrule - send at least once every second
        DiagnosticLimit =eOneSecond; //frequency of data collection
    }
}

uint8_t HeaterCounterIndex[MAX_HEATERS_NUM]= {0,0,0,0,0,0,0,0,0,0};
double HeaterTemperature[MAX_HEATERS_NUM][DIAGNOSTICS_LIMIT+1];
uint8_t  DancerCounterIndex[NUM_OF_DANCERS]= {0,0,0};
double DancerValue[NUM_OF_DANCERS][DIAGNOSTICS_DANCER_LIMIT+1];
int8_t  DancerErrorCounterIndex[NUM_OF_DANCERS]= {0,0,0};
double DancerErrorValue[NUM_OF_DANCERS][DIAGNOSTICS_DANCER_LIMIT+1];
uint8_t  MotorCounterIndex[MAX_THREAD_MOTORS_NUM]= {0,0,0};
double MotorValue[MAX_THREAD_MOTORS_NUM][DIAGNOSTICS_LIMIT+1];
uint8_t  SpeedCounterIndex= 0;
double SpeedValue[DIAGNOSTICS_DANCER_LIMIT+1];

void DiagnosticLoadMotor(int MotorId, int frequency)
{
    if (MotorId >= MAX_THREAD_MOTORS_NUM)
    {
        LOG_ERROR(MotorId,"wrong Heater Id");
        return;
    }

    MotorValue[MotorId][MotorCounterIndex[MotorId]] = frequency;
    MotorCounterIndex[MotorId]++;
    if (MotorCounterIndex[MotorId]>=DIAGNOSTICS_LIMIT )
        MotorCounterIndex[MotorId] = 0;
}
uint32_t DiagnosticsLoadDigitalValues(void)
{
    uint32_t index = 0;
    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_DRYER_SSR3_CTRL;
    if(F2_CTRL_Reg.ushort & CLEANER_PUMP_SSR3_CTRL)
        DigitalOutputState[index++].value = true;
    else
        DigitalOutputState[index++].value = false;

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_DRYER_SSR1_CTRL;
    DigitalOutputState[index++].value = GetHeaterState(1);

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_DRYER_SSR2_CTRL;
    DigitalOutputState[index++].value = GetHeaterState(2);

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_DYEINGH_SSR8_CTRL;
    DigitalOutputState[index++].value = GetHeaterState(3);

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_DYEINGH_SSR7_CTRL;
    DigitalOutputState[index++].value = GetHeaterState(4);

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_DYEINGH_SSR6_CTRL;
    DigitalOutputState[index++].value = GetHeaterState(5);

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_DYEINGH_SSR5_CTRL;
    DigitalOutputState[index++].value = GetHeaterState(6);

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_DILUTORPUMP_SSR10_CTRL;
    DigitalOutputState[index++].value = GetHeaterState(7);

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_EXTWINDER_SSR11_CTRL;
    DigitalOutputState[index++].value = GetHeaterState(8);

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_MIXCHIP_SSR4_CTRL;
    DigitalOutputState[index++].value = GetHeaterState(9);

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_SPARE_SSR13_CTRL;
    DigitalOutputState[index++].value = ReadHeadMagnetBit();

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_LED4;
    DigitalOutputState[index++].value = DataUpdated;

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPI_WCONTAINER_FULL;
    DigitalOutputState[index++].value = WHS_IsContainerFull();

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPI_WCONTAINER_WARN;
    DigitalOutputState[index++].value = WHS_IsContainerEmpty();

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_SPARE1_1; /*waste lower cartridge presence*/
    DigitalOutputState[index++].value = WHS_WasteCartridgeLowerPresent();

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_SPARE1_2; /*waste middle cartridge presence*/
    DigitalOutputState[index++].value = WHS_WasteCartridgeMiddlePresent();

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_SPARE_SSR12_CTRL; /*lubricant valve*/
    DigitalOutputState[index++].value = GetLubricantState();

    DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_TFEED_BREAK_1; /*Secondary Pump Active*/
    DigitalOutputState[index++].value = SecondaryPumpActive;

    return index;
}
void DiagnosticLoadTemperature(int HeaterId, int temperature)
{
    double temp = temperature;
    if (HeaterId >= MAX_HEATERS_NUM)
    {
        LOG_ERROR(HeaterId,"wrong Heater Id");
        return;
    }
    if ((temperature < 1000)||(temperature>28000)) //error read, all 0xF or all 0
    {
        return;
    }
    HeaterTemperature[HeaterId][HeaterCounterIndex[HeaterId]] = temp/100;
    HeaterCounterIndex[HeaterId]++;
    if (HeaterCounterIndex[HeaterId]>=DIAGNOSTICS_LIMIT )
        HeaterCounterIndex[HeaterId] = 0;
}
void DiagnosticLoadSpeedSensor(float value)
{
    SpeedValue[SpeedCounterIndex] = (double)(value);
    if (SpeedCounterIndex<DIAGNOSTICS_DANCER_LIMIT )
        SpeedCounterIndex++;
    //if (SpeedCounterIndex>=DIAGNOSTICS_DANCER_LIMIT )
    //    SpeedCounterIndex = 0;
}

void DiagnosticLoadDancer(int DancerId, uint16_t value)
{
    if (DancerId >= MAX_HEATERS_NUM)
    {
        LOG_ERROR(DancerId,"wrong Dancer Id");
        return;
    }
    DancerValue[DancerId][DancerCounterIndex[DancerId]] = value;
    if (DancerCounterIndex[DancerId]<DIAGNOSTICS_DANCER_LIMIT )
        DancerCounterIndex[DancerId]++;
    //if (DancerCounterIndex[DancerId]>=DIAGNOSTICS_DANCER_LIMIT )
    //    DancerCounterIndex[DancerId] = 0;
}
void DiagnosticLoadDancerError(int DancerId, double value)
{
    if (DancerId >= MAX_HEATERS_NUM)
    {
        LOG_ERROR(DancerId,"wrong Dancer Id");
        return;
    }
    DancerErrorValue[DancerId][DancerErrorCounterIndex[DancerId]] = value;
    if (DancerErrorCounterIndex[DancerId]<DIAGNOSTICS_DANCER_LIMIT )
        DancerErrorCounterIndex[DancerId]++;
    //if (DancerErrorCounterIndex[DancerId]>=DIAGNOSTICS_DANCER_LIMIT )
    //    DancerErrorCounterIndex[DancerId] = 0;

}
void DiagnosticTenMsecCollection(void)
{
    if (DiagnosticsActive == false)
        return;
    /*if (JobIsActive()== false)
        return;*/

    DiagnosticLoadDancer(WINDER_DANCER,Control_Read_Dancer_Position(WINDER_DANCER, 0,0));
    DiagnosticLoadDancer(POOLER_DANCER,Control_Read_Dancer_Position(POOLER_DANCER, 0,0));
    DiagnosticLoadDancer(FEEDER_DANCER,Control_Read_Dancer_Position(FEEDER_DANCER, 0,0));

    DiagnosticLoadDancerError(WINDER_DANCER,ThreadGetMotorCalculatedError(WINDER_DANCER));
    DiagnosticLoadDancerError(POOLER_DANCER,ThreadGetMotorCalculatedError(POOLER_DANCER));
    DiagnosticLoadDancerError(FEEDER_DANCER,ThreadGetMotorCalculatedError(FEEDER_DANCER));

    //DiagnosticLoadSpeedSensor(getSensorSpeedData());
}
void DiagnosticOneMinuteCollection(void)
{
    int i;

    DiagnosticsMonitor.n_dispensersinklevel = 0;
    DiagnosticsMonitor.n_midtanksinklevel = 0;
    for (i=0;i<MAX_SYSTEM_DISPENSERS;i++)
    {
        MidTankpressure[i][0] = Get_MidTank_Pressure_Sensor(i);
        dispenserinklevel[i][0] = 1.00-(IDS_Dispenser_Data[i].consumedinnanolitter/5800000);
        midtankinklevel[i][0] = Get_MidTank_Pressure_Sensor(i);
        if (DiagnosticsMonitor.dispensersinklevel)
        {
            DiagnosticsMonitor.n_dispensersinklevel = MAX_SYSTEM_DISPENSERS;
            //DiagnosticsDispenserPressure[i] = dispenserspressure[i];
            //double_array__init(&DiagnosticsDispenserPressure[i]);
            DispenserInkLevel[i].data = dispenserinklevel[i];
            DispenserInkLevel[i].n_data = 1;
            DiagnosticsMonitor.dispensersinklevel[i] = &DispenserInkLevel[i];
        }
        if (DiagnosticsMonitor.midtanksinklevel)
        {
            DiagnosticsMonitor.n_midtanksinklevel = MAX_SYSTEM_DISPENSERS;
            //DiagnosticsDispenserPressure[i] = dispenserspressure[i];
            //double_array__init(&DiagnosticsDispenserPressure[i]);
            DiagnosticsMidTankInkLevel[i].data = midtankinklevel[i];
            DiagnosticsMidTankInkLevel[i].n_data = 1;
            DiagnosticsMonitor.midtanksinklevel[i] = &DiagnosticsMidTankInkLevel[i];
        }

    }

    DiagnosticsMonitor.n_midtank1level = 1;
    DiagnosticsMonitor.midtank1level = MidTankpressure[0];
    DiagnosticsMonitor.n_midtank2level = 1;
    DiagnosticsMonitor.midtank2level = MidTankpressure[1];
    DiagnosticsMonitor.n_midtank3level = 1;
    DiagnosticsMonitor.midtank3level = MidTankpressure[2];
    DiagnosticsMonitor.n_midtank4level = 1;
    DiagnosticsMonitor.midtank4level = MidTankpressure[3];
    DiagnosticsMonitor.n_midtank5level = 1;
    DiagnosticsMonitor.midtank5level = MidTankpressure[4];
    DiagnosticsMonitor.n_midtank6level = 1;
    DiagnosticsMonitor.midtank6level = MidTankpressure[5];
    DiagnosticsMonitor.n_midtank7level = 1;
    DiagnosticsMonitor.midtank7level = MidTankpressure[6];
    DiagnosticsMonitor.n_midtank8level = 1;
    DiagnosticsMonitor.midtank8level = MidTankpressure[7];

    for (i=0;i<NUM_OF_CURRENT_HEATERS;i++)
    {
        if (Read_Head_MixChip_Heaters_Current(i) == true)
        {
            HeatersCurrent[i][0] = Get_Heaters_Current(i);
            switch(i)
            {
            case HEATER_DRYER_CURRENT_1:
                DiagnosticsMonitor.n_drierzone1heatercurrent = 1;
                DiagnosticsMonitor.drierzone1heatercurrent = HeatersCurrent[HEATER_DRYER_CURRENT_1];
                break;
            case HEATER_DRYER_CURRENT_2:
                DiagnosticsMonitor.n_drierzone2heatercurrent = 1;
                DiagnosticsMonitor.drierzone2heatercurrent = HeatersCurrent[HEATER_DRYER_CURRENT_2];
                break;
            case HEATER_MIXCHIP_CURRENT:
                DiagnosticsMonitor.n_mixer1heatercurrent = 1;
                DiagnosticsMonitor.mixer1heatercurrent = HeatersCurrent[HEATER_MIXCHIP_CURRENT];
                break;
            case HEATER_HEAD_CURRENT_ZONE_1:
                DiagnosticsMonitor.n_headzone1heatercurrent = 1;
                DiagnosticsMonitor.headzone1heatercurrent = HeatersCurrent[HEATER_HEAD_CURRENT_ZONE_1];
                break;
            case HEATER_HEAD_CURRENT_ZONE_2:
                DiagnosticsMonitor.n_headzone2heatercurrent = 1;
                DiagnosticsMonitor.headzone2heatercurrent = HeatersCurrent[HEATER_HEAD_CURRENT_ZONE_2];
                break;
            case HEATER_HEAD_CURRENT_ZONE_3:
                DiagnosticsMonitor.n_headzone3heatercurrent = 1;
                DiagnosticsMonitor.headzone3heatercurrent = HeatersCurrent[HEATER_HEAD_CURRENT_ZONE_3];
                break;
            case HEATER_HEAD_CURRENT_ZONE_4:
                DiagnosticsMonitor.n_headzone4heatercurrent = 1;
                DiagnosticsMonitor.headzone4heatercurrent = HeatersCurrent[HEATER_HEAD_CURRENT_ZONE_4];
                break;
            case HEATER_HEAD_CURRENT_ZONE_5_6:
                DiagnosticsMonitor.n_headzone5_6heatercurrent = 1;
                DiagnosticsMonitor.headzone5_6heatercurrent = HeatersCurrent[HEATER_HEAD_CURRENT_ZONE_5_6];
                break;
            default:
                break;
            }
        }
    }

    VOC_Sensor = (double) getGasReading();
    DiagnosticsMonitor.n_filterdeltapressure = 1;
    DiagnosticsMonitor.filterdeltapressure = &VOC_Sensor;

}
void DiagnosticOneSecCollection(void)
{
    int i;
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__MixerHeater, MillisecGetTemperatures( MIXER_PT100));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DYEINGH_TEMP1));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DYEINGH_TEMP2));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DYEINGH_TEMP3));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DYEINGH_TEMP4));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DYEINGH_TEMP5));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, MillisecGetTemperatures(HEAD6_PT100));

    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerAirTemperature, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DRYER_TEMP1));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DRYER_TEMP2));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DRYER_TEMP3));
    for (i=0;i<MAX_SYSTEM_DISPENSERS;i++)
    {
        dispensermotorfrequency[i][DiagnosticsIndex] = IdsGetMotorSpeed(i);
        //dispensermotorfrequency[i][DiagnosticsIndex] = IDS_Dispenser_Data[i].consumedinnanolitter;

        dispenserspressure[i][DiagnosticsIndex] = GetDispenserPressure(i); //Read_MidTank_Pressure_Sensor

    }
}
void Diagnostic100msecCollection(void)
{
    int i;
    if (DiagnosticsActive == false)
        return;

    //DiagnosticLoadSpeedSensor(getSensorSpeedData());
    if (DiagnosticMode == Diagnostic_Extended_Mode)
        DiagnosticTenMsecCollection(); //call the 10msec function every 100 msec to reduce transport

    if (DiagnosticMode >= Diagnostic_Extended_Mode)
    {
        //if (JobIsActive())
        {
            DiagnosticLoadMotor(FEEDER_MOTOR, ThreadGetMotorSpeed (FEEDER_MOTOR));
            DiagnosticLoadMotor(DRYER_MOTOR, ThreadGetMotorSpeed (DRYER_MOTOR));
            DiagnosticLoadMotor(POOLER_MOTOR, ThreadGetMotorSpeed (POOLER_MOTOR));
            DiagnosticLoadMotor(WINDER_MOTOR, ThreadGetMotorSpeed (WINDER_MOTOR));
            DiagnosticLoadMotor(SCREW_MOTOR, ThreadGetMotorSpeed (SCREW_MOTOR));
        }
    }
    /*
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__MixerHeater, MillisecGetTemperatures( MIXER_PT100));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DYEINGH_TEMP1));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DYEINGH_TEMP2));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DYEINGH_TEMP3));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DYEINGH_TEMP4));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DYEINGH_TEMP5));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, MillisecGetTemperatures(HEAD6_PT100));

    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerAirTemperature, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DRYER_TEMP1));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DRYER_TEMP2));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary, MillisecGetTemperatures(TEMP_SENSE_ANALOG_DRYER_TEMP3));
    for (i=0;i<MAX_SYSTEM_DISPENSERS;i++)
    {
        dispensermotorfrequency[i][DiagnosticsIndex] = IdsGetMotorSpeed(i);
        //dispensermotorfrequency[i][DiagnosticsIndex] = IDS_Dispenser_Data[i].consumedinnanolitter;

        dispenserspressure[i][DiagnosticsIndex] = GetDispenserPressure(i); //Read_MidTank_Pressure_Sensor
        if (midtankDisplay)
            MidTankpressure[i][0] = Get_MidTank_Pressure_Sensor(i);
        else
            MidTankpressure[i][0] = 1.00-(IDS_Dispenser_Data[i].consumedinnanolitter/5800000);
    }
    */

   /*dispenser1motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1);
     dispenser2motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2);
     dispenser3motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3);
     dispenser4motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4);
     dispenser5motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_5);
     dispenser6motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6);
     dispenser7motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7);
     dispenser8motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8);
*/
    DiagnosticsIndex++;
    if (DiagnosticsIndex >= DIAGNOSTICS_LIMIT) DiagnosticsIndex = DIAGNOSTICS_LIMIT;
    if (DiagnosticsIndex >= DiagnosticCollectionLimit)
        SendDiagnostics();

}
void DiagnosticsReset(void)
{
    memset(DancerCounterIndex,0,sizeof(DancerCounterIndex));
    memset(MotorCounterIndex,0,sizeof(MotorCounterIndex));
    memset(DancerErrorCounterIndex,0,sizeof(DancerErrorCounterIndex));
    SpeedCounterIndex = 0;
    memset(HeaterCounterIndex,0,sizeof(HeaterCounterIndex));
    DiagnosticsIndex = 0;
}
int j=0;
MessageContainer diagnosticsresponseContainer;
uint8_t diagnostics_response_buffer[3500];
uint8_t *diagnostics_response_ptr = diagnostics_response_buffer;
char * diagnosticscontainer_buffer=0;
int LargeMessagesD = 0;

void SendDiagnostics(void)
{
    //MessageContainer responseContainer;
    StartDiagnosticsResponse response = START_DIAGNOSTICS_RESPONSE__INIT;
    //int i;

    if (DiagnosticsActive == false)
    {
        DiagnosticsReset();
        return;
    }
    if (SuspendLargeMessages == true)
    {
        LargeMessagesD++;
        DiagnosticsReset();
        return;
    }


    //if (JobIsActive())
    {
        DiagnosticsMonitor.n_dispenser1motorfrequency = DiagnosticsIndex;
        DiagnosticsMonitor.n_dispenser2motorfrequency = DiagnosticsIndex;
        DiagnosticsMonitor.n_dispenser3motorfrequency = DiagnosticsIndex;
        DiagnosticsMonitor.n_dispenser4motorfrequency = DiagnosticsIndex;
        DiagnosticsMonitor.n_dispenser5motorfrequency = DiagnosticsIndex;
        DiagnosticsMonitor.n_dispenser6motorfrequency = DiagnosticsIndex;
        DiagnosticsMonitor.n_dispenser7motorfrequency = DiagnosticsIndex;
        DiagnosticsMonitor.n_dispenser8motorfrequency = DiagnosticsIndex;
        DiagnosticsMonitor.dispenser1motorfrequency =   dispensermotorfrequency[0];
        DiagnosticsMonitor.dispenser2motorfrequency =   dispensermotorfrequency[1];
        DiagnosticsMonitor.dispenser3motorfrequency =   dispensermotorfrequency[2];
        DiagnosticsMonitor.dispenser4motorfrequency =   dispensermotorfrequency[3];
        DiagnosticsMonitor.dispenser5motorfrequency =   dispensermotorfrequency[4];
        DiagnosticsMonitor.dispenser6motorfrequency =   dispensermotorfrequency[5];
        DiagnosticsMonitor.dispenser7motorfrequency =   dispensermotorfrequency[6];
        DiagnosticsMonitor.dispenser8motorfrequency =   dispensermotorfrequency[7];
    }
/*
    DiagnosticsMonitor.n_dancer1angle =  DiagnosticsIndex;
    DiagnosticsMonitor.n_dancer2angle =  DiagnosticsIndex;
    DiagnosticsMonitor.n_dancer3angle =  DiagnosticsIndex;
    DiagnosticsMonitor.dancer1angle = dancer1angle;
    DiagnosticsMonitor.dancer2angle = dancer2angle;
    DiagnosticsMonitor.dancer3angle = dancer3angle;
*/
    if ((JobIsActive())&&(DiagnosticMode >= Diagnostic_Extended_Mode))
    {
        DiagnosticsMonitor.n_dancer1angle =  DancerCounterIndex[0];
        DiagnosticsMonitor.n_dancer2angle =  DancerCounterIndex[1];
        DiagnosticsMonitor.n_dancer3angle =  DancerCounterIndex[2];
        DiagnosticsMonitor.dancer1angle = DancerValue[0];
        DiagnosticsMonitor.dancer2angle = DancerValue[1];
        DiagnosticsMonitor.dancer3angle = DancerValue[2];
        DiagnosticsMonitor.n_windertension =  DancerErrorCounterIndex[0];
        DiagnosticsMonitor.n_pullertension =  DancerErrorCounterIndex[1];
        DiagnosticsMonitor.n_feedertension =  DancerErrorCounterIndex[2];
        DiagnosticsMonitor.windertension = DancerErrorValue[0];
        DiagnosticsMonitor.pullertension = DancerErrorValue[1];
        DiagnosticsMonitor.feedertension = DancerErrorValue[2];
        DiagnosticsMonitor.n_threadspeed = SpeedCounterIndex;
        DiagnosticsMonitor.threadspeed = SpeedValue;


        DiagnosticsMonitor.feedermotorfrequency = MotorValue[FEEDER_MOTOR];
        DiagnosticsMonitor.dryermotor = MotorValue[DRYER_MOTOR];
        DiagnosticsMonitor.pollermotor = MotorValue[POOLER_MOTOR];
        DiagnosticsMonitor.windermotor = MotorValue[WINDER_MOTOR];
        DiagnosticsMonitor.screwmotor = MotorValue[SCREW_MOTOR];
        DiagnosticsMonitor.n_feedermotorfrequency =  MotorCounterIndex[FEEDER_MOTOR];
        DiagnosticsMonitor.n_dryermotor =  MotorCounterIndex[DRYER_MOTOR];
        DiagnosticsMonitor.n_pollermotor =  MotorCounterIndex[POOLER_MOTOR];
        DiagnosticsMonitor.n_windermotor =  MotorCounterIndex[WINDER_MOTOR];
        DiagnosticsMonitor.n_screwmotor = MotorCounterIndex[SCREW_MOTOR];
    }
    DiagnosticsMonitor.mixertemperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__MixerHeater];
    DiagnosticsMonitor.headzone1temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1];
    DiagnosticsMonitor.headzone2temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2];
    DiagnosticsMonitor.headzone3temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3];
    DiagnosticsMonitor.headzone4temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4];
    DiagnosticsMonitor.headzone5temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5];
    DiagnosticsMonitor.headzone6temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6];
    DiagnosticsMonitor.dryerzone1temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerAirTemperature];
    DiagnosticsMonitor.dryerzone2temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain];
    DiagnosticsMonitor.dryerzone3temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary];

    DiagnosticsMonitor.n_mixertemperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__MixerHeater];
    DiagnosticsMonitor.n_headzone1temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1];
    DiagnosticsMonitor.n_headzone2temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2];
    DiagnosticsMonitor.n_headzone3temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3];
    DiagnosticsMonitor.n_headzone4temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4];
    DiagnosticsMonitor.n_headzone5temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5];
    DiagnosticsMonitor.n_headzone6temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6];
    DiagnosticsMonitor.n_dryerzone1temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__DryerAirTemperature];
    DiagnosticsMonitor.n_dryerzone2temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain];
    DiagnosticsMonitor.n_dryerzone3temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary];
    DiagnosticsMonitor.n_blowervoltage = 1;
    diagvoltage = getBlowerState();
    DiagnosticsMonitor.blowervoltage = &diagvoltage;

    int i;
    DiagnosticsMonitor.n_dispensersmotorsfrequency = 0;
    /**/
	//if (JobIsActive())
    {
        j++;
        DiagnosticsMonitor.n_dispensersmotorsfrequency = MAX_SYSTEM_DISPENSERS;
        if (DiagnosticsMonitor.dispensersmotorsfrequency)
        {
            for (i = 0; i<MAX_SYSTEM_DISPENSERS;i++)
            {
                //double_array__init(&DispenserFreq[i]);
                //DispenserFreq[i] = dispensermotorfrequency[i];
                DispenserFreq[i].data = dispensermotorfrequency[i];
                DispenserFreq[i].n_data = DiagnosticsIndex;
                DiagnosticsMonitor.dispensersmotorsfrequency[i] = &DispenserFreq[i];
            }
        }
    }
    /**/
    DiagnosticsMonitor.n_dispenserspressure = 0;
    /**/
	DiagnosticsMonitor.n_dispenserspressure = MAX_SYSTEM_DISPENSERS;
    if (DiagnosticsMonitor.dispenserspressure)
    {
        for (i = 0; i<MAX_SYSTEM_DISPENSERS;i++)
        {
            //DiagnosticsDispenserPressure[i] = dispenserspressure[i];
            //double_array__init(&DiagnosticsDispenserPressure[i]);
            DiagnosticsDispenserPressure[i].data = dispenserspressure[i];
            DiagnosticsDispenserPressure[i].n_data = DiagnosticsIndex;
            DiagnosticsMonitor.dispenserspressure[i] = &DiagnosticsDispenserPressure[i];
        }
    }

    /**/
    DiagnosticsMonitor.n_dispenserslinearpositions = 0;
    DiagnosticsMonitor.n_dispensersangularencoders = 0;

    DiagnosticsMonitor.n_dispenser1pressure = DiagnosticsIndex;
    DiagnosticsMonitor.dispenser1pressure = dispenserspressure[0];
    DiagnosticsMonitor.n_dispenser2pressure = DiagnosticsIndex;
    DiagnosticsMonitor.dispenser2pressure = dispenserspressure[1];
    DiagnosticsMonitor.n_dispenser3pressure = DiagnosticsIndex;
    DiagnosticsMonitor.dispenser3pressure = dispenserspressure[2];
    DiagnosticsMonitor.n_dispenser4pressure = DiagnosticsIndex;
    DiagnosticsMonitor.dispenser4pressure = dispenserspressure[3];
    DiagnosticsMonitor.n_dispenser5pressure = DiagnosticsIndex;
    DiagnosticsMonitor.dispenser5pressure = dispenserspressure[4];
    DiagnosticsMonitor.n_dispenser6pressure = DiagnosticsIndex;
    DiagnosticsMonitor.dispenser6pressure = dispenserspressure[5];
    DiagnosticsMonitor.n_dispenser7pressure = DiagnosticsIndex;
    DiagnosticsMonitor.dispenser7pressure = dispenserspressure[6];
    DiagnosticsMonitor.n_dispenser8pressure = DiagnosticsIndex;
    DiagnosticsMonitor.dispenser8pressure = dispenserspressure[7];



    response.monitors = &DiagnosticsMonitor;
    //response.digitalpins = DigitalPinArray;
    response.n_componentsstates = 0;

    response.n_digitalinterfacestates = DiagnosticsLoadDigitalValues();
    response.digitalinterfacestates = digitalinterfacestates;

    for (i=HEATER_TYPE__DryerAirHeater;i<=HEATER_TYPE__MixerHeater;i++)
    {
        LoadHeaterState((HeaterType)i,&HeaterInfo[i]);
        //heatersstates[i] = &HeaterInfo[i];
    }
    response.n_heatersstates = HEATER_TYPE__MixerHeater+1;
    response.heatersstates = heatersstates;


//    diagnosticsresponseContainer = createContainer(MESSAGE_TYPE__StartDiagnosticsResponse,  DiagnosticsToken, false, &response, &start_diagnostics_response__pack, &start_diagnostics_response__get_packed_size);
    diagnosticsresponseContainer = createAllocatedContainer(MESSAGE_TYPE__StartDiagnosticsResponse,  DiagnosticsToken, false, &response, &start_diagnostics_response__pack, &start_diagnostics_response__get_packed_size,diagnostics_response_ptr);
    diagnosticsresponseContainer.continuous = true;

    if (diagnosticsresponseContainer.data.data)
    {
        if (diagnosticscontainer_buffer == 0) //send new diagnostics packet only if the previous one was sent and released
        {
            diagnosticscontainer_buffer = my_malloc(message_container__get_packed_size(&diagnosticsresponseContainer));
            if (diagnosticscontainer_buffer)
            {
                size_t container_size = message_container__pack(&diagnosticsresponseContainer, diagnosticscontainer_buffer);
                //        if (SendChars(diagnosticscontainer_buffer, container_size)== false) //comm tx mailbox full
                if (SendCharsWithType(diagnosticscontainer_buffer, container_size,MESSAGE_TYPE__StartDiagnosticsResponse) == false) //comm tx mailbox full
                {
                    DiagnosticsStop();
                    my_free(diagnosticscontainer_buffer);
                }
	            /*else
	            {
	                Report("Diagnostic Messgage",__FILE__,__LINE__,container_size,RpWarning,msec_millisecondCounter,  msec_millisecondCounter);
	            }*/
                //diag_size[diag_index] = container_size;
                //diag_time[diag_index] = msec_millisecondCounter;
                //if (diag_index++>=MAX_DIAG_LOG)
                //    diag_index = 0;
            }
            else
            {
                LOG_ERROR(Task_self(),"my_malloc failed");
                DiagnosticsStop();
                //CommunicationMailboxFlush();
            }
        }
        else
        {
            LOG_ERROR(Task_self(),"previous packet not sent");
        }
        //my_free(diagnosticsresponseContainer.data.data);
    }
    else
    {
        LOG_ERROR(Task_self(),"my_malloc failed");
        DiagnosticsStop();
    }
    DiagnosticsReset();
/*    DiagnosticsMonitor.n_drierzone1heatercurrent = 0;
    DiagnosticsMonitor.n_drierzone2heatercurrent = 0;
    DiagnosticsMonitor.n_mixer1heatercurrent = 0;
    DiagnosticsMonitor.n_headzone1heatercurrent = 0;
    DiagnosticsMonitor.n_headzone2heatercurrent = 0;
    DiagnosticsMonitor.n_headzone3heatercurrent = 0;
    DiagnosticsMonitor.n_headzone4heatercurrent = 0;
    DiagnosticsMonitor.n_headzone5_6heatercurrent = 0;
    DiagnosticsMonitor.n_filterdeltapressure = 0;
    DiagnosticsMonitor.n_midtank1level = 0;
    DiagnosticsMonitor.n_midtank2level = 0;
    DiagnosticsMonitor.n_midtank3level = 0;
    DiagnosticsMonitor.n_midtank4level = 0;
    DiagnosticsMonitor.n_midtank5level = 0;
    DiagnosticsMonitor.n_midtank6level = 0;
    DiagnosticsMonitor.n_midtank7level = 0;
    DiagnosticsMonitor.n_midtank8level = 0;
    DiagnosticsMonitor.n_dispensersinklevel = 0;
    DiagnosticsMonitor.n_midtanksinklevel = 0;
*/
}
uint32_t DiagnosticsControlId = 0xff;
uint32_t Diagnostics10MSControlId = 0xff;
uint32_t Diagnostics1SecControlId = 0xff;
uint32_t Diagnostics1MinControlId = 0xFF;
bool DiagnosticRequestAccepted = false;
uint32_t DiagnosticsStart(void)
{

    if ( DiagnosticsActive == false)
    {
        if (DiagnosticRequestAccepted == true)
        {
            DiagnosticsActive = true;
            if (DiagnosticsControlId == 0xFF)
                DiagnosticsControlId = AddControlCallback(NULL,Diagnostics_ControlTrigger,DiagnosticLimit,TemplateDataReadCBFunction,0,0,0);
            else
                LOG_ERROR(DiagnosticsControlId,"Diagnostics restarted");
            if (Diagnostics10MSControlId == 0xFF)
                Diagnostics10MSControlId = AddControlCallback(NULL,Diagnostics_TenMiliControlTrigger,DiagnosticFastLimit,TemplateDataReadCBFunction,0,0,0);
            else
                LOG_ERROR(DiagnosticsControlId,"Diagnostics restarted");
            if (Diagnostics1SecControlId == 0xFF)
                Diagnostics1SecControlId = AddControlCallback(NULL,Diagnostics_OneSecControlTrigger,eOneSecond,TemplateDataReadCBFunction,0,0,0);
            else
                LOG_ERROR(Diagnostics1SecControlId,"Diagnostics restarted");
            if (Diagnostics1MinControlId == 0xFF)
                Diagnostics1MinControlId = AddControlCallback(NULL,Diagnostics_OneMinControlTrigger,eOneMinute,TemplateDataReadCBFunction,0,0,0);
            else
                LOG_ERROR(Diagnostics1MinControlId,"Diagnostics restarted");
       }

    }
    return OK;
}
uint32_t DiagnosticsStop(void)
{
    if ( DiagnosticsActive == true)
    {
        DiagnosticsActive = false;
        if (DiagnosticsControlId != 0xFF)
            RemoveControlCallback(DiagnosticsControlId,Diagnostics_ControlTrigger);
        if (Diagnostics10MSControlId != 0xFF)
            RemoveControlCallback(Diagnostics10MSControlId,Diagnostics_TenMiliControlTrigger);
        if (Diagnostics1SecControlId != 0xFF)
            RemoveControlCallback(Diagnostics1SecControlId,Diagnostics_OneSecControlTrigger);
        if (Diagnostics1MinControlId != 0xFF)
            RemoveControlCallback(Diagnostics1MinControlId,Diagnostics_OneMinControlTrigger);

        DiagnosticsControlId = 0xFF;
        Diagnostics10MSControlId = 0xFF;
        Diagnostics1SecControlId = 0xFF;
        Diagnostics1MinControlId = 0xFF;

    }
    return OK;
}
uint32_t StartDiagnosticsRequestFunc(MessageContainer* requestContainer)
{
    uint32_t status = NOT_SUPPORTED;
    //MessageContainer responseContainer;

//    ReportInitParams InitParams;
    //ControlStart();
    DiagnosticRequestAccepted = true;
    DiagnosticsStart();

    StartDiagnosticsRequest* request = start_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
    ustrncpy (DiagnosticsToken, requestContainer->token,36);
    start_diagnostics_request__free_unpacked(request,NULL);
    return status;
}
uint32_t StopDiagnosticsRequestFunc(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    StopDiagnosticsRequest* request = stop_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    StopDiagnosticsResponse response = STOP_DIAGNOSTICS_RESPONSE__INIT;

    DiagnosticsStop();

    DiagnosticRequestAccepted = false;

    responseContainer = createContainer(MESSAGE_TYPE__StopDiagnosticsResponse,  requestContainer->token, false, &response, &stop_diagnostics_response__pack, &stop_diagnostics_response__get_packed_size);
    responseContainer.continuous = false;
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    SendChars(container_buffer, container_size);

return OK;
}
uint32_t Diagnostics_ControlTrigger(uint32_t IfIndex, uint32_t ReadValue)
{
    DiagnosticsMessageStruc Message;

    //send message to the Millisec task
    Message.messageId = DiagnosticsTrigger;
    Message.msglen = sizeof(DiagnosticsMessageStruc);
    if (DiagnosticsMsgQ != NULL)
        Mailbox_post(DiagnosticsMsgQ , &Message, BIOS_NO_WAIT);


    return OK;
}

uint32_t Diagnostics_TenMiliControlTrigger(uint32_t IfIndex, uint32_t ReadValue)
{
    DiagnosticsMessageStruc Message;

    if (DiagnosticMode == Diagnostic_Extreme_Mode)
    {
        Message.messageId = DiagnosticsTenMiliTrigger;
        Message.msglen = sizeof(DiagnosticsMessageStruc);
        if (DiagnosticsMsgQ != NULL)
            Mailbox_post(DiagnosticsMsgQ , &Message, BIOS_NO_WAIT);
    }
    return OK;
}

uint32_t Diagnostics_OneSecControlTrigger(uint32_t IfIndex, uint32_t ReadValue)
{
    DiagnosticsMessageStruc Message;

    Message.messageId = DiagnosticsOneSecTrigger;
    Message.msglen = sizeof(DiagnosticsMessageStruc);
    if (DiagnosticsMsgQ != NULL)
        Mailbox_post(DiagnosticsMsgQ , &Message, BIOS_NO_WAIT);


    return OK;
}
uint32_t Diagnostics_OneMinControlTrigger(uint32_t IfIndex, uint32_t ReadValue)
{
    DiagnosticsMessageStruc Message;

    Message.messageId = DiagnosticsOneMinuteTrigger;
    Message.msglen = sizeof(DiagnosticsMessageStruc);
    if (DiagnosticsMsgQ != NULL)
        Mailbox_post(DiagnosticsMsgQ , &Message, BIOS_NO_WAIT);


    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 DiagnosticsTask(UArg arg0, UArg arg1)
{
    DiagnosticsMessageStruc Message;
    //char str[60];
    //uint16_t length;
    //Clock_setTimeout(HostKAClock, 1000);
    //Clock_start(HostKAClock);
    DiagnosticsInit();
    Diagnostics_Task_Handle = Task_self();
    while(1)
    {
        Mailbox_pend(DiagnosticsMsgQ , &Message, BIOS_WAIT_FOREVER);
        switch (Message.messageId)
        {
            case DiagnosticsTrigger:
                Diagnostic100msecCollection();
                break;
            case DiagnosticsTenMiliTrigger:
                if (DiagnosticMode >= Diagnostic_Extreme_Mode)
                    DiagnosticTenMsecCollection();
                break;
            case DiagnosticsOneSecTrigger:
                DiagnosticOneSecCollection();
                break;
            case DiagnosticsOneMinuteTrigger:
                DiagnosticOneMinuteCollection();
                break;
            default:
                break;
        }
    }
}