double itemAnimationTime(LWItemID item, int *keys)
{
	double t = 0.0;
	LWChannelID chan[3];
	int		k[3]={0,0,0}, typ;
	LWDVector tim;

	VCLR(tim);
	typ = itemInfo->type(item);
	k[0] = itemKeys(item, LWIP_POSITION, chan,tim);
	k[1] = itemKeys(item, LWIP_ROTATION, chan,tim+1);
	if(typ==LWI_OBJECT)
		k[2] = itemKeys(item, LWIP_SCALING, chan,tim+2);
	else if(typ==LWI_LIGHT)
	{
		LWEnvelopeID intens;
		lightIntensity(item,0.0,&intens);
		if(intens)
			tim[2] = envDuration(intens, &(k[2]));
		k[2] += 2; // catch up to 3-channel types
	}
	t = MAX3(tim[0],tim[1],tim[2]);
	if(keys)
		*keys = MAX3(k[0],k[1],k[2]);
	return t;
}

int compare_boxes(BBOX &box1, BBOX &box2)
{

    double tol_x, tol_y, tol_z;

    tol_x = 0.01*MAX3( ABS(box1.x_max-box1.x_min), ABS(box2.x_max-box2.x_min), 1. );
    tol_y = 0.01*MAX3( ABS(box1.y_max-box1.y_min), ABS(box2.y_max-box2.y_min), 1. );
    tol_z = 0.01*MAX3( ABS(box1.z_max-box1.z_min), ABS(box2.z_max-box2.z_min), 1. );

//    tol_x = tol_y = tol_z = 0.;

    if ( (box1.x_min - box2.x_max) > tol_x ) return(0); // Return 0 if no overlap

    if ( (box2.x_min - box1.x_max) > tol_x ) return(0);

    if ( (box1.y_min - box2.y_max) > tol_y ) return(0);

    if ( (box2.y_min - box1.y_max) > tol_y ) return(0);

    if ( (box1.z_min - box2.z_max) > tol_z ) return(0);

    if ( (box2.z_min - box1.z_max) > tol_z ) return(0);

    return(1); // Boxes overlap

}

long point_triangle_intersection(Point3 p, Triangle3 t)
{
long sign12,sign23,sign31;
Point3 vect12,vect23,vect31,vect1h,vect2h,vect3h;
Point3 cross12_1p,cross23_2p,cross31_3p;

/* First, a quick bounding-box test:                               */
/* If P is outside triangle bbox, there cannot be an intersection. */

   if (p.x > MAX3(t.v1.x, t.v2.x, t.v3.x)) return(OUTSIDE);  
   if (p.y > MAX3(t.v1.y, t.v2.y, t.v3.y)) return(OUTSIDE);
   if (p.z > MAX3(t.v1.z, t.v2.z, t.v3.z)) return(OUTSIDE);
   if (p.x < MIN3(t.v1.x, t.v2.x, t.v3.x)) return(OUTSIDE);
   if (p.y < MIN3(t.v1.y, t.v2.y, t.v3.y)) return(OUTSIDE);
   if (p.z < MIN3(t.v1.z, t.v2.z, t.v3.z)) return(OUTSIDE);

/* For each triangle side, make a vector out of it by subtracting vertexes; */
/* make another vector from one vertex to point P.                          */
/* The crossproduct of these two vectors is orthogonal to both and the      */
/* signs of its X,Y,Z components indicate whether P was to the inside or    */
/* to the outside of this triangle side.                                    */

   SUB(t.v1, t.v2, vect12)
   SUB(t.v1,    p, vect1h);
   CROSS(vect12, vect1h, cross12_1p)
   sign12 = SIGN3(cross12_1p);      /* Extract X,Y,Z signs as 0..7 or 0...63 integer */

   SUB(t.v2, t.v3, vect23)
   SUB(t.v2,    p, vect2h);
   CROSS(vect23, vect2h, cross23_2p)
   sign23 = SIGN3(cross23_2p);

   SUB(t.v3, t.v1, vect31)
   SUB(t.v3,    p, vect3h);
   CROSS(vect31, vect3h, cross31_3p)
   sign31 = SIGN3(cross31_3p);

/* If all three crossproduct vectors agree in their component signs,  */
/* then the point must be inside all three.                           */
/* P cannot be OUTSIDE all three sides simultaneously.                */

   /* this is the old test; with the revised SIGN3() macro, the test
    * needs to be revised. */
#ifdef OLD_TEST
   if ((sign12 == sign23) && (sign23 == sign31))
      return(INSIDE);
   else
      return(OUTSIDE);
#else
   return ((sign12 & sign23 & sign31) == 0) ? OUTSIDE : INSIDE;
#endif
}

