/*
 * MachineStatus.c
 *
 *  Created on: Aug 15, 2019
 *      Author: shlomo
 */
#include <Container.h>
#include <DataDef.h>
#include <stdio.h>      /* puts(), etc. */
#include "Common/report/report.h"
#include "include.h"

#include "PMR/common/MessageContainer.pb-c.h"
#include "PMR/MachineStatus/StartMachineStatusUpdateRequest.pb-c.h"
#include "PMR/MachineStatus/StopMachineStatusUpdateRequest.pb-c.h"
#include "PMR/MachineStatus/StartMachineStatusUpdateResponse.pb-c.h"
#include "PMR/MachineStatus/StopMachineStatusUpdateResponse.pb-c.h"
#include "PMR/Diagnostics/EventType.pb-c.h"
#include "PMR/MachineStatus/MachineStatus.pb-c.h"
#include "PMR/MachineStatus/MachineState.pb-c.h"
#include "PMR/MachineStatus/IDSPackLevel.pb-c.h"

#include "PMR/IFS/CartridgeStatus.pb-c.h"
#include "PMR/IFS/InkFillingStatus.pb-c.h"
#include "PMR/IFS/StartInkFillingStatusRequest.pb-c.h"
#include "PMR/IFS/StartInkFillingStatusResponse.pb-c.h"

#include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
#include "Drivers/Heater/TemperatureSensor.h"
#include "drivers/Motors/Motor.h"

#include "StateMachines/Initialization/InitSequence.h"

#include "Modules/General/MachineStatus.h"
#include "Modules/AlarmHandling/AlarmHandling.h"
#include "Modules/Control/MillisecTask.h"
#include "modules/waste/waste_ex.h"
#include "modules/ids/ids_ex.h"
#include "modules/ifs/ifs.h"

MachineState StoredMachineStatus = MACHINE_STATE__PowerUp;
bool SetMAchineStateProblem = false;
void SetMachineStatus (MachineState New_state)
{
    MachineState State = New_state;
    if (StoredMachineStatus == MACHINE_STATE__PowerOff)
    {
        ReportWithPackageFilter(GeneralFilter,"not changing MachineStatus while on power down",__FILE__,__LINE__,State,RpWarning,StoredMachineStatus, 0);
        return;
    }
    if (State == MACHINE_STATE__Ready)
    {
        if ((GetMachineState()<MACHINE_STATE_MACHINE_READY_TO_DYE)&&(GetMachineState()!=MACHINE_STATE_NO_PROCESS_PARAMS))
        {
            ReportWithPackageFilter(GeneralFilter,"SetMachineStatus problem",__FILE__,__LINE__,State,RpWarning,GetMachineState(), 0);
            SetMAchineStateProblem = true;
            State = MACHINE_STATE__PowerUp;
        }
    }
    ReportWithPackageFilter(GeneralFilter,"SetMachineStatus",__FILE__,GetMachineState(),State,RpWarning,StoredMachineStatus, 0);

    StoredMachineStatus = State;
    MachineUpdateResponseFunc();
}
char MachineUpdateToken[36+1] = {0};
IDSPackLevel IDS_Level[MAX_SYSTEM_DISPENSERS];
TimerMotors_t msDispenserIdToMotorId[MAX_SYSTEM_DISPENSERS] = {HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_5,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8};

