/*
 * IDS_Cleaning.c
 *
 *  Created on: 18 αξψυ 2019
 *      Author: User
 */
#include "include.h"
#include "ids.h"
#include "ids_ex.h"
#include "../control/control.h"
#include "modules/General/process.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"
double ArcCleaningMotorSpeed = 750;
int SaveLeftRockerSpeed = 50, SaveRightRockerSpeed = 50;
typedef enum
{
    CleaningStageIdle,
    CleaningStageActuatorUp = 1,
    CleaningStageDelay1,
    CleaningStageActuatorDown,
    CleaningStageDelay2,
}CleaningStageEnum;
TimerMotors_t MotorId = HARDWARE_MOTOR_TYPE__MOTO_DH_LID;

/*
 *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 Tup = 4,Tdelay1 = 9,Tdelay2 = 29;//set CleaningStageCounter to Tdelay1 - 1
int CleaningStageCounter = 8;//Tdelay1 - 1 to start with stage moving down

void Init_CleaningStageCounter()
{
    if(Head_Type == HEAD_TYPE_FLAT) {
        CleaningStageCounter = Tdelay1 - 1;
    } else {
        CleaningStageCounter = 0;
    }
}

uint32_t cleaningMotorCBFunction (uint32_t deviceID, uint32_t Parameter1)
{
    CleaningStageCounter = 0;
    return 0;
}

uint32_t IDS_Cleaning_Move_Actuators()
{
   if((Head_Type != HEAD_TYPE_FLAT) && (Head_Type != HEAD_TYPE_ARC))
       return OK;
   if(Head_Type == HEAD_TYPE_FLAT) {
       if (CleaningStageCounter == 1)
       {
           Trigger_Head_Actuators_Stub(ACTIN, ENABLE, UP);
           //Trigger_Head_Actuators_Control(ACTIN, LOW,true);
           ReportWithPackageFilter(IDSFilter,"IDS_Cleaning_Move_ActuatorUp", __FILE__, __LINE__, 1, RpWarning,   CleaningStageCounter, 0);
       }
       else if (CleaningStageCounter == Tup)
       {
           Trigger_Head_Actuators_Stub(ACTIN, DISABLE, DONTCARE);
           //Trigger_Head_Actuators_Disable();
           ReportWithPackageFilter(IDSFilter,"IDS_Cleaning_Stop_Actuator", __FILE__, __LINE__, Tup, RpWarning,   CleaningStageCounter, 0);
       }
       else if (CleaningStageCounter == Tdelay1)
       {
           Trigger_Head_Actuators_Stub(ACTIN, ENABLE, DOWN);
           //Trigger_Head_Actuators_Control(ACTIN, LOW,false);
           ReportWithPackageFilter(IDSFilter,"IDS_Cleaning_MoveDown", __FILE__, __LINE__, Tdelay1, RpWarning,   CleaningStageCounter, 0);
       }
       else if (CleaningStageCounter == Tdelay2)
       {
           CleaningStageCounter = 0;
           Trigger_Head_Actuators_Stub(ACTIN, DISABLE, DONTCARE);
           ReportWithPackageFilter(IDSFilter,"IDS_Cleaning_Stop_delay", __FILE__, __LINE__, Tdelay2, RpWarning,   CleaningStageCounter, 0);
       }
   } else { //arc
       if (CleaningStageCounter == 0) {
           if (ArcCleaningMotorSpeed>1)
               MotorSetSpeed(MotorId, ArcCleaningMotorSpeed);
           else
               MotorSetSpeed(MotorId, 750);
           ReportWithPackageFilter(IDSFilter,"IDS_Cleaning_Motor_Run", __FILE__, __LINE__, 750, RpWarning,   (int)ArcCleaningMotorSpeed, 0);
       }
   }
   CleaningStageCounter++;
   return OK;
}

uint32_t IDS_Cleaning_stop_cleaner_motor()
{
    TimerMotors_t MotorId = HARDWARE_MOTOR_TYPE__MOTO_DH_LID;
    if (Head_Type != HEAD_TYPE_ARC)
       return OK;

    MotorStop(MotorId,Soft_Stop);
    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);
    ReportWithPackageFilter(IDSFilter,"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);
    HeadCard_Actuators_Relocate();
    //Trigger_Head_Actuators_Disable();
    //Trigger_Head_Actuators_Control(ACTIN, LOW,false);
    CleaningStageCounter = 0;
    CleaningStage = CleaningStageIdle;
    ReportWithPackageFilter(IDSFilter,"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);
    Init_CleaningStageCounter();
    ReportWithPackageFilter(IDSFilter,"IDS_Cleaning_Spray_Cleaning_Solution", __FILE__, __LINE__, CLEANER_DISPENSER, RpWarning,   dispenserSpeed, 0);
    ReportWithPackageFilter(IDSFilter,"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);
    ReportWithPackageFilter(IDSFilter,"IDS_Cleaning_Stop_Cleaning_Solution", __FILE__, __LINE__, CLEANER_DISPENSER, RpWarning,   status, 0);
    if(Head_Type == HEAD_TYPE_FLAT)
    {
        HeadCard_Actuators_Relocate();
        Init_CleaningStageCounter();
    }
    if(Head_Type == HEAD_TYPE_ARC)
    {
        MotorMovetoLimitSwitch(MotorId,1-MotorsCfg[MotorId].directionthreadwize, 30,  Motor_Id_to_LS_IdDown[MotorId], cleaningMotorCBFunction,30000);
    }

    ReportWithPackageFilter(IDSFilter,"IDS_Cleaning_Motor_Homing", __FILE__, __LINE__, 1, RpWarning,   CleaningStageCounter, 0);
    CleaningStage = CleaningStageIdle;
    ReportWithPackageFilter(IDSFilter,"IDS_Cleaning_Stop_Cleaning_Solution actuator relocate", __FILE__, __LINE__, CleaningStage, RpWarning,   CleaningStageCounter, 0);

    return status;

}