m1_objectdetection.cpp

/*******************************************************************************
 Implementation of M1 Object Detection module.
 Copyright (c) 2025 - Mach1
*******************************************************************************/

#include "m1_objectdetection.h"

#if M1_OBJECTDETECTION_ENABLED

namespace Mach1
{

//==============================================================================
struct ObjectDetector::Impl
{
    // Reference object data
    juce::Image referenceImage;
    std::vector<std::vector<uint8_t>> referenceData;
    int refWidth = 0;
    int refHeight = 0;
    int refChannels = 0;
    
    // Detection parameters
    float confidenceThreshold = 0.5f;
    float proximityWeight = 0.3f;
    
    // Tracking state
    juce::Point<float> lastKnownPosition;
    bool hasLastKnownPosition = false;
    
    // Threading support
    class DetectionThread : public juce::Thread
    {
    public:
        DetectionThread(Impl* parent) : Thread("ObjectDetectionThread"), parentImpl(parent) {}
        
        void run() override
        {
            while (!threadShouldExit())
            {
                // Wait for new frame to process
                if (frameReadyEvent.wait(100)) // 100ms timeout
                {
                    if (threadShouldExit()) break;
                    
                    // Process the frame on this background thread
                    processFrame();
                }
            }
        }
        
        void submitFrame(const juce::Image& frame)
        {
            const juce::ScopedLock lock(frameLock);
            if (frame.isValid())
            {
                frameSkipCounter++;
                if (frameSkipCounter >= frameSkipCount)
                {
                    frameSkipCounter = 0;
                    currentFrame = frame.createCopy();
                    frameReadyEvent.signal();
                }
            }
        }
        
        void setFrameSkipCount(int count) { frameSkipCount = count; }
        void setCallback(ObjectDetector::DetectionCallback cb) { callback = cb; }
        
    private:
        void processFrame()
        {
            juce::Image frameToProcess;
            {
                const juce::ScopedLock lock(frameLock);
                if (!currentFrame.isValid()) return;
                frameToProcess = currentFrame.createCopy();
            }
            
            if (!parentImpl || !parentImpl->hasReferenceObjectInternal())
                return;
                
            DBG("Object detection thread: processing frame " + juce::String(frameToProcess.getWidth()) + "x" + juce::String(frameToProcess.getHeight()));
            auto startTime = juce::Time::getMillisecondCounterHiRes();
            
            // Scale down the frame for faster processing (1/4 size)
            int scaledWidth = frameToProcess.getWidth() / 4;
            int scaledHeight = frameToProcess.getHeight() / 4;
            DBG("Object detection thread: scaling frame to " + juce::String(scaledWidth) + "x" + juce::String(scaledHeight));
            
            juce::Image scaledFrame = frameToProcess.rescaled(scaledWidth, scaledHeight, juce::Graphics::lowResamplingQuality);
            
            DBG("Object detection thread: converting image to vector format");
            auto frameData = parentImpl->juceImageToVector(scaledFrame);
            int channels = scaledFrame.hasAlphaChannel() ? 4 : 3;
            
            DBG("Object detection thread: converted to vector, size: " + juce::String(frameData.size()) + " pixels, channels: " + juce::String(channels));
            
            DBG("Object detection thread: starting template matching");
            auto detected = parentImpl->findBestMatch(frameData, scaledWidth, scaledHeight, channels);
            
            auto endTime = juce::Time::getMillisecondCounterHiRes();
            double processingTime = endTime - startTime;
            
            DBG("Object detection thread: processing took " + juce::String(processingTime, 2) + " ms, confidence: " + juce::String(detected.confidence, 3));
            
            // Scale the detected coordinates back up to original frame size
            juce::Point<float> detectedCenter;
            if (detected.confidence >= parentImpl->confidenceThreshold)
            {
                detectedCenter.setX(detected.centerPosition.getX() * 4.0f);
                detectedCenter.setY(detected.centerPosition.getY() * 4.0f);
                
                // Update tracking state
                parentImpl->lastKnownPosition = detectedCenter;
                parentImpl->hasLastKnownPosition = true;
                
                DBG("Object detection thread: object detected at " + juce::String(detectedCenter.getX()) + ", " + juce::String(detectedCenter.getY()));
            } else {
                DBG("Object detection thread: no object detected (confidence " + juce::String(detected.confidence, 3) + " < threshold " + juce::String(parentImpl->confidenceThreshold, 3) + ")");
            }
            
            // Call the callback on the message thread
            if (callback)
            {
                DBG("Object detection thread: calling callback on message thread");
                juce::MessageManager::callAsync([this, detectedCenter, frameToProcess, processingTime]() {
                    if (callback)
                    {
                        callback(detectedCenter, frameToProcess.getWidth(), frameToProcess.getHeight(), processingTime);
                    }
                });
            }
        }
        