bool temp_measure_alarm = false;
int MachineUpdateResponseFunc(void)
{
    int i;
    int internaltemp1,internaltemp2;
    double usetemp;
    bool int1valid = true,int2valid = true;
    MessageContainer responseContainer;
    if (MachineUpdateToken[0] == 0)
        return OK;

    StartMachineStatusUpdateResponse response = START_MACHINE_STATUS_UPDATE_RESPONSE__INIT;
    MachineStatus MachineStatus = MACHINE_STATUS__INIT;
    response.status = &MachineStatus;

    //machine_status__init(&MachineStatus);
    MachineStatus.has_state = true;
    MachineStatus.state = StoredMachineStatus;
    MachineStatus.n_idspackslevels = 0;
    MachineStatus.idspackslevels = (IDSPackLevel**)my_malloc(sizeof(IDSPackLevel *)*8);
    MachineStatus.has_spoolstate = true;

    internaltemp1 = MillisecGetTemperatures(TEMP_SENSE_AN_ENCLOSURETEMP1);
    internaltemp2 = MillisecGetTemperatures(TEMP_SENSE_AN_ENCLOSURETEMP2);

    if((internaltemp1<=MINIMUM_HEATER_READ*100)||(MAXIMUM_HEATER_READ*100<=internaltemp1))
        int1valid = false;
    if((internaltemp2<=MINIMUM_HEATER_READ*100)||(MAXIMUM_HEATER_READ*100<=internaltemp2))
        int2valid = false;
    MachineStatus.has_overalltemperature = true;
    if ((int2valid == true)&&(int1valid == true))
    {
            usetemp = (max(internaltemp1,internaltemp2));
            usetemp = (double)usetemp/100.0;

            if (temp_measure_alarm == true)
            {
                AlarmHandlingSetAlarm(EVENT_TYPE__TEMPERATURE_MEASUREMENT_ERROR, false);
                temp_measure_alarm = false;
            }
    }
    else
    {
        //AlarmHandlingSetAlarm(EVENT_TYPE__TEMPERATURE_MEASUREMENT_ERROR, true);
        temp_measure_alarm = true;
        if ((int2valid == false)&&(int1valid == true))
            usetemp = (double)internaltemp1/100.0;
        else if ((int2valid == false)&&(int1valid == false))
        {
            MachineStatus.has_overalltemperature = FALSE;
            usetemp = 0;
        }
        else  if ((int1valid == false)&&(int2valid == true))
            usetemp = (double)internaltemp2/100.0;
    }
    MachineStatus.overalltemperature = usetemp;



    if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT)
    {
      MachineStatus.spoolstate = SPOOL_STATE__Absent;
    }
    else
    {
        MachineStatus.spoolstate = SPOOL_STATE__Present;

    }
    if (MachineStatus.idspackslevels)
    {
        for (i = 0; i<MAX_SYSTEM_DISPENSERS;i++)
        {
            if (MotorsCfg[msDispenserIdToMotorId[i]].hardwaremotortype
                        != msDispenserIdToMotorId[i]) // dispenser not configured
                    continue;

            idspack_level__init(&IDS_Level[i]);
            IDS_Level[i].has_index = true;
            IDS_Level[i].index = i;
            IDS_Level[i].has_dispenserlevel = true;
            double dispcapacity = (1.00-(IDS_Dispenser_Data[i].consumedinnanolitter/5800000))*130*1000*1000;
            IDS_Level[i].dispenserlevel = (int32_t)dispcapacity;
            IDS_Level[i].has_midtanklevel = true;
            IDS_Level[i].midtanklevel = Get_MidTank_Pressure_Sensor(i);
            MachineStatus.idspackslevels[MachineStatus.n_idspackslevels] = &IDS_Level[i];
            MachineStatus.n_idspackslevels++;
        }
    }
    else
        return ERROR;

   // Report("Sending machine state",__FILE__,__LINE__,(int)MachineStatus.state,RpWarning,(int)MachineStatus.overalltemperature,0);

    responseContainer = /*MachineUpdate*/createContainer(MESSAGE_TYPE__StartMachineStatusUpdateResponse, MachineUpdateToken, false, &response, &start_machine_status_update_response__pack, &start_machine_status_update_response__get_packed_size);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    //if (responseContainer.data.data)
    {
        uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
        if (container_buffer)
        {
            size_t container_size = message_container__pack(&responseContainer, container_buffer);
            //        if (SendChars(container_buffer, container_size) == false) //comm tx mailbox full
            if (SendCharsWithType(container_buffer, container_size,MESSAGE_TYPE__StartMachineStatusUpdateResponse) == false) //comm tx mailbox full
            {
                //CommunicationMailboxFlush();
                MachineUpdateToken[0] = 0;
                my_free(container_buffer);
            }
        }
        my_free(responseContainer.data.data);
    }

    my_free(MachineStatus.idspackslevels);
        //Task_sleep(5);
    return OK;

}


uint32_t MachineUpdateInitFunc(MessageContainer* requestContainer)
{
    uint32_t status = NOT_SUPPORTED;
    //MessageContainer responseContainer;

//    MachineUpdateInitParams InitParams;

    StartMachineStatusUpdateRequest* request = start_machine_status_update_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
    ustrncpy (MachineUpdateToken, requestContainer->token,36);

    start_machine_status_update_request__free_unpacked(request,NULL);
    MachineUpdateResponseFunc();
    return status;
}
void MachineUpdateStopReporting(void)
{
    MachineUpdateToken[0] = 0;
}
uint32_t StopMachineUpdateFunc(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    StopMachineStatusUpdateRequest* request = stop_machine_status_update_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    StopMachineStatusUpdateResponse response = STOP_MACHINE_STATUS_UPDATE_RESPONSE__INIT;

//TODO Handle the request!!!!
    MachineUpdateStopReporting();
    responseContainer = createContainer(MESSAGE_TYPE__StopMachineStatusUpdateResponse, MachineUpdateToken, false, &response, &stop_machine_status_update_response__pack, &stop_machine_status_update_response__get_packed_size);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    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);
    //USBCDCD_sendData(container_buffer, container_size,10);
    SendChars(container_buffer, container_size);
    //free (container_buffer);

