#include "FED4.h"

/**

* Feeds the mouse by dispensing a pellet from the hopper

*/

void FED4::feed()

{

initFeeding();

dispense();

handlePelletSettling();

handlePelletInWell();

finishFeeding();

}

void FED4::initFeeding() {

pelletPresent = checkForPellet();

pelletDropped = didPelletDrop();

pelletReady = false;

dispenseError = false;

//lightsOff();

// Serial.println("Feeding!");

}

void FED4::dispense() {

while (!pelletPresent && !pelletDropped) // while no pellet is present and none has dropped

{

redPix();

// check if pellet has dropped or is present

pelletDropped = didPelletDrop();

pelletPresent = checkForPellet();

pelletReady = true;

// Check if button is pressed - if so, fake pelletPresent to exit dispense

if (digitalRead(BUTTON_1) == 1) {

hapticDoubleBuzz();

marioPipe();

pelletPresent = true;

}

// Increment block pellet count when pellet drops

if (pelletDropped) {

blockPelletCount++;

}

// small motor movement

stepper.step(-10);

delay(2);

motorTurns++;

// delay for 1s roughly each pellet position

if (motorTurns % 25 == 0)

{

Serial.print("Dispensing... ");

Serial.println(motorTurns/25);

releaseMotor();

delay(1000);

}

//handle jam movements

handleJams();

}

}

void FED4::handleJams() {

// if stepper is called too many times without a dispense do a small movement to remove jam

if (motorTurns % 100 == 0)

{

minorJamClear();

}

if (motorTurns % 200 == 0)

{

vibrateJamClear();

}

if (motorTurns > 2000) //how many motorTurns before FED4 stops trying and shuts off? Each full rotation of the hopper is ~1000 motorTurns

{

jammed();

}

}

void FED4::handlePelletSettling() {

if (pelletReady) {

// Serial.println("PelletDrop");

pelletDropTime = millis();

pelletCount++;

logData("PelletDrop");

}

releaseMotor();

// Wait up to 500ms for pellet to settle in well if 500ms passes without detection, set dispenseError to true

unsigned long startWait = millis();

bool pelletDetected = false;

while (millis() - startWait < 500) {

if (checkForPellet()) {

pelletDetected = true;

pelletWellTime = millis();

break; // Exit if pellet is detected

}

delay(10);

}

// if pellet is not detected, set dispenseError to true

if (!pelletDetected) {

dispenseError = true;

}

// Calculate time since pellet drop

retrievalTime = (millis() - pelletWellTime) / 1000.0;

// Serial.println("Pellet in Well");

}

void FED4::handlePelletInWell() {

pelletPresent = checkForPellet();

updateDisplay();

// Reset wakePad at start to clear any stale interrupt flags

wakePad = 0;

while (pelletPresent)

{ // while pellet is in well, monitor for pokes and retrieval time

bluePix();

pelletPresent = checkForPellet();

retrievalTime = (static_cast<float>(millis() - pelletWellTime)) / 1000.0f;

if (retrievalTime > 20)

break;

// Use same majority-vote touch detection as normal pokes (direct L/C/R mapping)

if (wakePad != 0) {

interpretTouch();

if (leftTouch) {

retrievalTime = 0.0;

dispenseError = false;

logData("LeftWithPellet");

click();

updateDisplay();

outputPulse(1, 100);

resetTouchFlags();

} else if (centerTouch) {

retrievalTime = 0.0;

dispenseError = false;

logData("CenterWithPellet");

click();

updateDisplay();

bluePix();

outputPulse(2, 100);

resetTouchFlags();

} else if (rightTouch) {

retrievalTime = 0.0;

dispenseError = false;

logData("RightWithPellet");

click();

updateDisplay();

bluePix();

outputPulse(2, 100);

resetTouchFlags();

}

}

delay(10); // Small delay to prevent excessive CPU usage

}

}

void FED4::finishFeeding() {

purplePix();

// Serial.println("Pellet Removed");

if (pelletReady) {

if (dispenseError) {

retrievalTime = 0.0;

logData("PelletNotDetected");

} else {

logData("PelletTaken");

blockPokeCount = 0; // Reset block poke count when pellet is taken

}

}

// Reset variables

pelletReady = false;

retrievalTime = 0.0;

dispenseError = false;

// Reset touch states after handling the feed

leftTouch = false;

centerTouch = false;

rightTouch = false;

// Rebaseline touch sensors

reBaselineTouches = 3;

if ((leftCount + rightCount + centerCount) % reBaselineTouches == 0 && (leftCount + rightCount + centerCount) > 5)

{

calibrateTouchSensors();

}

}

/**

* Checks if the pellet is present in the center port

*

* @return bool - true if pellet is present, false otherwise

*/

bool FED4::checkForPellet()

{

return !mcp.digitalRead(EXP_PHOTOGATE_1);

}

/**

* Checks if the pellet is present dropped

*

* @return bool - true if pellet dropped, false otherwise

*/

bool FED4::didPelletDrop()

{

if (dropSensorAvailable) {

//With drop sensor use:

return !mcp.digitalRead(EXP_PHOTOGATE_4);

} else {

//Without drop sensor use:

return false; // Always return false when sensor is not available

}

}

/**

* Initializes the drop sensor and checks its status

*

* @return bool - true if drop sensor is working (HIGH), false if broken or not present (LOW)

*/

bool FED4::initializeDropSensor()

{

// Read the drop sensor status

bool sensorStatus = mcp.digitalRead(EXP_PHOTOGATE_4);

// Set the flag based on sensor status

dropSensorAvailable = sensorStatus;

// Return true if HIGH (sensor is good), false if LOW (broken or not present)

return sensorStatus;

}