using System;
using System.Collections.Generic;
using System.Linq
/*
 * DiagnosticsJogging.c
 *
 *  Created on: 16 aug 2018
 *      Author: shlomo
 */
/*
 * DiagnosticsHoming.c

 *
 *  Created on: July 29 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 "StateMachines/Printing/PrintingSTM.h"

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

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

    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 (MotorId <= NUM_OF_MOTORS)
    {
        if ((JobIsActive() == false)&&(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;
                }
                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.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);

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

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))
        {
            switch (request->direction)
            {
                case MOTOR_DIRECTION__Forward:
                    Control3WayValvesWithCallback ((Valves_t) request->index, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
                    direction = MotorsCfg[MotorId].directionthreadwize;
                    break;
                case MOTOR_DIRECTION__Backward:
                    Control3WayValvesWithCallback ((Valves_t) request->index, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
                    direction = 1-MotorsCfg[MotorId].directionthreadwize;
                    break;
            }
            MotorSetDirection(MotorId,direction);
            MotorSetSpeed(MotorId, speed);
        }
        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.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);
    Control3WayValvesWithCallback ((Valves_t) request->index, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer

    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;
}