return OK;
}
/*struct  _Cartridge
{
  ProtobufCMessage base;
  protobuf_c_boolean has_slot;
  CartridgeSlot slot;
  CartridgeTagContent *tag;
  protobuf_c_boolean has_index;
  int32_t index;
};
 typedef enum _CartridgeSlot {
  CARTRIDGE_SLOT__Ink = 0,
  CARTRIDGE_SLOT__WasteMiddle = 1,
  CARTRIDGE_SLOT__WasteLower = 2
    PROTOBUF_C__FORCE_ENUM_TO_BE_INT_SIZE(CARTRIDGE_SLOT)
} CartridgeSlot;
 * */

char WasteEmptyingToken[36+1] = {0};
uint32_t CartridgeStateUpdate(CartridgeSlot Slot, int32_t index, CartridgeState State, double percentage)
{
    if (WasteEmptyingToken[0] == 0)
        return ERROR;
    uint32_t status = NOT_SUPPORTED;
    MessageContainer responseContainer;
    CartridgeStatus CartridgeStatus = CARTRIDGE_STATUS__INIT;
    Cartridge CartData = CARTRIDGE__INIT;
    StartInkFillingStatusResponse response = START_INK_FILLING_STATUS_RESPONSE__INIT;
    response.status = my_malloc(sizeof(InkFillingStatus));
    ink_filling_status__init(response.status);
    response.status->cartridgesstatuses = my_malloc(sizeof(void *));

    response.status->n_cartridgesstatuses = 1;
    response.status->cartridgesstatuses[0] = &CartridgeStatus;
    CartridgeStatus.has_state = true;
    CartridgeStatus.state = State;
    CartridgeStatus.cartridge = &CartData;
    if (percentage>1)
    {
        CartridgeStatus.has_progresspercentage = true;
        CartridgeStatus.progresspercentage = percentage;
    }
    CartData.has_slot = true;
    CartData.slot = Slot;

    CartData.has_index = true;
    CartData.index = index;

    /*
        struct  _CartridgeStatus
        {
          ProtobufCMessage base;
          Cartridge *cartridge;
          protobuf_c_boolean has_state;
          CartridgeState state;
          protobuf_c_boolean has_progresspercentage;
          double progresspercentage;
          char *message;
        };
          size_t n_cartridgesstatuses;
          CartridgeStatus **cartridgesstatuses;
          typedef enum _CartridgeState {
              CARTRIDGE_STATE__None = 0,
              CARTRIDGE_STATE__Absent = 1,
              CARTRIDGE_STATE__Exists = 2,
              CARTRIDGE_STATE__Inserted = 3,
              CARTRIDGE_STATE__Filling = 4,
              CARTRIDGE_STATE__FillingCompleted = 5,
              CARTRIDGE_STATE__Emptying = 6,
              CARTRIDGE_STATE__EmptyingCompleted = 7,
              CARTRIDGE_STATE__Empty = 8,
              CARTRIDGE_STATE__Full = 9,
              CARTRIDGE_STATE__Error = 10
              PROTOBUF_C__FORCE_ENUM_TO_BE_INT_SIZE(CARTRIDGE_STATE)
        } CartridgeState;
     */

    responseContainer = createContainer(MESSAGE_TYPE__StartInkFillingStatusResponse, WasteEmptyingToken, false, &response, &start_ink_filling_status_response__pack, &start_ink_filling_status_response__get_packed_size);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    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);
    //USBCDCD_sendData(container_buffer, container_size,10);

    SendChars((char*)container_buffer, container_size);
    //MessageContainer responseContainer;
    my_free(response.status->cartridgesstatuses);
    my_free(response.status);

    return status;
}

uint32_t StartInkFillingStatusRequestFunc(MessageContainer* requestContainer)
{
    uint32_t status = NOT_SUPPORTED;
    //MessageContainer responseContainer;

//    MachineUpdateInitParams InitParams;

    Report("StartInkFillingStatusRequestFunc",__FILE__,__LINE__,(int)0,RpWarning,(int)0,0);
    StartInkFillingStatusRequest* request = start_ink_filling_status_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
    ustrncpy (WasteEmptyingToken, requestContainer->token,36);

    CartridgeStateUpdate(CARTRIDGE_SLOT__Ink,0, CARTRIDGE_STATE__None,0);
    CartridgeStateUpdate(CARTRIDGE_SLOT__WasteMiddle, 0, CARTRIDGE_STATE__None,0);
    CartridgeStateUpdate(CARTRIDGE_SLOT__WasteLower, 1, CARTRIDGE_STATE__None,0);
    start_ink_filling_status_request__free_unpacked(request,NULL);
    return status;
//    case MESSAGE_TYPE__StartWasteEmptyingRequest:

}