/*
 * 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/I2C_Communication/I2C_Task.h>

#include "drivers/motors/motor.h"
#include "drivers/valves/valve.h"
#include "Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.h"

int SaveLeftRockerSpeed = 50, SaveRightRockerSpeed = 50;
typedef enum
{
    CleaningStageIdle,
    CleaningStageActuatorUp = 1,
    CleaningStageDelay1,
    CleaningStageActuatorDown,
    CleaningStageDelay2,
}CleaningStageEnum;

/*
 *Cleaning sequence:


t up � time actuator goes up
t delay1 � time actuator stays in position (up position � not limit switch)
Down to limit -  actuator goes to lower limit switch
t delay2 � time actuator stays in lower limit switch position

Values for start:
t up � 300 msec
t delay1 � 500 msec
t delay2 � 2 sec

 */

CleaningStageEnum CleaningStage = CleaningStageIdle;
int CleaningStageCounter = 0;
int Tup = 4,Tdelay1 = 9,Tdelay2 = 29;

uint32_t IDS_Cleaning_Move_Actuators()
{
   if(Head_Type != HEAD_TYPE_SYLKO)
       return OK;
   if (CleaningStageCounter == 1)
   {
       Trigger_Head_Actuators_Stub(ACTIN, true, true);
       //Trigger_Head_Actuators_Control(ACTIN, LOW,true);
       Report("IDS_Cleaning_Move_ActuatorUp", __FILE__, __LINE__, 1, RpWarning,   CleaningStageCounter, 0);
   }
   else if (CleaningStageCounter == Tup)
   {
       Trigger_Head_Actuators_Stub(ACTIN, false, false);
       //Trigger_Head_Actuators_Disable();
       Report("IDS_Cleaning_Stop_Actuator", __FILE__, __LINE__, Tup, RpWarning,   CleaningStageCounter, 0);
   }
   else if (CleaningStageCounter == Tdelay1)
   {
       Trigger_Head_Actuators_Stub(ACTIN, true, false);
       //Trigger_Head_Actuators_Control(ACTIN, LOW,false);
       Report("IDS_Cleaning_MoveDown", __FILE__, __LINE__, Tdelay1, RpWarning,   CleaningStageCounter, 0);
   }
   else if (CleaningStageCounter == Tdelay2)
   {
       CleaningStageCounter = 0;
       Report("IDS_Cleaning_Stop_delay", __FILE__, __LINE__, Tdelay2, RpWarning,   CleaningStageCounter, 0);
   }



   CleaningStageCounter++;

   return OK;
}
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);
    Trigger_Head_Actuators_Disable();
    Trigger_Head_Actuators_Control(ACTIN, LOW,false);
    CleaningStageCounter = 0;
    CleaningStage = CleaningStageIdle;
    Report("IDS_Cleaning_Center_And_Stop_Rockers actuator down", __FILE__, __LINE__, timeout, RpWarning,   123456, 0);

    return status;
}
uint32_t IDS_Cleaning_Spray_Cleaning_Solution (int dispenserSpeed,callback_fptr callback)
{
    uint32_t status = OK;
    Tup = LeftRockerSpeed/100;
    Tdelay1 = LeftRockerSpeed%100;
    Tdelay2 = RightRockerSpeed;
    status = IDS_Dispenser_Start_Motor_and_Open_Valve(CLEANER_DISPENSER, dispenserSpeed, callback);
    Report("IDS_Cleaning_Spray_Cleaning_Solution", __FILE__, __LINE__, CLEANER_DISPENSER, RpWarning,   dispenserSpeed, 0);
    Report("IDS_Cleaning parameters", __FILE__, Tup*100, Tdelay1*100, RpWarning,   Tdelay2*100, 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(CLEANER_DISPENSER,callback);
    Report("IDS_Cleaning_Stop_Cleaning_Solution", __FILE__, __LINE__, CLEANER_DISPENSER, RpWarning,   status, 0);
    if(Head_Type == HEAD_TYPE_SYLKO)
    {
        Trigger_Head_Actuators_Control(ACTIN, LOW,false);
        Report("IDS_Cleaning_MoveDown cleaning stop", __FILE__, __LINE__, Tdelay1, RpWarning,   CleaningStageCounter, 0);
    }

    return status;

}