#include "Eigenbackground.h"

#if CV_MAJOR_VERSION >= 2 && CV_MAJOR_VERSION <= 3

using namespace bgslibrary::algorithms::dp;

Eigenbackground::Eigenbackground()
{
  m_pcaData = NULL;
  m_pcaAvg = NULL;
  m_eigenValues = NULL;
  m_eigenVectors = NULL;
}

Eigenbackground::~Eigenbackground()
{
  if (m_pcaData != NULL) cvReleaseMat(&m_pcaData);
  if (m_pcaAvg != NULL) cvReleaseMat(&m_pcaAvg);
  if (m_eigenValues != NULL) cvReleaseMat(&m_eigenValues);
  if (m_eigenVectors != NULL) cvReleaseMat(&m_eigenVectors);
}

void Eigenbackground::Initalize(const BgsParams& param)
{
  m_params = (EigenbackgroundParams&)param;

  m_background = cvCreateImage(cvSize(m_params.Width(), m_params.Height()), IPL_DEPTH_8U, 3);
  m_background.Clear();
}

void Eigenbackground::InitModel(const RgbImage& data)
{
  if (m_pcaData != NULL) cvReleaseMat(&m_pcaData);
  if (m_pcaAvg != NULL) cvReleaseMat(&m_pcaAvg);
  if (m_eigenValues != NULL) cvReleaseMat(&m_eigenValues);
  if (m_eigenVectors != NULL) cvReleaseMat(&m_eigenVectors);

  m_pcaData = cvCreateMat(m_params.HistorySize(), m_params.Size() * 3, CV_8UC1);

  m_background.Clear();
}

void Eigenbackground::Update(int frame_num, const RgbImage& data, const BwImage& update_mask)
{
  // the eigenbackground model is not updated (serious limitation!)
}

void Eigenbackground::Subtract(int frame_num, const RgbImage& data,
  BwImage& low_threshold_mask, BwImage& high_threshold_mask)
{
  // create eigenbackground
  if (frame_num == m_params.HistorySize())
  {
    // create the eigenspace
    m_pcaAvg = cvCreateMat(1, m_pcaData->cols, CV_32F);
    m_eigenValues = cvCreateMat(m_pcaData->rows, 1, CV_32F);
    m_eigenVectors = cvCreateMat(m_pcaData->rows, m_pcaData->cols, CV_32F);
    cvCalcPCA(m_pcaData, m_pcaAvg, m_eigenValues, m_eigenVectors, CV_PCA_DATA_AS_ROW);

    int index = 0;
    for (unsigned int r = 0; r < m_params.Height(); ++r)
    {
      for (unsigned int c = 0; c < m_params.Width(); ++c)
      {
        for (int ch = 0; ch < m_background.Ptr()->nChannels; ++ch)
        {
          m_background(r, c, 0) = static_cast<unsigned char>(cvmGet(m_pcaAvg, 0, index) + 0.5);
          index++;
        }
      }
    }
  }

  if (frame_num >= m_params.HistorySize())
  {
    // project new image into the eigenspace
    int w = data.Ptr()->width;
    int h = data.Ptr()->height;
    int ch = data.Ptr()->nChannels;
    CvMat* dataPt = cvCreateMat(1, w*h*ch, CV_8UC1);
    CvMat data_row;
    cvGetRow(dataPt, &data_row, 0);
    cvReshape(&data_row, &data_row, 3, data.Ptr()->height);
    cvCopy(data.Ptr(), &data_row);

    CvMat* proj = cvCreateMat(1, m_params.EmbeddedDim(), CV_32F);
    cvProjectPCA(dataPt, m_pcaAvg, m_eigenVectors, proj);

    // reconstruct point
    CvMat* result = cvCreateMat(1, m_pcaData->cols, CV_32F);
    cvBackProjectPCA(proj, m_pcaAvg, m_eigenVectors, result);

    // calculate Euclidean distance between new image and its eigenspace projection
    int index = 0;
    for (unsigned int r = 0; r < m_params.Height(); ++r)
    {
      for (unsigned int c = 0; c < m_params.Width(); ++c)
      {
        double dist = 0;
        bool bgLow = true;
        bool bgHigh = true;
        for (int ch = 0; ch < 3; ++ch)
        {
          dist = (data(r, c, ch) - cvmGet(result, 0, index))*(data(r, c, ch) - cvmGet(result, 0, index));
          if (dist > m_params.LowThreshold())
            bgLow = false;
          if (dist > m_params.HighThreshold())
            bgHigh = false;
          index++;
        }

        if (!bgLow)
        {
          low_threshold_mask(r, c) = FOREGROUND;
        }
        else
        {
          low_threshold_mask(r, c) = BACKGROUND;
        }

        if (!bgHigh)
        {
          high_threshold_mask(r, c) = FOREGROUND;
        }
        else
        {
          high_threshold_mask(r, c) = BACKGROUND;
        }
      }
    }

    cvReleaseMat(&result);
    cvReleaseMat(&proj);
    cvReleaseMat(&dataPt);
  }
  else
  {
    // set entire image to background since there is not enough information yet
    // to start performing background subtraction
    for (unsigned int r = 0; r < m_params.Height(); ++r)
    {
      for (unsigned int c = 0; c < m_params.Width(); ++c)
      {
        low_threshold_mask(r, c) = BACKGROUND;
        high_threshold_mask(r, c) = BACKGROUND;
      }
    }
  }

  UpdateHistory(frame_num, data);
}

void Eigenbackground::UpdateHistory(int frame_num, const RgbImage& new_frame)
{
  if (frame_num < m_params.HistorySize())
  {
    CvMat src_row;
    cvGetRow(m_pcaData, &src_row, frame_num);
    cvReshape(&src_row, &src_row, 3, new_frame.Ptr()->height);
    cvCopy(new_frame.Ptr(), &src_row);
  }
}

#endif
