/*
 * 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 "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 "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"

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 DispensersCollection(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];
double dancer1angle[DIAGNOSTICS_LIMIT];
double dancer2angle[DIAGNOSTICS_LIMIT];
double dancer3angle[DIAGNOSTICS_LIMIT];

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


double dispensermotorfrequency[MAX_SYSTEM_DISPENSERS][DIAGNOSTICS_LIMIT];
double dispenserspressure[MAX_SYSTEM_DISPENSERS][DIAGNOSTICS_LIMIT];
double MidTankpressure[MAX_SYSTEM_DISPENSERS][1];

DoubleArray DispenserFreq[MAX_SYSTEM_DISPENSERS];
DoubleArray DiagnosticsDispenserPressure[MAX_SYSTEM_DISPENSERS];

HeaterState **heatersstates;
HeaterState  HeaterInfo[HEATER_TYPE__MixerHeater+1];
DigitalInterfaceState **digitalinterfacestates;
DigitalInterfaceState DigitalOutputState[MAX_HEATERS_NUM/*+1*/];
/*double dispenser1motorfrequency[DIAGNOSTICS_LIMIT];
double dispenser2motorfrequency[DIAGNOSTICS_LIMIT];
double dispenser3motorfrequency[DIAGNOSTICS_LIMIT];
double dispenser4motorfrequency[DIAGNOSTICS_LIMIT];
double dispenser5motorfrequency[DIAGNOSTICS_LIMIT];
double dispenser6motorfrequency[DIAGNOSTICS_LIMIT];
double dispenser7motorfrequency[DIAGNOSTICS_LIMIT];
double dispenser8motorfrequency[DIAGNOSTICS_LIMIT];
*/
Task_Handle Diagnostics_Task_Handle;
Mailbox_Handle          DiagnosticsMsgQ = NULL;

bool blowervolatgedisplay = false;
double diagvoltage = 0;
/******************** Functions  ********************************************/
//uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
//**********************************************************************
typedef enum
{
    DiagnosticsTrigger,
    DiagnosticsTenMiliTrigger,
    DispensersCollectionTrigger,
}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];

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

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

    Error_init(&eb);

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

    DiagnosticsMonitor.dispensersmotorsfrequency = (DoubleArray**)my_malloc(sizeof(DoubleArray *)*8);
    DiagnosticsMonitor.dispenserspressure = (DoubleArray**)my_malloc(sizeof(DoubleArray)*8);
    DiagnosticsMonitor.dispenserslinearpositions = (DoubleArray**)my_malloc(sizeof(DoubleArray)*8);
    DiagnosticsMonitor.dispensersangularencoders = (DoubleArray**)my_malloc(sizeof(DoubleArray)*8);
    for (i = 0; i<MAX_SYSTEM_DISPENSERS;i++)
    {
        double_array__init(&DispenserFreq[i]);
        double_array__init(&DiagnosticsDispenserPressure[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);
    for (i = 0;i<MAX_HEATERS_NUM;i++ )
    {
        digital_interface_state__init(&DigitalOutputState[i]);
        DigitalOutputState[i].has_interfaceio = true;
        DigitalOutputState[i].has_value = true;
        digitalinterfacestates[i] = &DigitalOutputState[i];
    }

    return;
}

void  SetDiagnosticCollectionLimit(int limit)
{
    if ((limit)&&(limit<= DIAGNOSTICS_LIMIT))
        DiagnosticCollectionLimit = limit;
}

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

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;
}
void DiagnosticsLoadDigitalValues(void)
{
    DigitalOutputState[0].interfaceio = INTERFACE_IOS__GPO_DRYER_SSR3_CTRL;
    DigitalOutputState[0].value = GetHeaterState(0);

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

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

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

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

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

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

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

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

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

    //DigitalOutputState[9].interfaceio = INTERFACE_IOS__GPO_SPARE_SSR13_CTRL;
    //DigitalOutputState[9].value = GetHeaterState(9);

}
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 Diagnostic100msecCollection(void)
{
    int i;
    if (DiagnosticsActive == false)
        return;

    DiagnosticLoadSpeedSensor(getSensorSpeedData());

    //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, TemperatureSensorRead( MIXER_PT100));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1, TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP1));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2, TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP2));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3, TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP3));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4, TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP4));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP5));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, TemperatureSensorRead(HEAD6_PT100));

    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerAirTemperature, TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP1));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain, TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP2));
    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary, TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP3));
    for (i=0;i<MAX_SYSTEM_DISPENSERS;i++)
    {
        dispensermotorfrequency[i][DiagnosticsIndex] = IdsGetMotorSpeed(i);
        dispenserspressure[i][DiagnosticsIndex] = GetDispenserPressure(i); //Read_MidTank_Pressure_Sensor
        MidTankpressure[i][0] = Get_MidTank_Pressure_Sensor(i);
    }
/*     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;
uint8_t* 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())
    {
        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];


    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];
    response.monitors = &DiagnosticsMonitor;
    //response.digitalpins = DigitalPinArray;
    response.n_componentsstates = 0;

    DiagnosticsLoadDigitalValues();
    response.n_digitalinterfacestates = MAX_HEATERS_NUM;
    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();
}
void DispensersCollectionCall(void)
{
    int i;
    for (i = 0; i<MAX_SYSTEM_DISPENSERS;i++)
    {
        IDS_Dispenser_Content_Calculation(i);
    }
}
uint32_t DiagnosticsControlId = 0xff;
uint32_t Diagnostics10MSControlId = 0xff;
uint32_t DispensersControlId = 0xff;
bool DiagnosticRequestAccepted = false;
uint32_t DiagnosticsStart(void)
{

    if ( DiagnosticsActive == false)
    {
        if (DiagnosticRequestAccepted == true)
        {
            DiagnosticsActive = true;
            if (DiagnosticsControlId == 0xFF)
                DiagnosticsControlId = AddControlCallback(Diagnostics_ControlTrigger,DiagnosticLimit,TemplateDataReadCBFunction,0,0,0);
            else
                LOG_ERROR(DiagnosticsControlId,"Diagnostics restarted");
            if (Diagnostics10MSControlId == 0xFF)
                Diagnostics10MSControlId = AddControlCallback(Diagnostics_TenMiliControlTrigger,DiagnosticFastLimit,TemplateDataReadCBFunction,0,0,0);
            else
                LOG_ERROR(DiagnosticsControlId,"Diagnostics restarted");

            if (DispensersControlId == 0xFF)
                DispensersControlId = AddControlCallback(DispensersCollection,eOneSecond,TemplateDataReadCBFunction,0,0,0);
            else
                LOG_ERROR(DispensersControlId,"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 (DispensersControlId != 0xFF)
            RemoveControlCallback(DispensersControlId,DispensersCollection);

        DiagnosticsControlId = 0xFF;
        Diagnostics10MSControlId = 0xFF;
        DispensersControlId = 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 DispensersCollection(uint32_t IfIndex, uint32_t ReadValue)
{
    DiagnosticsMessageStruc Message;

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


    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;

    //send message to the Millisec task
    Message.messageId = DiagnosticsTenMiliTrigger;
    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:
                DiagnosticTenMsecCollection();
                break;
            case DispensersCollectionTrigger:
                DispensersCollectionCall();
                break;
            default:
                break;
        }
    }
}