/*
 * IDS_Cleaning.c
 *
 *  Created on: 18 במרץ 2019
 *      Author: User
 */
#include "include.h"
#include "ids.h"
#include "ids_ex.h"
#include "../control/control.h"
#include "../control/pidalgo.h"
#include "../thread/thread.h"
#include "PMR/Hardware/Hardwaremotor.pb-c.h"
#include "PMR/Hardware/HardwareDispenser.pb-c.h"
#include "StateMachines/Printing/printingSTM.h"
#include "drivers/motors/motor.h"
#include "drivers/valves/valve.h"

int SaveLeftRockerSpeed = 50, SaveRightRockerSpeed = 50;
#define CLEANING_DISPENSER_ID 6
uint32_t IDS_Cleaning_Move_Rockers (int LeftRockerSpeed,int RightRockerSpeed)
{
    uint32_t status = OK;
    SaveLeftRockerSpeed = LeftRockerSpeed;
    SaveRightRockerSpeed = RightRockerSpeed;
    status |= MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize);
    status |= MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD, RightRockerSpeed);
    status |= MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize);
    status |= MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH, LeftRockerSpeed);
    Report("IDS_Cleaning_Move_Rockers", __FILE__, __LINE__, LeftRockerSpeed, RpWarning,   RightRockerSpeed, 0);

    return status;
}
uint32_t IDS_Cleaning_Center_And_Stop_Rockers (int timeout,callback_fptr callback)
{
    uint32_t status = OK;
    status |= MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, SaveRightRockerSpeed,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], callback,timeout);
    status |= MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize, SaveLeftRockerSpeed,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH], callback,timeout);
    Report("IDS_Cleaning_Center_And_Stop_Rockers", __FILE__, __LINE__, timeout, RpWarning,   123456, 0);

    return status;
}
uint32_t IDS_Cleaning_Spray_Cleaning_Solution (int dispenserSpeed,callback_fptr callback)
{
    uint32_t status = OK;
    status = IDS_Dispenser_Start_Motor_and_Open_Valve(CLEANING_DISPENSER_ID, dispenserSpeed, callback);
    Report("IDS_Cleaning_Spray_Cleaning_Solution", __FILE__, __LINE__, CLEANING_DISPENSER_ID, RpWarning,   dispenserSpeed, 0);

    return status;

}
uint32_t IDS_Cleaning_Stop_Cleaning_Solution (callback_fptr callback)
{
    uint32_t status = OK;
    status = IDS_Dispenser_Close_Valve_And_Stop_Motor(CLEANING_DISPENSER_ID,callback);
    Report("IDS_Cleaning_Stop_Cleaning_Solution", __FILE__, __LINE__, CLEANING_DISPENSER_ID, RpWarning,   status, 0);

    return status;

}