// Parameters I used:
segMinScore(0.5)
segConvThresh(6.0)
segNormThresh(10.0)
MAX_DST_PM(0.2)
MAX_DST_FAC(4.0)


// Segmentation:
double segScoreL = frame->connectivityH.get(col-1,row)*frame->segmentConnectH.get(col-1,row);
double segScoreU = frame->connectivityV.get(col,row-1)*frame->segmentConnectV.get(col,row-1);
bool connectLeft = (segScoreL > segMinScore);
bool connectUpper = (segScoreU > segMinScore);


// function for calculating the "segmentConnectH" and "segmentConnectV" values
double connectWith(FrameSPtr frame, int col, int row, int col2, int row2)
{
  double convexThresh = cos((90.0f - segConvThresh)*M_PI/180); // convert to RAD
  double normalSimThresh = 1-cos(segNormThresh*M_PI/180); // convert to RAD
  
  DVector &n1 = frame->normal3D.get(col,row);
  DVector &n2 = frame->normal3D.get(col2,row2);
  double n1x = n1(0);
  double n1y = n1(1);
  double n1z = n1(2);
  double n2x = n2(0);
  double n2y = n2(1);
  double n2z = n2(2);

  if (n1x+n1y+n1z == 0.0) return 0.0; // length 0 meaning normal vector does not exist -> always reject
  if (n2x+n2y+n2z == 0.0) return 0.0; // length 0 meaning normal vector does not exist -> always reject

  DVector &p1 = frame->point3D.get(col,row);
  DVector &p2 = frame->point3D.get(col2,row2);
  double d12x = p2(0) - p1(0);
  double d12y = p2(1) - p1(1);
  double d12z = p2(2) - p1(2);
  double d12l = length(d12x, d12y, d12z);

  double retVal = 0.0;

  // normal direction approx. vertical
  retVal = max(retVal,  min(1.0f-fabs(n1z),1.0f-fabs(n2z)) );

  // similar normal direction
  double dp2n = dotProduct(n1x, n1y, n1z, n2x, n2y, n2z);
  retVal = max(retVal, sigmoidLikeSoftThresh(1-dp2n, normalSimThresh, 10.0) );

  // convexity
  double c1x, c1y, c1z, c2x, c2y, c2z;
  crossProduct(n1x, n1y, n1z, d12x, d12y, d12z, c1x, c1y, c1z);
  crossProduct(n2x, n2y, n2z, -d12x, -d12y, -d12z, c2x, c2y, c2z);

  retVal = max(retVal,  min( sigmoidLikeSoftThresh(dotProduct(n1x, n1y, n1z, d12x, d12y, d12z), convexThresh*d12l, 10.0)
                       ,min( sigmoidLikeSoftThresh(dotProduct(n2x, n2y, n2z, -d12x, -d12y, -d12z), convexThresh*d12l, 10.0)
                       ,min( sigmoidLikeSoftThresh(abs(dotProduct(c2x, c2y, c2z, n1x, n1y, n1z)), 0.3*d12l, 10.0)
                            ,sigmoidLikeSoftThresh(abs(dotProduct(c1x, c1y, c1z, n2x, n2y, n2z)), 0.3*d12l, 10.0)
                            ))));

  return retVal;
};


// function used for calculating the "connectivityH" and "connectivityV" values
/*! tries to estimate if two pixels/measurements belong to the same surface
 * \param d1 distance of one pixel of connection
 * \param d2 distance of other pixel of connection
 * \param diff distance-difference between the pixels
 * \param diffL, distance differences of left neighboring connection
 * \param diffR, distance differences of right neighboring connection
*/
inline double connectivity(double d1, double d2, double diff, double diffL, double diffR, double MAX_DST_PM, double MAX_DST_FAC) {
  if ((d1 == DBL_MAX) || (d2 == DBL_MAX))
    return 0.0;
  if (fabs(diff) < 0.1f) // always accept a small connection, so only check further if sufficient large connection (avoids division by zero)
    return 1.0;
  if ((fabs(diffL) < 0.01f) || (fabs(diffR) < 0.01f)) // always reject connection if it is not small but one neighboring connection is (avoids division by zero)
    return 0.0;

  double dist = std::min(d1, d2);
//  double keep = (dist < DBL_MAX) ? 1.0 : 0.0;
//  keep = fmin(keep, sigmoidLikeSoftThresh(diff, MAX_DST_PM*dist, 2/MAX_DST_PM/dist));  //  keep = keep && (diff < MAX_DST_PM_LR*dist);
//  keep = fmin(keep, sigmoidLikeSoftThresh(diff/diffL, MAX_DST_FAC, 1.0));  //  keep = keep && (diff/diffL < MAX_DST_FAC_LR);
//  keep = fmin(keep, sigmoidLikeSoftThresh(diff/diffR, MAX_DST_FAC, 1.0));  //  keep = keep && (diff/diffR < MAX_DST_FAC_LR);

  double keep = sigmoidLikeSoftThresh(fabs(diff), MAX_DST_PM*dist, 2/MAX_DST_PM/dist);
  keep = fmin(keep, sigmoidLikeSoftThresh(fabs((diff-diffL)/diffL), MAX_DST_FAC, 1.0f));
  keep = fmin(keep, sigmoidLikeSoftThresh(fabs((diff-diffR)/diffR), MAX_DST_FAC, 1.0f));
  return keep;

};


// normal vector calculation by weighted cross products
// pin[idx] are precomputed normalized distance vectors (center_point3D - point3D[idx])/norm_2(center_point3D - point[idx])
// wi[idx] are modified connectivityH/connectivityV: wi[idx] = (distance[idx] > center_distance + 2*lidarMeasStdDevConst) ? 1.0 : connectivity[idx]
// lidarMeasStdDevConst set to 0.015 for Velodyne
DVector n = zero_vector(3);
for (unsigned int i = 0; i < 4; ++i) { // calculates right-top, top-left, left-bottom and bottom-left cross products
  if (useConnection(i) && useConnection(i+1)) {
    double cpw = wi[i] * wi[i+1]; // weight by connection weights
    tmpN += cpw * cross_product(pin[i], pin[i+1];
  }
}
n /= norm_2(n); // normalize length to 1


// Utility funciton:
//! returns 1 for x << 0, and 0 for x >> 0, and 0.5 for x = 0
inline double sigmoidLikeShiftedUp(double x)
{
  return 0.5f + ((-0.5f*x)/sqrt(1+x*x));
}
//! returns 1 for x << thresh, and 0 for x >> thresh
inline double sigmoidLikeSoftThresh(double x, double thresh, double narrowFac, bool normalizeAtZero = false)
{
  x = (x-thresh)*narrowFac;
  if (normalizeAtZero)
    return sigmoidLikeShiftedUp(x)/sigmoidLikeShiftedUp(0.0f); // normalize so value f(0)=1
  else
    return sigmoidLikeShiftedUp(x);
}