bool
NBRampsComputer::fulfillsRampConstraints(
    NBEdge* potHighway, NBEdge* potRamp, NBEdge* other, SUMOReal minHighwaySpeed, SUMOReal maxRampSpeed) {
    // do not build ramps on rail edges
    if (isRailway(potHighway->getPermissions()) || isRailway(potRamp->getPermissions())) {
        return false;
    }
    // do not build ramps on connectors
    if (potHighway->isMacroscopicConnector() || potRamp->isMacroscopicConnector() || other->isMacroscopicConnector()) {
        return false;
    }
    // check whether a lane is missing
    if (potHighway->getNumLanes() + potRamp->getNumLanes() <= other->getNumLanes()) {
        return false;
    }
    // check conditions
    // is it really a highway?
    SUMOReal maxSpeed = MAX3(potHighway->getSpeed(), other->getSpeed(), potRamp->getSpeed());
    if (maxSpeed < minHighwaySpeed) {
        return false;
    }
    /*
    if (potHighway->getSpeed() < minHighwaySpeed || other->getSpeed() < minHighwaySpeed) {
        return false;
    }
    */
    // is it really a ramp?
    if (maxRampSpeed > 0 && maxRampSpeed < potRamp->getSpeed()) {
        return false;
    }
    return true;
}

void
MELoop::checkCar(MEVehicle* veh) {
    const SUMOTime leaveTime = veh->getEventTime();
    MESegment* const onSegment = veh->getSegment();
    MESegment* const toSegment = nextSegment(onSegment, veh);
    const bool teleporting = (onSegment == 0); // is the vehicle currently teleporting?
    if (changeSegment(veh, leaveTime, toSegment, teleporting)) {
        return;
    }
    if (MSGlobals::gTimeToGridlock > 0 && veh->getWaitingTime() > MSGlobals::gTimeToGridlock && !veh->isStopped()) {
        teleportVehicle(veh, toSegment);
        return;
    }
    if (veh->getBlockTime() == SUMOTime_MAX) {
        veh->setBlockTime(leaveTime);
    }
    if (leaveTime < toSegment->getEntryBlockTime()) {
        // receiving segment has recently received another vehicle
        veh->setEventTime(toSegment->getEntryBlockTime());
    } else if (toSegment->hasSpaceFor(veh, leaveTime) && !veh->mayProceed()) {
        // either the junction is blocked or the traffic light is red
        veh->setEventTime(leaveTime + MAX2(SUMOTime(1), myLinkRecheckInterval));
    } else {
        SUMOTime newEventTime = MAX3(toSegment->getEventTime() + 1, leaveTime + 1, leaveTime + myFullRecheckInterval);
        if (MSGlobals::gTimeToGridlock > 0) {
            // if teleporting is enabled, make sure we look at the vehicle when the the gridlock-time is up
            newEventTime = MIN2(newEventTime, veh->getBlockTime() + MSGlobals::gTimeToGridlock + 1);
        }
        veh->setEventTime(newEventTime);
    }
    addLeaderCar(veh, onSegment->getLink(veh));
}

inline
void rgb_to_hls(float r, float g, float b, float &h, float &l, float &s) {
/* Given: r, b, g each in [0.1]
 * Desired: h in [0,360), l and s in [0,1]
 */

  float max = MAX3(r,g,b);
  float min = MIN3(r,g,b);
  l = (max+min)/2.0f;   /* This is lightness */
  /* Next calculate saturation */
  if (max == min) {   /* Achromatic, because r = g = b */
    s = 0.0f;
    h = 0.0f;       /* Actually: UNDEFINED */
  }
  else  {     /* Chromatic case */
    float delta = max-min;

    /* First calculate saturation */
    s = (l<=0.5f) ? (delta / (max+min)) : (delta / (2.0f - (max+min)));
    /* Next calculate the hue */
    if (r == max)
      h = (g-b)/delta;
    else if (g == max)
      h = 2.0f + (b-r)/delta;
    else if (b == max)
      h = 4.0f + (r-g)/delta;
    h *= 60.0f;
    if (h<0.0f)
      h += 360.0f;
  } /*Chromatic case */
} /* RGB_To_HLS */

