#include <Arduino.h>

constexpr auto outClk = 27;
constexpr auto outData = 13;
constexpr auto outLatch = 14;

constexpr auto inClk = 17;
constexpr auto inData = 5;
constexpr auto inLatch = 16;

constexpr auto wt = 20;

uint8_t ReadInput(void);
void WriteOutput(uint32_t);

void setup() {
  pinMode(outClk, OUTPUT);
  pinMode(outData, OUTPUT);
  pinMode(outLatch, OUTPUT);

  pinMode(inClk, OUTPUT);
  pinMode(inData, INPUT);
  pinMode(inLatch, OUTPUT);

  digitalWrite(inLatch, HIGH);
  digitalWrite(inClk, LOW);
  digitalWrite(inData, HIGH);

  digitalWrite(outClk, LOW);
  digitalWrite(outData, LOW);
  digitalWrite(outLatch, LOW);
  Serial.begin(115200);

  digitalWrite(inLatch, HIGH);

  pinMode(4, OUTPUT);
  digitalWrite(4, LOW);
}

// uint8_t debounce(void)
// {
//   bool debounce = false;
//   uint32_t wait{millis()};
//   while(!debounce){
//     uint8_t tmp = ReadInput();
//     delay(10);
//     if(tmp == ReadInput()) {
//       debounce = true;
//       return tmp;
//     }
//     if((millis() - wait) > 1000) return 255;
//   }
// }

void loop() {
  enum class Func{
    waitHydro,
    stopped,
    moveFirst,
    moveSecond,
  };
  Func status{Func::waitHydro};
  bool hydroOn{false};
  
  union{
    uint32_t val{0};
    struct{
      uint32_t dummy1:16;
      bool Hydro:1;
      bool AutoOn:1;
      bool Move1:1;
      bool Move2:1;
      bool empty:1;
      bool IndMove1:1;
      bool IndMove2:1;
      uint32_t dummy2:1;
    }; 
  }outputs;
  union{
    uint8_t val{0};
    struct{
      bool On:1;
      bool Off:1;
      bool Auto:1;
      bool Hand1:1;
      bool Hand2:1;
      bool Sensor1:1;                          
      bool Sensor2:1;
      bool Safety:1;
      };
    }inputs;
  uint8_t input_old{0};
  uint32_t output_old{0};
  uint32_t delayHydro{0};

  while(1)
  {
    bool debounce = false;
    while(!debounce){
      uint8_t tmp = ReadInput();
      delay(10);
      if(tmp == ReadInput()) {
        debounce = true;
        inputs.val = tmp;
      }
    }

    

    if(!inputs.Safety && inputs.Off && (inputs.On || hydroOn))
      hydroOn = true;
    else{
      hydroOn = false;
      outputs.Hydro = false;
    }
    
    switch (status)
    {
    case Func::waitHydro:
        if(hydroOn){
          outputs.Hydro = true;
          if(delayHydro == 0) delayHydro = millis();
          if((millis() - delayHydro) > 100){  
            status = Func::stopped;
          }
        }
      break;
    case Func::stopped:
      if(outputs.Hydro){
        if(!inputs.Auto) 
          status = Func::moveFirst;
        else
        {
          if(inputs.Hand1 && !inputs.Hand2) status = Func::moveFirst;
          if(inputs.Hand2 && !inputs.Hand1) status = Func::moveSecond;
        }
      }
      else{
        status = Func::waitHydro;
        delayHydro = 0;
      }
      break;
    case Func::moveFirst:
      if(outputs.Hydro){
        if(inputs.Sensor1){
          outputs.Move1 = false;
          if(!inputs.Auto) 
            status = Func::moveSecond;
          else
            status = Func::stopped;   
        }else{
          if(!inputs.Auto)
            outputs.Move1 = true;
          else{
            if(inputs.Hand1)
              outputs.Move1 = true;
            else{
              outputs.Move1 = false;
              status = Func::stopped;   
              //prev_status
            }
          }
        }
      }else{
        outputs.Move1 = false;
        //status = Func::stopped;
        status = Func::waitHydro;
        delayHydro = 0;
      }
      break;
    case Func::moveSecond:
      if(outputs.Hydro){
        if(inputs.Sensor2)
        {
          outputs.Move2 = false;
          if(!inputs.Auto) 
            status = Func::moveFirst;
          else
            status = Func::stopped;
        }else{
          if(!inputs.Auto){
            outputs.Move2 = true;
          }else{
            if(inputs.Hand2){
              outputs.Move2 = true;
            }else{
              outputs.Move2 = false;
              status = Func::stopped;
            }
          }
        }
      }else{
        outputs.Move2 = false;
        //status = Func::stopped;
        status = Func::waitHydro;
        delayHydro = 0;
      }
      break;
    default:
      break;
    }

    (!inputs.Auto)?(outputs.AutoOn = true):(outputs.AutoOn = false);
    (outputs.Move1)?(outputs.IndMove1 = true):(outputs.IndMove1 = false);
    (outputs.Move2)?(outputs.IndMove2 = true):(outputs.IndMove2 = false);

    if(output_old != outputs.val) {
      WriteOutput(outputs.val);
      output_old = outputs.val;
    }
  }    
};
  




  // put your main code here, to run repeatedly:


// put function definitions here:
uint8_t ReadInput(void)
{
  uint8_t data{0};
  digitalWrite(inLatch, HIGH);
  digitalWrite(inLatch, LOW);
  digitalWrite(inLatch, HIGH);
  for(uint8_t i = 0; i < 7; i++)
  {
    data |= digitalRead(inData);
    data <<= 1;
    digitalWrite(inClk, HIGH);
    digitalWrite(inClk, LOW);
  }
  data |= digitalRead(inData);
  return ~data;
}

void WriteOutput(uint32_t data)
{
  digitalWrite(outClk, LOW);
  digitalWrite(outLatch, LOW);
  for(uint8_t i = 0; i < 24; i++){
    (data & (1 << 23))?(digitalWrite(outData, HIGH)):(digitalWrite(outData, LOW));
    digitalWrite(outClk, HIGH);
    digitalWrite(outClk, LOW);
    data <<= 1;
  }
  digitalWrite(outLatch, HIGH);
  digitalWrite(outLatch, LOW);
}