/* calcMsgProd.c - Calculate BP messages products for each variable
 *
 * P = calcMsgProd(E0,E1,M,N)
 *
 *
 * Jerod Weinman
 * jerod@acm.org
 *
 * $Id: calcMsgProd.c 1 2009-05-07 13:41:00Z weinman $
 
// Jerod Weinman
// jerod@acm.org
// Copyright 2004, 2008, 2009
 
//    This file is part of Matlab GridCRF.
//
//    Matlab GridCRF is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    Matlab GridCRF is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with Matlab GridCRF.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "mex.h"
#include "math.h"

/* calcMsgProd - Does the msg product calculations */

void calcMsgProd(double* P, const double* E0, const double* E1, double* M, 
                 const int numEdges, const int numLabels, const int numNodes)
{


  /* P:  NxL
   * E0: Ex1
   * E1: Ex1
   * M:  ExLx2
   */

  /* pE0: Pointer to edge node 0
   * pE1: Pointer to edge node 1
   * pM:  Pointer to edge message
   * pP:  Pointer to message product
   */
  double *pE0, *pE1, *pM,*pP;
  

  int e,c; /* Edge num, label index */
   
  int msgOffset = numEdges * numLabels;

  pE0 = E0;
  pE1 = E1;
  pM = M;
           
  for (e=0 ; e<numEdges ; e++)
  {


    /* 
     * Add message going into E0 node (second index of M)
     */

    /* Point to proper node index for the sum */
    pP = P + ((int)(*pE0)) - 1; /* minus 1 for zero indexing in C */

    for (c=0 ; c<numLabels ; c++)
    {
      /*printf("%d\t%d\t%d\t%d\t%f\n",e,((int)(*pE0)) - 1,c,
        c*numEdges + msgOffset, *(pM + c*numEdges + msgOffset)); */

      *pP += *(pM + c*numEdges + msgOffset);
      pP += numNodes;
    }

    /*
     * Add messages going into E1 node (first index of M)
     */

    /* Point to proper node index for the sum */
    pP = P + ((int)(*pE1)) - 1; /* minus 1 for zero indexing in C */
    
    for (c=0 ; c<numLabels ; c++)
    {
      *pP += *(pM + c*numEdges);
      pP += numNodes;
    }

    pE0++;
    pE1++;
    pM++;

  }

}

  /* Gateway routine */
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{

  mxArray* mxT;

  double *P, *E0, *E1, *M;

  int numEdges, numLabels, numNodes;

  /* Check for proper number of arguments */
  if (nrhs!=4)
	mexErrMsgTxt("Four inputs required");

  /* Check to make sure the arguments are real */
  int i;
  for (i=0 ; i<nrhs ; i++)
    if ( mxIsComplex(prhs[i]) )
      mexErrMsgTxt("Input arguments must be real.");

  if (mxGetNumberOfDimensions(prhs[2])!=3)
    mexErrMsgTxt("M must have three dimensions");

  const int* szM = mxGetDimensions(prhs[2]);
  
  numEdges = szM[0];
  numLabels = szM[1];

  numNodes = mxGetScalar(prhs[3]);

  E0 = mxGetPr(prhs[0]);
  E1 = mxGetPr(prhs[1]);
  M = mxGetPr(prhs[2]);

  /* Create output matrix */

  plhs[0] = mxCreateDoubleMatrix(numNodes,numLabels,mxREAL);
  P = mxGetPr(plhs[0]);

  /*printf("numEdges: %d\nnumLabels: %d\nnumNodes: %d\n",
    numEdges,numLabels,numNodes);*/

  /* Calculate message products */
  calcMsgProd(P,E0,E1,M,numEdges,numLabels,numNodes);


}
