// SERVO      44, 45
// DISTANCE   25
// METAL      27
// CONVEYOR   4
// MOISTURE   A10

#include <Servo.h>

bool isNotIdle = false;

// Input
int distance = 25;
int metalSensor = 27;
int moisture = A10;

// Output
Servo servo1, servo2;
int conveyor = 7;

// Program
enum Material {
  dry = 0, wet = 1, metal = 2, none = 3, unknown = 4,
};
Material material = none;
Material prevMat;
int wetThreshold = 900;

int tickDuration = 100; // Every clock ticks in ms
int conveyorTimer;
int servoTimer;
int wetThresholdCounter;

// Duration for timer to reset. 1 : 100 ms (1 : 0,1 s)
int conveyorDuration = 200;
int servoDuration = 70;
int wetDuration = 7;

// Counting
int total;
int metalPassed;
int wetPassed;
int dryPassed;

// Servo positions
int servo1Init = 40;
int servo1On = 5;
int servo2Init = 60;
int servo2On = 10;

// Turning off conveyor after servoTime == -1
bool justTurnedOff = true;

void setup() {
  pinMode(metalSensor, INPUT_PULLUP); digitalWrite(metal, HIGH);
  pinMode(moisture, INPUT);
  pinMode(distance, INPUT);

  pinMode(conveyor, OUTPUT);
  servo1.attach(44); servo2.attach(45);
  servo1.write(servo1Init); servo2.write(servo2Init);

  Serial.begin(9600);
}

void loop() {
  int distanceState = !digitalRead(distance);
  int metalState = !digitalRead(metalSensor);
  int moistureState = analogRead(moisture);

  // Checking Logic
  if (distanceState) {
    conveyorTimer = conveyorDuration;
  }

  if (metalState) {
    servoTimer = servoDuration;
    material = metal;
  }

  if (moistureState <= wetThreshold &&  material != wet) {
    wetThresholdCounter++;
    if (wetThresholdCounter >= wetDuration) {
      servoTimer = servoDuration;
      material = wet;
    }
    //servoTimer = servoDuration;
    //material = wet;
  }

  // Timer & Logics
  if (conveyorTimer >= 0) {
    conveyorTimer--;
    if (conveyorTimer == conveyorDuration - 1) {
      justTurnedOff = false;
    }
    if (conveyorTimer == 0) {
      if (material == none || material == unknown || prevMat == none) {
        material = dry;
      }
    } 
  } else {
    material = none;
  }

  if (servoTimer >= 0) {
    servoTimer--; 
    if (servoTimer == servoDuration) {
      servo1.write(servo1Init);
      servo2.write(servo2Init);
    }
    if (material == metal) {
      servo1.write(servo1On);
    } else if (material == wet) {
      servo2.write(servo2On);
    }
  } else {
    servo1.write(servo1Init);
    servo2.write(servo2Init);
    if (!justTurnedOff && material != none) {
      conveyorTimer = -1;
      wetThresholdCounter = 0;
      justTurnedOff = true;
    }
  }
  
  digitalWrite(conveyor, conveyorTimer >= 0);

  // Counting
  if (prevMat != material) {
    if (material == metal) {
      metalPassed++;
    } else if (material == wet) {
      wetPassed++;
    } else if (material == dry) {
      dryPassed++;
    }

    total = metalPassed + wetPassed + dryPassed;
    
    prevMat = material;
  }

  // On or off?
  if (conveyorTimer >= 0) {
    isNotIdle = true;
  } else {
    isNotIdle = false;
  }

  // Debugging
  
  Serial.print("{\"running\":");
  Serial.print(isNotIdle);
  Serial.print(",\"material\":");
  Serial.print(material);
  Serial.print(",\"countTotal\":");
  Serial.print(total);
  Serial.print(",\"countMetal\":");
  Serial.print(metalPassed);
  Serial.print(",\"countWet\":");
  Serial.print(wetPassed);
  Serial.print(",\"countDry\":");
  Serial.print(dryPassed);
  Serial.print(",\"conveyorTimer\":");
  Serial.print(conveyorTimer);
  Serial.print(",\"servoTimer\":");
  Serial.print(servoTimer);
  Serial.print(",\"wetTimer\":");
  Serial.print(wetThresholdCounter);
  Serial.print(",\"distance\":");
  Serial.print(distanceState);
  Serial.print(",\"moisture\":");
  Serial.print(moistureState);
  Serial.print(",\"metal\":");
  Serial.print(metalState);
  Serial.println("}");/*
  Serial.print("Distance ");
  Serial.print(distanceState);
  Serial.print(" Metal ");
  Serial.print(digitalRead(27));
  Serial.print(" Moist ");
  Serial.print(moistureState);
  Serial.print(" | Conveyor Timer ");
  Serial.print(conveyorTimer);
  Serial.print(" Servo Timer ");
  Serial.print(servoTimer);
  Serial.print(" Wet Timer ");
  Serial.println(wetThresholdTimer);*/
  
  delay(tickDuration);
}