hsv_color RGBtoHSV(rgba_color rgb) {
	float min, max, delta;
	min = MIN3(rgb.r, rgb.g, rgb.b);
	max = MAX3(rgb.r, rgb.g, rgb.b);

	hsv_color ret;
	ret.a = rgb.a;

	ret.v = max;				// v
	delta = max - min;
	if (max != 0)
		ret.s = delta / max;		// s
	else {
		// r = g = b = 0		// s = 0, v is undefined
		ret.s = 0;
		ret.h = -1;
		return ret;
	}
	if (rgb.r == max)
		ret.h = (rgb.g - rgb.b) / delta;		// between yellow & magenta
	else if (rgb.g == max)
		ret.h = 2 + (rgb.b - rgb.r) / delta;	// between cyan & yellow
	else
		ret.h = 4 + (rgb.r - rgb.g) / delta;	// between magenta & cyan
	ret.h *= 60;				// degrees
	if (ret.h < 0)
		ret.h += 360;

	return ret;
}

/**
 * @brief Convert an sRGB color to Hue-Saturation-Lightness (HSL)
 *
 * @param H, S, L pointers to hold the result
 * @param R, G, B the input sRGB values scaled in [0,1]
 *
 * This routine transforms from sRGB to the double hexcone HSL color space
 * The sRGB values are assumed to be between 0 and 1.  The outputs are
 *   H = hexagonal hue angle                (0 <= H < 360),
 *   S = { C/(2L)     if L <= 1/2           (0 <= S <= 1),
 *       { C/(2 - 2L) if L >  1/2
 *   L = (max(R',G',B') + min(R',G',B'))/2  (0 <= L <= 1),
 * where C = max(R',G',B') - min(R',G',B').  The inverse color transformation
 * is given by Hsl2Rgb.
 *
 * Wikipedia: http://en.wikipedia.org/wiki/HSL_and_HSV
 */
void Rgb2Hsl(num *H, num *S, num *L, num R, num G, num B)
{
	num Max = MAX3(R, G, B);
	num Min = MIN3(R, G, B);
	num C = Max - Min;


	*L = (Max + Min)/2;

	if(C > 0)
	{
		if(Max == R)
		{
			*H = (G - B) / C;

			if(G < B)
				*H += 6;
		}
		else if(Max == G)
			*H = 2 + (B - R) / C;
		else
			*H = 4 + (R - G) / C;

		*H *= 60;
		*S = (*L <= 0.5) ? (C/(2*(*L))) : (C/(2 - 2*(*L)));
	}
	else
		*H = *S = 0;
}

void Rgb2Hf(float *H, float R, float G, float B)
{
	float Max = MAX3(R, G, B);
	float Min = MIN3(R, G, B);
	float C = Max - Min;

	if(C > 0)
	{
		if(Max == R)
		{
			*H = (G - B) / C;

			if(G < B)
				*H += 6;
		}
		else if(Max == G)
			*H = 2 + (B - R) / C;
		else
			*H = 4 + (R - G) / C;

		*H *= 60;
	}
	else
		*H = 0;
}

/**
 * @brief Convert an sRGB color to Hue-Saturation-Value (HSV)
 *
 * @param H, S, V pointers to hold the result
 * @param R, G, B the input sRGB values scaled in [0,1]
 *
 * This routine transforms from sRGB to the hexcone HSV color space.  The
 * sRGB values are assumed to be between 0 and 1.  The output values are
 *   H = hexagonal hue angle   (0 <= H < 360),
 *   S = C/V                   (0 <= S <= 1),
 *   V = max(R',G',B')         (0 <= V <= 1),
 * where C = max(R',G',B') - min(R',G',B').  The inverse color transformation
 * is given by Hsv2Rgb.
 *
 * Wikipedia: http://en.wikipedia.org/wiki/HSL_and_HSV
 */
void Rgb2Hsv(num *H, num *S, num *V, num R, num G, num B)
{
	num Max = MAX3(R, G, B);
	num Min = MIN3(R, G, B);
	num C = Max - Min;


	*V = Max;

	if(C > 0)
	{
		if(Max == R)
		{
			*H = (G - B) / C;

			if(G < B)
				*H += 6;
		}
		else if(Max == G)
			*H = 2 + (B - R) / C;
		else
			*H = 4 + (R - G) / C;

		*H *= 60;
		*S = C / Max;
	}
	else
		*H = *S = 0;
}

struct hsv_color rgbtohsv_int(struct rgb_color rgb) {
    struct hsv_color hsv;
    uint8_T rgb_min, rgb_max;

    rgb_min = MIN3(rgb.r, rgb.g, rgb.b);
    rgb_max = MAX3(rgb.r, rgb.g, rgb.b);
    
    if((rgb_max-rgb_min)==0){
        return hsv;
    }else if(rgb_max==0){
        return hsv;
    }
    
