 /*
  * Khoros: $Id: lvstereo.c,v 1.2 1991/12/18 09:27:02 dkhoros Exp $
  */

#if !defined(lint) && !defined(SABER)
static char rcsid[] = "Khoros: $Id: lvstereo.c,v 1.2 1991/12/18 09:27:02 dkhoros Exp $";
#endif

 /*
  * $Log: lvstereo.c,v $
 * Revision 1.2  1991/12/18  09:27:02  dkhoros
 * HellPatch3
 *
  */

/*
 *----------------------------------------------------------------------
 *
 * Copyright 1991, University of New Mexico.  All rights reserved.
 * Permission to copy and modify this software and its documen-
 * tation only for internal use in your organization is hereby
 * granted, provided that this notice is retained thereon and
 * on all copies.  UNM makes no representations as to the sui-
 * tability and operability of this software for any purpose.
 * It is provided "as is" without express or implied warranty.
 * 
 * UNM DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
 * INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FIT-
 * NESS.  IN NO EVENT SHALL UNM BE LIABLE FOR ANY SPECIAL,
 * INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY OTHER DAMAGES WHAT-
 * SOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER
 * IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
 * ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PER-
 * FORMANCE OF THIS SOFTWARE.
 * 
 * No other rights, including, for example, the right to redis-
 * tribute this software and its documentation or the right to
 * prepare derivative works, are granted unless specifically
 * provided in a separate license agreement.
 *---------------------------------------------------------------------
 */

#include "unmcopyright.h"        /* Copyright 1991 by UNM */

/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
 >>>>
 >>>>         File Name: lvstereo.c
 >>>>
 >>>>      Program Name: vstereo
 >>>>
 >>>> Date Last Updated: Sat Dec 14 18:14:47 1991 
 >>>>
 >>>>          Routines: lvstereo - the library call for vstereo
 >>>>
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/


#include "vinclude.h"


/* -library_includes */
/* -library_includes_end */


/****************************************************************
*
* Routine Name: lvstereo - library call for vstereo
*
* Purpose:
*    
*    Computes the depth in a matched pair of stereo image
*    
*    

* Input:
* Output:
*
* Written By: Pascal Adam
****************************************************************/


/* -library_def */
int lvstereo(image,output,focal,length,height,dist,max_dist)
struct xvimage *image, *output;
float focal,length,height,dist,max_dist;
/* -library_def_end */

