/*@!Encoding:1252*/
////////////////////////////////////////////////////////////////////////////////
//
// Visualize Traffic Light using MAP and SPAT
//
////////////////////////////////////////////////////////////////////////////////

includes
{
  //#include "Car2xAPI.cin"
  #include "..\Vector_CN_ASNV\ASNV_Template_MAP.cin"
  #include "..\Vector_CN_ASNV\ASNV_Template_SPAT.cin"
}

variables
{
  struct TrafficLight
  {
    long intersectionId;  // id of the intersection we are interested in
    long signalGroupId;   // id of the intersection's signal group we are interested in
    long bitmapHdl;       // Handle for the TrafficLight grafic object drawn by CAPL-Map Drawing-API
    long rsuTextHandle;   // Handle for the RSU text grafic object drawn by CAPL-Map Drawing-API
  };
  struct TrafficLight tl =
  {
    1,    // intersection id
    3,    // signal group id
    0,    // traffic light bitmap handle
    0     // rsu text handle
  };

  msTimer tlWatchDog;
}

////////////////////////////////////////////////////////////////////////////////
// register rx callback
////////////////////////////////////////////////////////////////////////////////
on start
{
  C2xRegisterCallback(kRxIndication, "TlOnRxMAP" , "MAP" );
  C2xRegisterCallback(kRxIndication, "TlOnRxSPAT", "SPAT");
}

////////////////////////////////////////////////////////////////////////////////
// Handle MAP
////////////////////////////////////////////////////////////////////////////////
void TlOnRxMAP(LONG channel, LONG dir, LONG radioChannel, LONG signalStrength, LONG sigQuality, LONG packet)
{
  struct MAP  mapPdu;
  float latitude;
  float longitude;
  long iIntersection;
  long found;

  if (!API_GetMAPParams(packet, mapPdu))
  {
    tlWatchDog.set(1200);

    found = 0;
    if (mapPdu.map.intersections.isValidFlag)
    {
      for (iIntersection = 0 ; iIntersection < mapPdu.map.intersections.length ; ++iIntersection)
      {
        found = mapPdu.map.intersections.arrayValue[iIntersection].id.id == tl.intersectionId;
        if (found)
          break ;
      }
    }
    if (!found)
      return;

    latitude  = mapPdu.map.intersections.arrayValue[iIntersection].refPoint.lat / 1e7;
    longitude = mapPdu.map.intersections.arrayValue[iIntersection].refPoint.lon / 1e7;

    if (!tl.bitmapHdl)
    {
      tl.bitmapHdl = C2xCreateMapObject(kBitmap);
      C2xSetMapObjectBmpFilePath(tl.bitmapHdl, "bmp\\RSU_TrafficLight.png");
      C2xSetMapObjectBmpCount(tl.bitmapHdl, 6);
    }
    C2xSetMapObjectPosition(tl.bitmapHdl, latitude, longitude );
    C2xSetMapObjectHeading(tl.bitmapHdl, 90);
    C2xSetMapObjectSize(tl.bitmapHdl, 20, 53);
    C2xDrawMapObject(tl.bitmapHdl);
  }
}

////////////////////////////////////////////////////////////////////////////////
// Handle SPAT
////////////////////////////////////////////////////////////////////////////////
void TlOnRxSPAT(LONG channel, LONG dir, LONG radioChannel, LONG signalStrength, LONG sigQuality, LONG packet)
{
  struct SPAT spatPdu;
  float rsuLat, rsuLon;
  long state;
  long iIntersection;
  long iState;
  long found;

  if (!API_GetSPATParams(packet, spatPdu))
  {
    tlWatchDog.set(1200);

    if (!tl.bitmapHdl)
      return;

    // show rsu test
    rsuLat = C2xGetTokenInt(packet, "geo_eh", "lpvLatitude") / 1e7;
    rsuLon = C2xGetTokenInt(packet, "geo_eh", "lpvLongitude") / 1e7;
    rsuLat += 0.00008;
    rsuLon += 0.0002;
    if (tl.rsuTextHandle == 0)
      tl.rsuTextHandle = C2xCreateMapObject(kText);
    C2xSetMapObjectPosition(tl.rsuTextHandle, rsuLat, rsuLon );
    C2xSetMapObjectText(tl.rsuTextHandle, "RSU transmitting MAPs and SPATs");
    C2xSetMapObjectFillColor(tl.rsuTextHandle, makeRGB(0,0,0));
    C2xDrawMapObject(tl.rsuTextHandle);

    found = 0;
    for (iIntersection = 0 ; iIntersection < spatPdu.spat.intersections.length ; ++iIntersection)
    {
      found = spatPdu.spat.intersections.arrayValue[iIntersection].id.id == tl.intersectionId ;
      if (found)
        break ;
    }
    if (!found)
      return;

    found = 0;
    for (iState = 0 ; iState < spatPdu.spat.intersections.arrayValue[iIntersection].states.length ; ++iState)
    {
      found = spatPdu.spat.intersections.arrayValue[iIntersection].states.arrayValue[iState].signalGroup == tl.signalGroupId ;
      if (found)
        break ;
    }
    if (!found)
      return;

    switch (spatPdu.spat.intersections.arrayValue[iIntersection].states.arrayValue[iState].state_time_speed.arrayValue[0].eventState)
    {
    case 3: // stop and remain
      state = 2;
      break;
    case 4: // pre movement
      state = 3;
      break;
    case 5: // permissive movement allowed
    case 6: // protected movevemnt allowed
      state = 4;
      break;
    case 7: // permisive clearance
    case 8: // protected clearance
      state = 5;
      break;
    default:
      state = 1;
      break;
    }

    C2xSetMapObjectBmpIndex(tl.bitmapHdl, state);
    C2xSetMapObjectSize(tl.bitmapHdl, 20, 53);
    C2xDrawMapObject(tl.bitmapHdl);
  }
}

////////////////////////////////////////////////////////////////////////////////
// Delete RSU TL from Map Window
////////////////////////////////////////////////////////////////////////////////
on timer tlWatchDog
{
  C2xDeleteMapObject(tl.bitmapHdl);
  tl.bitmapHdl = 0;

  C2xDeleteMapObject(tl.rsuTextHandle);
  tl.rsuTextHandle = 0;
}

////////////////////////////////////////////////////////////////////////////////
// EOF
////////////////////////////////////////////////////////////////////////////////