    /* Compute hue */
    if (rgb_max == rgb.r) {
        hsv.hue = 0 + 43*(rgb.g - rgb.b)/(rgb_max - rgb_min);
    } else if (rgb_max == rgb.g) {
        hsv.hue = 85 + 43*(rgb.b - rgb.r)/(rgb_max - rgb_min);
    } else /* rgb_max == rgb.b */ {
        hsv.hue = 171 + 43*(rgb.r - rgb.g)/(rgb_max - rgb_min);
    }
    hsv.val = rgb_max;
    hsv.sat = 255*(long)(rgb_max - rgb_min)/rgb_max;
    return hsv;
}

int  _IsolateMinorE4(graphP theGraph)
{
isolatorContextP IC = &theGraph->IC;

     if (IC->px != IC->x)
     {
         if (_MarkPathAlongBicompExtFace(theGraph, IC->r, IC->w) != OK ||
             _MarkPathAlongBicompExtFace(theGraph, IC->py, IC->r) != OK)
             return NOTOK;
     }
     else
     {
         if (_MarkPathAlongBicompExtFace(theGraph, IC->r, IC->px) != OK ||
             _MarkPathAlongBicompExtFace(theGraph, IC->w, IC->r) != OK)
             return NOTOK;
     }

     if (theGraph->functions.fpMarkDFSPath(theGraph, MIN3(IC->ux, IC->uy, IC->uz),
                                    MAX3(IC->ux, IC->uy, IC->uz)) != OK ||
         _MarkDFSPathsToDescendants(theGraph) != OK ||
         _JoinBicomps(theGraph) != OK ||
         _AddAndMarkUnembeddedEdges(theGraph) != OK)
         return NOTOK;

     theGraph->IC.minorType |= MINORTYPE_E4;
     return OK;
}

void
MSPerson::MSPersonStage_Waiting::proceed(MSNet* net, MSTransportable* person, SUMOTime now, Stage* previous) {
    previous->getEdge()->addPerson(person);
    myWaitingStart = now;
    const SUMOTime until = MAX3(now, now + myWaitingDuration, myWaitingUntil);
    net->getPersonControl().setWaitEnd(until, person);
}

struct hsv_color rgb_to_hsv(struct rgb_color rgb) {
    struct hsv_color hsv;
    unsigned char rgb_min, rgb_max;
    rgb_min = MIN3(rgb.r, rgb.g, rgb.b);
    rgb_max = MAX3(rgb.r, rgb.g, rgb.b);
    hsv.val = rgb_max;
    if (hsv.val == 0) {
        hsv.hue = hsv.sat = 0;
        return hsv;
    }
    hsv.sat = 255*long(rgb_max - rgb_min)/hsv.val;
    if (hsv.sat == 0) {
        hsv.hue = 0;
        return hsv;
    }
    /* Compute hue */
    if (rgb_max == rgb.r) {
        hsv.hue = 0 + 43*(rgb.g - rgb.b)/(rgb_max - rgb_min);
    } else if (rgb_max == rgb.g) {
        hsv.hue = 85 + 43*(rgb.b - rgb.r)/(rgb_max - rgb_min);
    } else /* rgb_max == rgb.b */ {
        hsv.hue = 171 + 43*(rgb.r - rgb.g)/(rgb_max - rgb_min);
    }
    return hsv;
}

int  _IsolateMinorE4(graphP theGraph)
{
    isolatorContextP IC = &theGraph->IC;

    if (IC->px != IC->x)
    {
        if (_MarkPathAlongBicompExtFace(theGraph, IC->r, IC->w) != OK ||
                _MarkPathAlongBicompExtFace(theGraph, IC->py, IC->r) != OK)
            return NOTOK;
    }
    else
    {
        if (_MarkPathAlongBicompExtFace(theGraph, IC->r, IC->px) != OK ||
                _MarkPathAlongBicompExtFace(theGraph, IC->w, IC->r) != OK)
            return NOTOK;
    }

    // Note: The x-y path is already marked, due to identifying E as the type of non-planarity minor
    if (theGraph->functions.fpMarkDFSPath(theGraph, MIN3(IC->ux, IC->uy, IC->uz),
                                          MAX3(IC->ux, IC->uy, IC->uz)) != OK ||
            _MarkDFSPathsToDescendants(theGraph) != OK ||
            _JoinBicomps(theGraph) != OK ||
            _AddAndMarkUnembeddedEdges(theGraph) != OK)
        return NOTOK;

    theGraph->IC.minorType |= MINORTYPE_E4;
    return OK;
}

