//Water Bank => Tank 3, used for unsterilized water storage
//    -> Handle the Refill Water Bank Pump
//    -> Dependency: water bank's float switch Low and High

//STERILIZATION  Bank => Tank 2, used for sterilizing the water
//PURED Bank => Tank 3, used for store sterilized water

#define WATER_BANK_STATE_NONE 0
#define WATER_BANK_STATE_IDLE 1
#define WATER_BANK_STATE_START_FILLING 2
#define WATER_BANK_STATE_CHECK_FILLING_TO_LOWER 3
#define WATER_BANK_STATE_CHECK_FILLING_TO_UPPER 4

uint32_t waterBankExpectedTimeToFilledLower = 1*60*1000;//in mulliseconds
uint32_t waterBankExpectedTimeToFilledUpper = 3*60*1000;//in mulliseconds
uint32_t waterBankTick = 0;
bool waterBankSterilizationFloatSwitchStatePrevious = false;
bool waterBankSterilizationFloatSwitchStateCurrent = false;
bool waterBankSterilizedFloatSwitchStatePrevious = false;
bool waterBankSterilizedFloatSwitchStateCurrent = false;
bool waterBankFloatSwitchStatePrevious = false;
bool waterBankFloatSwitchStateCurrent = false;
bool waterBankSterilizationUpperFloatSwitchStatePrevious = false;
bool waterBankSterilizationUpperFloatSwitchStateCurrent = false;

//bool waterBankSystemIsRunning = false;//if true, water bank is starting once
//bool waterBankSystemIsAutomatic = false; //if true, it will keep check float switch for water refill

int waterBankState = WATER_BANK_STATE_NONE;
bool waterBankAutomatic = false;

#define WATER_BANK_ERROR_NONE 0
#define WATER_BANK_ERROR_LOWER_FLOAT_SWITCH 1 //lower switch error
#define WATER_BANK_ERROR_UPPER_FLOAT_SWITCH 2 //upper switch error
#define WATER_BANK_ERROR_UPPER_FLOAT_SWITCH_OR_NO_WATER 3
#define WATER_BANK_ERROR_LOWER_FLOAT_SWITCH_OR_NO_WATER 4
bool waterBankError = false;
byte waterBankErrorCode = WATER_BANK_ERROR_NONE;
String waterBankErrorString = "";

bool IsWaterBankError(){
  return waterBankError;
}

String WaterBankErrorString(){
  if (waterBankError) return waterBankErrorString;
  return "";
}

bool IsWaterBankSystemRunning(){
  if (!waterBankAutomatic) return (waterBankState!=WATER_BANK_STATE_NONE);
  return (waterBankState>WATER_BANK_STATE_IDLE);
}

void ClearWaterBankError(){
  waterBankError = false;
  waterBankErrorCode = WATER_BANK_ERROR_NONE;
  waterBankErrorString = "";        
}