        Impl* parentImpl;
        juce::CriticalSection frameLock;
        juce::WaitableEvent frameReadyEvent;
        juce::Image currentFrame;
        int frameSkipCount = 5;
        int frameSkipCounter = 0;
        ObjectDetector::DetectionCallback callback;
    };
    
    std::unique_ptr<DetectionThread> detectionThread;
    bool asyncDetectionActive = false;
    
    // Helper functions
    float calculateSimilarity(const std::vector<std::vector<uint8_t>>& patch1,
                             const std::vector<std::vector<uint8_t>>& patch2,
                             int width, int height, int channels);
    
    std::vector<std::vector<uint8_t>> extractPatch(const std::vector<std::vector<uint8_t>>& imageData,
                                                  int imageWidth, int imageHeight,
                                                  int patchX, int patchY, 
                                                  int patchWidth, int patchHeight,
                                                  int channels);
    
    std::vector<std::vector<uint8_t>> juceImageToVector(const juce::Image& image);
    
    DetectedObject findBestMatch(const std::vector<std::vector<uint8_t>>& frameData,
                                int frameWidth, int frameHeight, int channels);
    
    bool hasReferenceObjectInternal() const
    {
        return !referenceData.empty() && refWidth > 0 && refHeight > 0;
    }
};

//==============================================================================
ObjectDetector::ObjectDetector()
    : pimpl(std::make_unique<Impl>())
{
}

ObjectDetector::~ObjectDetector() 
{
    // Stop async processing if active
    stopAsyncDetection();
}

//==============================================================================
bool ObjectDetector::setReferenceObject(const std::vector<std::vector<uint8_t>>& imageData,
                                       int width, int height, int channels)
{
    if (imageData.empty() || width <= 0 || height <= 0 || channels <= 0)
        return false;
    
    pimpl->referenceData = imageData;
    pimpl->refWidth = width;
    pimpl->refHeight = height;
    pimpl->refChannels = channels;
    
    return true;
}

bool ObjectDetector::setReferenceObject(const juce::Image& referenceImage)
{
    if (!referenceImage.isValid())
        return false;
    
    pimpl->referenceImage = referenceImage;
    pimpl->referenceData = pimpl->juceImageToVector(referenceImage);
    pimpl->refWidth = referenceImage.getWidth();
    pimpl->refHeight = referenceImage.getHeight();
    pimpl->refChannels = referenceImage.hasAlphaChannel() ? 4 : 3;
    
    return true;
}

//==============================================================================
// ASYNCHRONOUS API (threaded processing)

bool ObjectDetector::startAsyncDetection(DetectionCallback callback, int frameSkipCount)
{
    if (pimpl->asyncDetectionActive)
        return false; // Already running
    
    if (!callback)
        return false; // Invalid callback
    
    DBG("Starting asynchronous object detection");
    
    pimpl->detectionThread = std::make_unique<Impl::DetectionThread>(pimpl.get());
    pimpl->detectionThread->setCallback(callback);
    pimpl->detectionThread->setFrameSkipCount(frameSkipCount);
    pimpl->detectionThread->startThread();
    pimpl->asyncDetectionActive = true;
    
    DBG("Asynchronous object detection started");
    return true;
}

void ObjectDetector::stopAsyncDetection()
{
    if (!pimpl->asyncDetectionActive)
        return; // Not running
    
    DBG("Stopping asynchronous object detection");
    
    if (pimpl->detectionThread)
    {
        pimpl->detectionThread->signalThreadShouldExit();
        pimpl->detectionThread->stopThread(1000); // Wait up to 1 second
        pimpl->detectionThread.reset();
    }
    
    pimpl->asyncDetectionActive = false;
    DBG("Asynchronous object detection stopped");
}

bool ObjectDetector::submitFrame(const juce::Image& frameImage)
{
    if (!pimpl->asyncDetectionActive || !pimpl->detectionThread)
        return false;
    
    pimpl->detectionThread->submitFrame(frameImage);
    return true;
}

bool ObjectDetector::isAsyncDetectionActive() const
{
    return pimpl->asyncDetectionActive;
}

//==============================================================================
std::vector<DetectedObject> ObjectDetector::detectObjects(const std::vector<std::vector<uint8_t>>& frameData,
                                                         int frameWidth, int frameHeight, int channels,
                                                         int maxMatches)
{
    std::vector<DetectedObject> results;
    
    if (!hasReferenceObject())
        return results;
    
    // For now, implement simple single-object detection
    // This can be extended to find multiple objects
    auto detected = pimpl->findBestMatch(frameData, frameWidth, frameHeight, channels);
    
    if (detected.confidence >= pimpl->confidenceThreshold)
    {
        results.push_back(detected);
    }
    
    return results;
}

std::vector<DetectedObject> ObjectDetector::detectObjects(const juce::Image& frameImage, int maxMatches)
{
    if (!frameImage.isValid())
        return std::vector<DetectedObject>();
    
    auto frameData = pimpl->juceImageToVector(frameImage);
    int channels = frameImage.hasAlphaChannel() ? 4 : 3;
    
    return detectObjects(frameData, frameImage.getWidth(), frameImage.getHeight(), channels, maxMatches);
}

//==============================================================================
void ObjectDetector::setConfidenceThreshold(float threshold)
{
    pimpl->confidenceThreshold = juce::jlimit(0.0f, 1.0f, threshold);
}

float ObjectDetector::getConfidenceThreshold() const
{
    return pimpl->confidenceThreshold;
}

void ObjectDetector::setProximityWeight(float weight)
{
    pimpl->proximityWeight = juce::jlimit(0.0f, 1.0f, weight);
}

float ObjectDetector::getProximityWeight() const
{
    return pimpl->proximityWeight;
}

void ObjectDetector::resetTracking()
{
    pimpl->hasLastKnownPosition = false;
    pimpl->lastKnownPosition = juce::Point<float>();
}

bool ObjectDetector::hasReferenceObject() const
{
    return !pimpl->referenceData.empty() && pimpl->refWidth > 0 && pimpl->refHeight > 0;
}

//==============================================================================
// Helper function implementations
float ObjectDetector::Impl::calculateSimilarity(const std::vector<std::vector<uint8_t>>& patch1,
                                               const std::vector<std::vector<uint8_t>>& patch2,
                                               int width, int height, int channels)
{
    if (patch1.empty() || patch2.empty())
        return 0.0f;
    
    double sumSquaredDiff = 0.0;
    double sumSquaredRef = 0.0;
    
    for (int y = 0; y < height; ++y)
    {
        for (int x = 0; x < width; ++x)
        {
            for (int c = 0; c < channels; ++c)
            {
                int index = y * width * channels + x * channels + c;
                if (index < patch1.size() && index < patch2.size())
                {
                    double diff = patch1[index][0] - patch2[index][0];
                    sumSquaredDiff += diff * diff;
                    sumSquaredRef += patch2[index][0] * patch2[index][0];
                }
            }
        }
    }
    
    if (sumSquaredRef == 0.0)
        return 0.0f;
    
    // Normalized Cross-Correlation (NCC)
    return static_cast<float>(1.0 - (sumSquaredDiff / sumSquaredRef));
}

std::vector<std::vector<uint8_t>> ObjectDetector::Impl::extractPatch(const std::vector<std::vector<uint8_t>>& imageData,
                                                                    int imageWidth, int imageHeight,
                                                                    int patchX, int patchY,
                                                                    int patchWidth, int patchHeight,
                                                                    int channels)
{
    std::vector<std::vector<uint8_t>> patch;
    
    for (int y = 0; y < patchHeight; ++y)
    {
        for (int x = 0; x < patchWidth; ++x)
        {
            int srcY = patchY + y;
            int srcX = patchX + x;
            
            if (srcY >= 0 && srcY < imageHeight && srcX >= 0 && srcX < imageWidth)
            {
                int index = srcY * imageWidth * channels + srcX * channels;
                if (index < imageData.size())
                {
                    patch.push_back(imageData[index]);
                }
            }
        }
    }
    
    return patch;
}

std::vector<std::vector<uint8_t>> ObjectDetector::Impl::juceImageToVector(const juce::Image& image)
{
    std::vector<std::vector<uint8_t>> result;
    
    if (!image.isValid())
        return result;
    
    int width = image.getWidth();
    int height = image.getHeight();
    bool hasAlpha = image.hasAlphaChannel();
    int channels = hasAlpha ? 4 : 3;
    
    // Reserve space for efficiency
    result.reserve(width * height);
    
    juce::Image::BitmapData bitmapData(image, juce::Image::BitmapData::readOnly);
    
    for (int y = 0; y < height; ++y)
    {
        for (int x = 0; x < width; ++x)
        {
            juce::Colour pixel = bitmapData.getPixelColour(x, y);
            
            std::vector<uint8_t> pixelData;
            pixelData.reserve(channels);
            pixelData.push_back(pixel.getRed());
            pixelData.push_back(pixel.getGreen());
            pixelData.push_back(pixel.getBlue());
            if (hasAlpha)
                pixelData.push_back(pixel.getAlpha());
            
            result.push_back(std::move(pixelData));
        }
    }
    
    return result;
}

DetectedObject ObjectDetector::Impl::findBestMatch(const std::vector<std::vector<uint8_t>>& frameData,
                                                  int frameWidth, int frameHeight, int channels)
{
    DetectedObject bestMatch;
    float bestSimilarity = 0.0f;
    
    if (refWidth == 0 || refHeight == 0)
        return bestMatch;
    
    // Simple template matching
    for (int y = 0; y <= frameHeight - refHeight; y += 4) // Skip pixels for performance
    {
        for (int x = 0; x <= frameWidth - refWidth; x += 4)
        {
            auto patch = extractPatch(frameData, frameWidth, frameHeight, x, y, refWidth, refHeight, channels);
            float similarity = calculateSimilarity(patch, referenceData, refWidth, refHeight, channels);
            
            // Apply proximity weighting if we have a last known position
            if (hasLastKnownPosition && proximityWeight > 0.0f)
            {
                juce::Point<float> currentCenter(x + refWidth / 2.0f, y + refHeight / 2.0f);
                float distance = lastKnownPosition.getDistanceFrom(currentCenter);
                float maxDistance = std::sqrt(frameWidth * frameWidth + frameHeight * frameHeight);
                float proximityBonus = (1.0f - (distance / maxDistance)) * proximityWeight;
                similarity += proximityBonus;
            }
            
            if (similarity > bestSimilarity)
            {
                bestSimilarity = similarity;
                bestMatch.centerPosition = juce::Point<float>(x + refWidth / 2.0f, y + refHeight / 2.0f);
                bestMatch.bounds = juce::Rectangle<float>(x, y, refWidth, refHeight);
                bestMatch.confidence = similarity;
            }
        }
    }
    
    return bestMatch;
}

} // namespace Mach1

#endif // M1_OBJECTDETECTION_ENABLED