/* TODO This function is only used by the fade to random HSV. It might be possible to eliminate */
void rgb_to_hsv(uint8_t red, uint8_t green, uint8_t blue, uint8_t* hue, uint8_t* sat, uint8_t* val) {
    uint8_t rgb_min, rgb_max;
    rgb_min = MIN3(red, green, blue);
    rgb_max = MAX3(red, green, blue);

    *val = rgb_max;
    if (*val == 0) {
        *hue = *sat = 0;
        return;
    }

    *sat = 255*(rgb_max - rgb_min)/ *val;
    if (*sat == 0) {
        *hue = 0;
        return;
    }

    /* Compute hue */
    if (rgb_max == red) {
        *hue = 0 + 43*(green - blue)/(rgb_max - rgb_min);
    } else if (rgb_max == green) {
        *hue = 85 + 43*(blue - red)/(rgb_max - rgb_min);
    } else /* rgb_max == rgb.b */ {
        *hue = 171 + 43*(red - green)/(rgb_max - rgb_min);
    }
}

static hsv_color rgb_to_hsv(rgb_color rgb) {
  uint8_t rgb_min, rgb_max;
  hsv_color hsv;
  //integer find min and max RGB value
  rgb_min = MIN3(rgb.r, rgb.g, rgb.b);
  rgb_max = MAX3(rgb.r, rgb.g, rgb.b);    
  //compute value
  hsv.v = rgb_max;
  if (hsv.v == 0) {
    hsv.h = hsv.s = 0;
  } else {
    //integer compute saturation
    hsv.s = 255*((uint16_t)(rgb_max - rgb_min))/hsv.v;
    if (hsv.s == 0) {
      hsv.h = 0;
    } else {
      //integer compute hue
      if (rgb_max == rgb.r) {
        hsv.h = 0 + 43*(rgb.g - rgb.b)/(rgb_max - rgb_min);
      } else if (rgb_max == rgb.g) {
        hsv.h = 85 + 43*(rgb.b - rgb.r)/(rgb_max - rgb_min);
      } else /* rgb_max == rgb.b */ {
        hsv.h = 171 + 43*(rgb.r - rgb.g)/(rgb_max - rgb_min);
      }
    }
  }
  return hsv;
}

double OperaColors::RGB2HUE(double r, double g, double b) {
	double H;
	double m2 = MAX3(r, g, b);
	double m1 = CENTER(r, g, b);
	double m0 = MIN3(r, g, b);

	if (m2 == m1) {
		if (r == g) {
			H = 60;
			goto _RGB2HUE_END;
		}
		if (g == b) {
			H = 180;
			goto _RGB2HUE_END;
		}
		H = 60;
		goto _RGB2HUE_END;
	}
	double F = 60 * (m1 - m0) / (m2 - m0);
	if (r == m2) {
		H = 0 + F * (g - b);
		goto _RGB2HUE_END;
	}
	if (g == m2) {
		H = 120 + F * (b - r);
		goto _RGB2HUE_END;
	}
	H = 240 + F * (r - g);

_RGB2HUE_END:
	if (H < 0)
		H = H + 360;
	return H;
}

/* Find substrings matching .(expr), where expr is a balanced expression
 * (following R rules for matching [], (), {}, '', "", ``, %%, \, and comments.)
 * Returns a _list_ of _character arrays_ one array per each input string.
 * Each character array alternates between "outside quote" and "inside quote."
 */
SEXP _find_subst_expressions_list(SEXP strs, SEXP begin, SEXP end) {
  int ns, nb, ne;
  assert_type(strs, STRSXP);
  assert_type(begin, STRSXP);
  assert_type(end, STRSXP);
  ns = LENGTH(strs);
  nb = LENGTH(begin);
  ne = LENGTH(end);

  int nout;
  if (MIN3(ns, nb, ne) == 0) {
    nout = 0;
  } else {
    nout = MAX3(ns, nb, ne);
    if ((nout % ns > 0)||(nout % nb > 0)||(nout % ne > 0)) {
      warning("longer object length is not a multiple"
              " of shorter object length");
    }
  }

  SEXP out = PROTECT(allocVector(VECSXP, nout));
  if (ns == nout)
    setAttrib(out, R_NamesSymbol, getAttrib(strs, R_NamesSymbol));
  for (int i = 0; i < nout; i++) {
    SET_VECTOR_ELT(out, i,
                   find_subst_expressions(STRING_ELT(strs, i%ns),
                                          STRING_ELT(begin, i%nb),
                                          STRING_ELT(end, i%ne)));
  }

  UNPROTECT(1);
  return out;
}

void
MSPerson::MSPersonStage_Waiting::proceed(MSNet* net,
        MSPerson* person, SUMOTime now,
        const MSEdge & /*previousEdge*/) {
    const SUMOTime until = MAX3(now, now + myWaitingDuration, myWaitingUntil);
    net->getPersonControl().setArrival(until, person);
}