//system running state =====================================================
void WaterBankSystemRoutine(){
  switch(waterBankState){
    case WATER_BANK_STATE_NONE:{
      //do nothing but make sure the pump is off
      if (GetSwitchState(WATER_BANK_FILL_PUMP_SWITCH)){
        Serial.println("WaterBank: TURN OFF PUMP SWITCH");
        SetSwitchState(WATER_BANK_FILL_PUMP_SWITCH, false);
      }
    };
    break;
    case WATER_BANK_STATE_IDLE:{
      if (waterBankAutomatic){
        //make sure the pump is off first
        if (GetSwitchState(WATER_BANK_FILL_PUMP_SWITCH)){
          Serial.println("WaterBank: TURN OFF PUMP SWITCH");
          SetSwitchState(WATER_BANK_FILL_PUMP_SWITCH, false);
        }
        //trigger condition: 
        // state => WATER_BANK_STATE_CHECK_IF_EMPTY: sterilization bank is empty or sterilized bank is empty or water bank is empty
        waterBankFloatSwitchStateCurrent = GetInSwitchState(WATER_BANK_FLOAT_SWITCH_LOWER);
        waterBankSterilizationFloatSwitchStateCurrent = GetInSwitchState(STERILIZATION_CHAMBER_FLOAT_SWITCH_LOWER);
        waterBankSterilizedFloatSwitchStateCurrent = GetInSwitchState(STERILIZED_CHAMBER_FLOAT_SWITCH_LOWER);
        waterBankSterilizationUpperFloatSwitchStateCurrent = GetInSwitchState(STERILIZATION_CHAMBER_FLOAT_SWITCH_UPPER);
        bool intoFillingWaterBank = false;
        //check if water bank empty
        if ((waterBankFloatSwitchStateCurrent!=waterBankFloatSwitchStatePrevious) && (!waterBankFloatSwitchStatePrevious)){
          intoFillingWaterBank = true;
          Serial.println("WaterBank -> Trigger reason: Water Bank empty");
        }
        //check if sterilization chamber empty
        if ((waterBankSterilizationFloatSwitchStateCurrent!=waterBankSterilizationFloatSwitchStatePrevious) && (!waterBankSterilizationFloatSwitchStateCurrent)){
          intoFillingWaterBank = true;
          Serial.println("WaterBank -> Trigger reason: Sterilization chamber empty");
        }
        //check if sterilized chamber empty
        if ((waterBankSterilizedFloatSwitchStateCurrent!=waterBankSterilizedFloatSwitchStatePrevious) && (!waterBankSterilizedFloatSwitchStatePrevious)){
          intoFillingWaterBank = true;
          Serial.println("WaterBank -> Trigger reason: Sterilized chamber empty");
        }
        //check if sterilization chamber just full lower switch 1, and upper switch from 1 to 0 (indicate begin to unload)
        if (waterBankSterilizationFloatSwitchStateCurrent && (waterBankSterilizationUpperFloatSwitchStateCurrent!=waterBankSterilizationUpperFloatSwitchStatePrevious) && (!waterBankSterilizationUpperFloatSwitchStateCurrent)){
          intoFillingWaterBank = true;
          Serial.println("WaterBank -> Trigger reason: Sterilized chamber unload");
        }
        //update state
        waterBankFloatSwitchStatePrevious = waterBankFloatSwitchStateCurrent;
        waterBankSterilizationFloatSwitchStatePrevious = waterBankSterilizationFloatSwitchStateCurrent;
        waterBankSterilizedFloatSwitchStatePrevious = waterBankSterilizedFloatSwitchStateCurrent;
        waterBankSterilizationUpperFloatSwitchStatePrevious = waterBankSterilizationUpperFloatSwitchStateCurrent;
        if (intoFillingWaterBank){
          ClearWaterBankError();
          waterBankState = WATER_BANK_STATE_START_FILLING;
          Serial.println("WaterBank -> WATER_BANK_STATE_START_FILLING");
        }
      }else{
        waterBankState = WATER_BANK_STATE_NONE;
        Serial.println("WaterBank -> WATER_BANK_STATE_NONE");
      }
    };
    break;
    case WATER_BANK_STATE_START_FILLING:{
      //call to this state to check if water is fully empty or lower level is filled
      bool lowerSwitchState = GetInSwitchState(WATER_BANK_FLOAT_SWITCH_LOWER);
      bool upperSwitchState = GetInSwitchState(WATER_BANK_FLOAT_SWITCH_UPPER);
      if (lowerSwitchState && (!upperSwitchState)){
        //try to fill till full
        ClearWaterBankError();
        waterBankTick = millis();
        waterBankState = WATER_BANK_STATE_CHECK_FILLING_TO_UPPER;
        Serial.println("WaterBank -> WATER_BANK_STATE_CHECK_FILLING_TO_UPPER");
      }else if ((!lowerSwitchState) && (!upperSwitchState)){
        //try to fill to lower first
        ClearWaterBankError();
        waterBankTick = millis();
        waterBankState = WATER_BANK_STATE_CHECK_FILLING_TO_LOWER;
        Serial.println("WaterBank -> WATER_BANK_STATE_CHECK_FILLING_TO_LOWER");
      }else if (!lowerSwitchState && upperSwitchState){
        //float switch error! Cancel the action
        waterBankError = true;
        waterBankErrorCode = WATER_BANK_ERROR_LOWER_FLOAT_SWITCH;
        //waterBankErrorString = "Tank 1 lower float switch error.";
        waterBankErrorString = "Tangki 3 float switch bawah error.";
        Serial.println("WaterBank Error -> WATER_BANK_ERROR_LOWER_FLOAT_SWITCH");
        if (waterBankAutomatic){
          waterBankState = WATER_BANK_STATE_IDLE;
          Serial.println("WaterBank -> WATER_BANK_STATE_IDLE");
        }else{
          waterBankState = WATER_BANK_STATE_NONE;
          Serial.println("WaterBank -> WATER_BANK_STATE_NONE");
        }
      }
    };
    break;
    case WATER_BANK_STATE_CHECK_FILLING_TO_LOWER:{
      //waiting for filling into lower level, or stop when timeout
      //switch on the pump
      if (!GetSwitchState(WATER_BANK_FILL_PUMP_SWITCH)){
        Serial.println("WaterBank: TURN ON PUMP SWITCH");
        SetSwitchState(WATER_BANK_FILL_PUMP_SWITCH, true);
      }
      bool lowerSwitchState = GetInSwitchState(WATER_BANK_FLOAT_SWITCH_LOWER);
      bool upperSwitchState = GetInSwitchState(WATER_BANK_FLOAT_SWITCH_UPPER);
      if (lowerSwitchState || upperSwitchState){
        //check if lower float switch on
        //try to fill till full
        ClearWaterBankError();
        waterBankTick = millis();
        waterBankState = WATER_BANK_STATE_CHECK_FILLING_TO_UPPER;
        Serial.println("WaterBank -> WATER_BANK_STATE_CHECK_FILLING_TO_UPPER");
      }else{
        //check if timed out
        if (millis()-waterBankTick>=waterBankExpectedTimeToFilledLower){
          //error
          waterBankError = true;
          waterBankErrorCode = WATER_BANK_ERROR_LOWER_FLOAT_SWITCH_OR_NO_WATER;
          //waterBankErrorString = "Tank 1 insufficient input water, or lower float switch error, or input pump error.";
          waterBankErrorString = "Tidak ada air untuk mengisi tangki 3, atau float switch error, atau pompa rusak.";
          Serial.println("WaterBank Error -> WATER_BANK_ERROR_LOWER_FLOAT_SWITCH");
          if (waterBankAutomatic){
            waterBankState = WATER_BANK_STATE_IDLE;
            Serial.println("WaterBank -> WATER_BANK_STATE_IDLE");
          }else{
            waterBankState = WATER_BANK_STATE_NONE;
            Serial.println("WaterBank -> WATER_BANK_STATE_NONE");
          }
        }
      }
    };
    break;
    case WATER_BANK_STATE_CHECK_FILLING_TO_UPPER:{
      //waiting for filling into upper level, or stop when timeout
      //switch on the pump
      if (!GetSwitchState(WATER_BANK_FILL_PUMP_SWITCH)){
        Serial.println("WaterBank: TURN ON PUMP SWITCH");
        SetSwitchState(WATER_BANK_FILL_PUMP_SWITCH, true);
      }
      bool upperSwitchState = GetInSwitchState(WATER_BANK_FLOAT_SWITCH_UPPER);
      bool stopFill = false;
      if (upperSwitchState){
        //check if upper float switch on
        ClearWaterBankError();
        stopFill = true;
        Serial.println("WaterBank -> Full");
      }else{
        //check if timed out
        if (millis()-waterBankTick>=waterBankExpectedTimeToFilledUpper){
          //error
          stopFill = true;
          waterBankError = true;
          waterBankErrorCode = WATER_BANK_ERROR_UPPER_FLOAT_SWITCH_OR_NO_WATER;
          waterBankErrorString = "Tank 3 insufficient input water, or upper float switch error, or input pump error.";
          Serial.println("WaterBank Error -> WATER_BANK_ERROR_UPPER_FLOAT_SWITCH_OR_NO_WATER");
        }
      }
      if (stopFill){
        SetSwitchState(WATER_BANK_FILL_PUMP_SWITCH, false); //matiin pompa
        if (waterBankAutomatic){
          waterBankState = WATER_BANK_STATE_IDLE;
          Serial.println("WaterBank -> WATER_BANK_STATE_IDLE");
        }else{
          waterBankState = WATER_BANK_STATE_NONE;
          Serial.println("WaterBank -> WATER_BANK_STATE_NONE");
        }
      }
    };
    break;
  }
}

