/*
 * DiagnosticsJogging.c
 *
 *  Created on: 16 aug 2018
 *      Author: shlomo
 */
#include <DataDef.h>
#include "include.h"

#include <inc/hw_ints.h>
#include <Container.h>

#include "diagnostics.h"
#include "drivers/Motors/Motor.h"
#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
#include "drivers/FPGA/FPGA.h"

#include  <PMR/Diagnostics/DispenserJoggingRequest.pb-c.h>
#include  <PMR/Diagnostics/DispenserJoggingResponse.pb-c.h>
#include  <PMR/Diagnostics/DispenserAbortJoggingRequest.pb-c.h>
#include  <PMR/Diagnostics/DispenserAbortJoggingResponse.pb-c.h>

#include  <PMR/Diagnostics/MotorJoggingRequest.pb-c.h>
#include  <PMR/Diagnostics/MotorJoggingResponse.pb-c.h>
#include  <PMR/Diagnostics/MotorAbortJoggingRequest.pb-c.h>
#include  <PMR/Diagnostics/MotorAbortJoggingResponse.pb-c.h>

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

#include "modules/ids/ids_ex.h"
#include "StateMachines/Printing/PrintingSTM.h"

/********************************************************************************
 * Motor Jogging
 ********************************************************************************/

uint32_t MotorJoggingRequestFunc(MessageContainer* requestContainer)
{
    uint32_t status = OK;
    bool allowedInJob  =true;

    MessageContainer responseContainer;

    MotorJoggingRequest* request = motor_jogging_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    MotorJoggingResponse response = MOTOR_JOGGING_RESPONSE__INIT;

    TimerMotors_t MotorId = (TimerMotors_t)request->motortype;
    bool direction;
    int speed = request->speed;
    if (speed == 0) speed = 150;

    if (JobIsActive() == true)
    {
        switch (MotorId)
        {
            case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1:case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2:case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3:case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4:
            case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_5:case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6:case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7:case HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8:
            case HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING:case HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM:case HARDWARE_MOTOR_TYPE__MOTO_SCREW:case HARDWARE_MOTOR_TYPE__MOTO_WINDER:
            case HARDWARE_MOTOR_TYPE__MOTO_LDRIVING:case HARDWARE_MOTOR_TYPE__MOTO_LLOADING:case HARDWARE_MOTOR_TYPE__MOTO_RDRIVING:case HARDWARE_MOTOR_TYPE__MOTO_RLOADING:
                allowedInJob = false;
                break;
            default:
                break;
        }
    }
    if (MotorId <= NUM_OF_MOTORS)
    {
        if ((allowedInJob == true)&&(isMotorConfigured(MotorId) == true))
        {
            {
                switch (request->direction)
                {
                    case MOTOR_DIRECTION__Forward:
                        direction = MotorsCfg[MotorId].directionthreadwize;
                        break;
                    case MOTOR_DIRECTION__Backward:
                        direction = 1-MotorsCfg[MotorId].directionthreadwize;
                        break;
                }
                if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID)
                {
                    uint32_t Pos_Value = ((direction == 1)?0:2);
                    Report("MotorJoggingRequestFunc Dryer lid",__FILE__,MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType,direction,RpMessage,Pos_Value,0);
                    MotorGoTo(MotorId, Pos_Value );
                }
                else
                {
                    MotorSetDirection(MotorId,direction);
                    MotorSetSpeed(MotorId, speed);
                }
            }
        }
        else
        {
            status = ERROR;
        }
    }
    else
    {
        status = ERROR;
    }

    responseContainer = createContainer(MESSAGE_TYPE__MotorJoggingResponse,  requestContainer->token, false, &response, &motor_jogging_response__pack, &motor_jogging_response__get_packed_size);
    if (status!= OK)
    {
        responseContainer.has_error = true;
        responseContainer.error = ERROR_CODE__INVALID_PROCESS_ID;
        responseContainer.errormessage = "JOb Active or incorrect parameters";
    }
    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 MotorAbortJoggingRequestFunc(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    MotorAbortJoggingRequest* request = motor_abort_jogging_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    MotorAbortJoggingResponse response = MOTOR_ABORT_JOGGING_RESPONSE__INIT;

    TimerMotors_t MotorId = (TimerMotors_t)request->motortype;

    MotorStop(MotorId,Hard_Hiz);
    if (MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM)
        MotorStop(MotorId,Hard_Stop );

    responseContainer = createContainer(MESSAGE_TYPE__MotorAbortJoggingResponse,  requestContainer->token, false, &response, &motor_abort_jogging_response__pack, &motor_abort_jogging_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;
}

/********************************************************************************
 * Dispenser Jogging
 ********************************************************************************/
extern int32_t CurrentDispenserSpeed[MAX_SYSTEM_DISPENSERS];

uint32_t DispenserJoggingRequestFunc(MessageContainer* requestContainer)
{
    uint32_t status = OK;

    MessageContainer responseContainer;

    DispenserJoggingRequest* request = dispenser_jogging_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    DispenserJoggingResponse response = DISPENSER_JOGGING_RESPONSE__INIT;

    TimerMotors_t MotorId = (request->index)+HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
    int speed = request->speed;
    if (speed == 0) speed = 150;

    bool direction;

    if (MotorId <= NUM_OF_MOTORS)
    {
//        if ((JobIsActive() == false)&&(isMotorConfigured(MotorId) == true))
        if ((JobIsActive())&&(DispenserUsedInJob[request->index] == true))
        {
            responseContainer.error = ERROR_CODE__GENERAL_ERROR;
            responseContainer.errormessage = "Dispenser active in job";
        }
        else if (isMotorConfigured(MotorId) == true)
        {
            switch (request->direction)
            {
                case MOTOR_DIRECTION__Forward:
                    Control3WayValvesWithCallback ((Valves_t) request->index, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
                    SysCtlDelay(180000);
                    direction = MotorsCfg[MotorId].directionthreadwize;
                    if (request->index == LUBRICANT_DISPENSER)
                    {
                        REPORT_MSG (request->index, "Open lubrication valve");
                        Lubricant_2Way_Valve (START);
                    }
                    break;
                case MOTOR_DIRECTION__Backward:
                    Control3WayValvesWithCallback ((Valves_t) request->index, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
                    SysCtlDelay(180000);
                    direction = 1-MotorsCfg[MotorId].directionthreadwize;
                    break;
            }
            MotorSetDirection(MotorId,direction);
            MotorSetSpeed(MotorId, speed);
            CurrentDispenserSpeed[request->index] = speed;
            if(request->direction == MOTOR_DIRECTION__Backward)
                CurrentDispenserSpeed[request->index] *= (-1);

        }
        else
        {
            status = ERROR;
        }
    }
    else
    {
        status = ERROR;
    }

    responseContainer = createContainer(MESSAGE_TYPE__DispenserJoggingResponse,  requestContainer->token, false, &response, &dispenser_jogging_response__pack, &dispenser_jogging_response__get_packed_size);
    if (status!= OK)
    {
        responseContainer.has_error = true;
        responseContainer.error = ERROR_CODE__INVALID_PROCESS_ID;
        responseContainer.errormessage = "JOb Active or incorrect parameters";
    }
    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 DispenserAbortJoggingRequestFunc(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    DispenserAbortJoggingRequest* request = dispenser_abort_jogging_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    DispenserAbortJoggingResponse response = DISPENSER_ABORT_JOGGING_RESPONSE__INIT;

    TimerMotors_t MotorId = (request->index)+HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
    MotorStop(MotorId,Hard_Hiz);
    CurrentDispenserSpeed[request->index] = 0;
    Control3WayValvesWithCallback ((Valves_t) request->index, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer

    if (request->index == LUBRICANT_DISPENSER)
    {
        REPORT_MSG (request->index, "Close lubrication valve");
        Lubricant_2Way_Valve (STOP);
    }

    responseContainer = createContainer(MESSAGE_TYPE__DispenserAbortJoggingResponse,  requestContainer->token, false, &response, &dispenser_abort_jogging_response__pack, &dispenser_abort_jogging_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;
}