/* -library_code */
{
int NC,
    NR,
    NCO,
    NRO,
    dim,
    dimo,
    i,
    j,
    k,kk,
    lim1, lim2,
    indl,
    indr,
    numreg, 
    nband,
    kband,
    interl, interr,
    *inp, *inp2;

int   *out;

double *Xl,     *Xr, 
       *Yl,     *Yr, 
       *Zg,     *Xgl,    *Ygl,    *Xgr,    *Ygr, 
       *nl,     *nr,
       indx,
       maxz;
       
    NC   = image  -> row_size; NR  = image  -> col_size;
    NCO  = NC / 2;
    NRO  = NR;
    dim  = NR  * NC;
    dimo = NRO * NCO;

    inp = (int *) image  -> imagedata;
    out = (int *) output -> imagedata;

    inp2 = (int *) malloc(dim * sizeof(int));

    for (i = 0; i < dim; i++) inp2[i] = inp[i];
  
    for (i=1; i < (NC-1); i++)
    {
       for (j=1; j < (NR-1); j++)
       {
          if (inp[i+NC*j] != 0)
          {
             if ((inp[i+NC*(j-1)]   != 0) &&
                 (inp[i+NC*(j+1)]   != 0) &&
                 (inp[i+1+NC*j]     != 0) &&
                 (inp[i-1+NC*j]     != 0) &&
                 (inp[i-1+NC*(j-1)] != 0) &&
                 (inp[i-1+NC*(j+1)] != 0) &&
                 (inp[i+1+NC*(j+1)] != 0) &&
                 (inp[i+1+NC*(j-1)] != 0)   )  inp2[i+NC*j] = 0;
             else inp2[i+NC*j] = inp[i+NC*j];
          }
          else inp2[i+NC*j] = 0;
       }
    }   
             
    inp = inp2;
    numreg = 0;
    for ( i=0 ; i < dim ; i++)
    {  
       if (numreg < inp[i]) numreg = inp[i];
    }
    numreg += 1;

    Xl    = (double *) malloc( sizeof(double) * numreg);
    Yl    = (double *) malloc( sizeof(double) * numreg);
    nl    = (double *) malloc( sizeof(double) * numreg);

    Xr    = (double *) malloc( sizeof(double) * numreg);
    Yr    = (double *) malloc( sizeof(double) * numreg);
    nr    = (double *) malloc( sizeof(double) * numreg);

    Xgl   = (double *) malloc( sizeof(double) * numreg);
    Ygl   = (double *) malloc( sizeof(double) * numreg);

    Xgr   = (double *) malloc( sizeof(double) * numreg);
    Ygr   = (double *) malloc( sizeof(double) * numreg);

    Zg    = (double *) malloc( sizeof(double) * numreg);


    if ((Xl    ==NULL)||(Yl    ==NULL)|| 
        (Xr    ==NULL)||(Yr    ==NULL)|| 
        (nl    ==NULL)||(nr    ==NULL)|| 
        (Xgl   ==NULL)||(Ygl   ==NULL)||
        (Xgr   ==NULL)||(Ygr   ==NULL)|| 
        (Zg    ==NULL) ) 
    {
       (void)fprintf(stderr,
       "lvstereo: Not enough available memory for allocation! \n");
       return(0);
    }


    for (i = 1; i< numreg; i++)
    {
       Xl[i]     = (double) 0.0;
       Xr[i]     = (double) 0.0;
       Yl[i]     = (double) 0.0;
       Yr[i]     = (double) 0.0;
       nl[i]     = (double) 0.0;
       nr[i]     = (double) 0.0;

    }

    for (i=0; i < NCO; i++)
    {
       for (j=0; j < NRO; j++)
       {
          indr = j*NC+i; indl = j*NC+i+NCO;

          indl = inp[indl];
          if (indl != 0)
          {
            Xl  [indl] +=(double)i;
            Yl  [indl] +=(double)j;
            nl  [indl] +=(double) 1;

          }
          
          indr = inp[indr];
          if (indr != 0)
          {
            Xr  [indr] +=(double)i;
            Yr  [indr] +=(double)j;
            nr  [indr] +=(double) 1;

          } 
       }
    }

    inp = (int *) image  -> imagedata;

    /* conversion of the result from pixel values to millimeters values */
    /* if x = pixel and l = length of the image in mm then x * l / nc   */
    /* is the value of x in mm (nc is the number of column in the image */
    indx   = (double) length / (double) NCO; 

    free(inp2);

    for (i = 1; i < numreg; i++)
    {
       Xgl[i] = Xl[i]/nl[i];
       Ygl[i] = Yl[i]/nl[i];
       Xgr[i] = Xr[i]/nr[i];
       Ygr[i] = Yr[i]/nr[i];
       if (((int)Xgl[i]-(int)Xgr[i]) < 0)
       {
          Zg[i] = (double)0.0;
       }

       else 
       {
          if ( (((int)Xgl[i]-(int)Xgr[i]) < 3) && (((int)Xgl[i]-(int)Xgr[i]) > 0) )  
          {
             Zg[i] = (double)max_dist;
          }
          else 
          {
             Zg[i]  = ((double)dist * (double)focal / (indx *(Xgl[i]-Xgr[i])));
             if (Zg[i] > (double)max_dist) Zg[i] = (double)max_dist;
          }
       }
    }
    
    for (i = 0; i< NCO; i++)
    {
       for (j=0; j < NRO; j++)
       {
          if (inp[i+NCO+j*NC] != 0) 
          {
             out[i+j*NCO] = (int) Zg[inp[i+NCO+j*NC]];
          }
          else out[i+j*NCO] = (int)0.0;

       }
    }
    return(1);
}
/* -library_code_end */