void StartWaterBankSystem(bool automatic, bool forceRunIfAutomatic = false){
  RefreshInSwitchState();
  ClearWaterBankError();
  waterBankAutomatic = automatic;
  bool runNow = false;
  if (waterBankAutomatic){
    waterBankState = WATER_BANK_STATE_IDLE;
    runNow = forceRunIfAutomatic;
    if (!runNow) Serial.println("StartWaterBankSystem -> WATER_BANK_STATE_IDLE");
  }else{
    runNow = true;
  }

  if (runNow){
    waterBankState = WATER_BANK_STATE_START_FILLING;
    Serial.println("StartWaterBankSystem -> WATER_BANK_STATE_START_FILLING");
  }
}

void StopWaterBankSystem(){
  waterBankState = WATER_BANK_STATE_NONE;
  ClearWaterBankError();
  WaterBankSystemRoutine(); //call it once to stop the switch
}

//=========================================================================
//Test unit for WaterBankSystemRoutine
bool WaterBankTestUnit(){
  bool testA = false;
  bool testB = false;
  bool testC = false;
  bool testD = false;
  bool testE = false;
  
  //normal test with automatic mode
  ClearInSwitch(); //clear floating switch state
  StopWaterBankSystem();
  StartWaterBankSystem(true, false);
  WaterBankSystemRoutine();
  testA = waterBankState==WATER_BANK_STATE_IDLE;
  Serial.println("Test idle state " + (String) (testA?"OK":"FAIL"));
  //simulate float switching
  SetInSwitchState(WATER_BANK_FLOAT_SWITCH_LOWER, true);
  WaterBankSystemRoutine();
  SetInSwitchState(WATER_BANK_FLOAT_SWITCH_LOWER, false); //simulate trigger to fill water
  WaterBankSystemRoutine();
  testB = (waterBankState==WATER_BANK_STATE_START_FILLING) || (waterBankState==WATER_BANK_STATE_CHECK_FILLING_TO_LOWER);
  Serial.println("Test start filling " + (String) (testB?"OK":"FAIL"));
  WaterBankSystemRoutine();
  testC = waterBankState==WATER_BANK_STATE_CHECK_FILLING_TO_LOWER;
  Serial.println("Test filling to lower " + (String) (testC?"OK":"FAIL"));
  //simulate water on lower
  SetInSwitchState(WATER_BANK_FLOAT_SWITCH_LOWER, true);
  WaterBankSystemRoutine();
  testD = waterBankState==WATER_BANK_STATE_CHECK_FILLING_TO_UPPER;
  Serial.println("Test filling to upper " + (String) (testD?"OK":"FAIL"));
  //simulate tank full
  SetInSwitchState(WATER_BANK_FLOAT_SWITCH_UPPER, true);
  WaterBankSystemRoutine();
  testE = waterBankState==WATER_BANK_STATE_IDLE;
  Serial.println("Test filling to full " + (String) (testE?"OK":"FAIL"));
  StopWaterBankSystem();
  bool passNormalTest = testA && testB && testC && testD && testE;
  Serial.println("All Normal Test: " + (String) (passNormalTest?"OK":"FAIL"));
  
  //bool passFloatSwitchErrorTest = false;

  //bool passFillLowerTimeoutTest = false;

  //bool passFillUpperTimeoutTest = false;
  
//  return passNormalTest;

  //cleanup test
  ClearInSwitch(); //clear floating switch state
  StopWaterBankSystem();
  
  return true;
}
//=========================================================================