Full utility in C++ for converting Xanavi/Clarion/NissanLeaf map Mesh IDs (including encoded ones)

libbs.cpp

#include "libbs.h"
#include <stdint.h>
#include <cstdint>
#include <cstdio>
#include <sys/types.h>

/*
 * "MonsterID" is encoded Mesh ID composed for charging station updates, so far used only for CarWings CP updates
 */
int32_t UnpackMonsterIDIntoMeshID(int32_t monsterId)
{
  int32_t res = 0;
  res |= (monsterId & 0xf);  // bits 0-3
  res |= ((monsterId >> 4 & 0xf) << 16);  // y 4-7 -> res 16-19
  res |= ((monsterId >> 8 & 0x7) << 4);  // y 8-10 -> res 4-6
  res |= ((monsterId >> 11 & 1) << 20);  // y11 -> res20
  res |= ((monsterId >> 12 & 1) << 21);  // y12 -> res21
  res |= ((monsterId >> 13 & 1) << 22);  // y13 -> res22
  res |= ((monsterId >> 14 & 1) << 7);  // y14 -> res7
  res |= ((monsterId >> 15 & 0xff) << 8);  // y15-22 -> res8-15
  res |= ((monsterId >> 23 & 1) << 31);  // y23 -> res31
  res |= ((monsterId >> 24 & 0xff) << 23);  // y24-31 -> res23-30
  return res;
}

void UnpackMeshId(int32_t meshId,UnpackedMeshID *unpackedMeshId)
{
  if (unpackedMeshId != nullptr) {
    uint val1Ref = meshId >> 0x17 & 0x7f;
    unpackedMeshId->val1 = val1Ref;
    unpackedMeshId->val3 = meshId >> 0x14 & 7;
    unpackedMeshId->val5 = meshId >> 0x10 & 0xf;
    uint val2Ref = meshId >> 7 & 0x1ff;
    unpackedMeshId->val4 = meshId >> 4 & 7;
    unpackedMeshId->val6 = meshId & 0xf;
    unpackedMeshId->val2 = val2Ref;
    if ((meshId & 0x40000000) != 0) {
      unpackedMeshId->val1 = val1Ref | 0xffffff80;
    }
    if ((meshId & 0x80000000) != 0) {
      unpackedMeshId->val2 = val2Ref | 0xfffffe00;
    }
  }
}

int MeshID_DetermineLevel(int32_t param_1)
{
  int returningLevel;

  uint decodedLevelHex = (param_1 >> 0xc & 0xff ^ param_1 & 0xff) & 0xf ^ param_1 >> 0xc & 0xff;
  if (decodedLevelHex == 0xf2) {
    returningLevel = 2;
  }
  else if (decodedLevelHex == 0xf3) {
    returningLevel = 3;
  }
  else if (decodedLevelHex == 0xf4) {
    returningLevel = 4;
  }
  else if (decodedLevelHex == 0xf5) {
    returningLevel = 5;
  }
  else if (decodedLevelHex == 0xf6) {
    returningLevel = 6;
  }
  else {
    returningLevel = 1;
    if ((param_1 & 0xc000c) != 0) {
      returningLevel = 0x7fff;
    }
  }
  return returningLevel;
}

bool ValidateMeshID(int32_t meshId, short *output)
{
  UnpackedMeshID unpackedMeshId;

  if ((output != nullptr) && (meshId != 0)) {
    short judgeLvl = MeshID_DetermineLevel(meshId);
    short level = judgeLvl;
    *output = level;
    if (level != 0x7fff) {
      UnpackMeshId(meshId,&unpackedMeshId);
      if (level == 1) {
        if ((int)unpackedMeshId.val3 < 8 && (int)unpackedMeshId.val4 < 8 &&
            (int)unpackedMeshId.val5 < 4 && (int)unpackedMeshId.val6 < 4) {
          return true;
        }
      }
      else if (level == 2) {
        if ((int)unpackedMeshId.val3 < 8 && (int)unpackedMeshId.val4 < 8) {
          return true;
        }
      }
      else {
        uint meta;
        bool finalValidation;
        if (level == 3) {
          if (unpackedMeshId.val3 != 0 && unpackedMeshId.val3 != 1) {
            return false;
          }
          if (unpackedMeshId.val4 == 0) {
            return true;
          }
          finalValidation = unpackedMeshId.val4 == 1;
        }
        else if (level == 4) {
          meta = unpackedMeshId.val1 & 1;
          if ((int)unpackedMeshId.val1 < 0 && (meta != 0)) {
            meta = meta - 2;
          }
          if (meta != 0) {
            return false;
          }
          meta = unpackedMeshId.val2 & 1;
          if ((int)unpackedMeshId.val2 < 0 && meta != 0) {
            meta = meta - 2;
          }
          finalValidation = meta == 0;
        }
        else {
          if (level != 5) {
            if (level == 6) {
              return meshId == 0x1fe200f6;
            }
            return true;
          }
          meta = unpackedMeshId.val1 & 7;
          if ((int)unpackedMeshId.val1 < 0 && meta != 0) {
            meta = meta - 8;
          }
          uint valRef = -(uint) ((meta & 0x80000000) != 0);
          if ((meta ^ valRef) != valRef) {
            return false;
          }
          meta = unpackedMeshId.val2 & 7;
          if ((int)unpackedMeshId.val2 < 0 && meta != 0) {
            meta = meta - 8;
          }
          valRef = -(uint)((meta & 0x80000000) != 0);
          finalValidation = (meta ^ valRef) == valRef;
        }
        return finalValidation;
      }
    }
  }
  return false;
}

int ConvertToUTM(int32_t meshID,int confchar,int *out_x,int *out_y)
{
  double utm_y;
  double utm_x;
  short outLvl [2];
  UnpackedMeshID local_84;
  double meshScalingTable [4];

  meshScalingTable[0] = 0.0;
  meshScalingTable[1] = 32.0;
  meshScalingTable[2] = 8.0;
  meshScalingTable[3] = 2.0;
  outLvl[0] = 0;
  int *out_x_c = out_x;
  int *out_y_c = out_y;
  int meshIdReturn = ValidateMeshID(meshID, outLvl);
  int *out_y_c2 = out_y_c;
  int *out_x_c2 = out_x_c;
  if (((meshIdReturn == 0) || (out_x_c == nullptr)) || (out_y_c == nullptr)) {
    return 2;
  }
  UnpackMeshId(meshID,&local_84);
  meshIdReturn = (int)outLvl[0];
  double meshVal1 = (double) (long long) (int) local_84.val1;
  double meshVal2 = (double) (long long) (int) local_84.val2;

  printf("%f %f \n", meshVal1, meshVal2);

  if (meshIdReturn == 1) {
    utm_x = (double)(long long)(int)local_84.val5 * 0.03125 +
            (double)(long long)(int)local_84.val3 * 0.125;
    utm_y = (double)(long long)(int)local_84.val6 * 0.03125 +
            (double)(long long)(int)local_84.val4 * 0.125;
  }
  else if (meshIdReturn == 2) {
    utm_x = (double)(long long)(int)local_84.val3 * 0.125;
    utm_y = (double)(long long)(int)local_84.val4 * 0.125;
  }
  else {
    if (meshIdReturn != 3) {
      if ((meshIdReturn < 4) || (6 < meshIdReturn)) {
        return 1;
      }
      goto LAB_40fc2fc4;
    }
    utm_x = (double)(long long)(int)local_84.val3 * 0.5;
    utm_y = (double)(long long)(int)local_84.val4 * 0.5;
  }
  meshVal1 = utm_x + meshVal1;
  meshVal2 = utm_y + meshVal2;
LAB_40fc2fc4:
  if (confchar == 1) {
    utm_y = 0.5 / meshScalingTable[meshIdReturn];
    if ((int)local_84.val1 < 0) {
      meshVal1 = meshVal1 - utm_y;
    }
    else {
      meshVal1 = utm_y + meshVal1;
    }
    if ((int)local_84.val2 < 0) {
      meshVal2 = meshVal2 - utm_y;
    }
    else {
      meshVal2 = utm_y + meshVal2;
    }
  }
  // depending on map region these values need to be adjusted,
  // so far only values for JAPAN map needs to be adjusted,
  // setting as 0 for now.
  constexpr long regionalOffset1 = (long long)0;
  constexpr long regionalOffset2 = (long long)0;
  *out_x_c2 = (int)(long long)
                   ((meshVal1 - (double)regionalOffset1) * 0.6666666666666666 * 8388608.0);
  *out_y_c2 = (int)(long long)(((meshVal2 + 100.0) - (double)regionalOffset2) * 8388608.0);
  return 0;
}

int MeshPoint_isValid(MeshPoint *param_1, short *param_2)

{
  if (int iVar1; param_1 != nullptr && (iVar1 = ValidateMeshID(param_1->meshID,param_2), iVar1 != 0) &&
                 param_1->y < 0x800 &&
                 (-1 < param_1->y && param_1->x < 0x800 &&
                  -1 < param_1->x)) {
          return 1;
  }
  return 0;
}

int MeshPoint_To_MapPoint(MeshPoint *meshPoint,MapPoint *mapPoint)
{
  int result;
  short meshIdLevel [2];
  int x;
  int y;
  double scalingItems [4];

  meshIdLevel[0] = 0;
  scalingItems[0] = 128.0;
  scalingItems[1] = 32.0;
  scalingItems[2] = 8.0;
  scalingItems[3] = 2.0;
  int meshPntValid = MeshPoint_isValid(meshPoint, meshIdLevel);
  if ((meshPntValid == 0) || (mapPoint == nullptr)) {
    result = 2;
  }
  else {
    result = ConvertToUTM(meshPoint->meshID,0,&x,&y);
    double utmLat = (double) (long long) x * 1.1920928955078125e-07;
    double utmLon = (double) (long long) y * 1.1920928955078125e-07;

    if ((meshPoint->x != 0) || meshPoint->y != 0) {
      utmLon = ((double)(long long)(int)meshPoint->x * 0.00048828125) / scalingItems[meshIdLevel[0]] +
              utmLon;
      utmLat = ((double)(long long)(int)meshPoint->y * 0.00048828125) /
              scalingItems[meshIdLevel[0]] * 0.6666666666666666 + utmLat;
    }

    mapPoint->lat = (int)(long long)(utmLat * 3600.0 * 512.0);
    mapPoint->lon = (int)(long long)(utmLon * 3600.0 * 512.0);
  }
  return result;
}

libbs.h

#ifndef LIBBS_H
#define LIBBS_H
#include <sys/types.h>

struct UnpackedMeshID {
    uint val1;
    uint val2;
    uint val3;
    uint val4;
    uint val5;
    uint val6;
};

struct MapPoint {
    int32_t lat;
    int32_t lon;

};

struct MeshPoint {
    int32_t meshID;
    int16_t x;
    int16_t y;
};

int32_t UnpackMonsterIDIntoMeshID(int32_t monsterId);
void UnpackMeshId(int32_t meshId,UnpackedMeshID *unpackedMeshId);
bool ValidateMeshID(int32_t meshId, short *output);
int MeshID_DetermineLevel(int32_t param_1);
int ConvertToUTM(int32_t meshID,int confchar,int *out_x,int *out_y);
int MeshPoint_To_MapPoint(MeshPoint *meshPoint,MapPoint *mapPoint);

#endif //LIBBS_H

main.cpp

#include <fstream>
#include <iostream>

#include "libbs.h"

uint32_t readBigEndianUInt32(const unsigned char* bytes) {
    return (static_cast<uint32_t>(bytes[0]) << 24) |
           (static_cast<uint32_t>(bytes[1]) << 16) |
           (static_cast<uint32_t>(bytes[2]) << 8)  |
           static_cast<uint32_t>(bytes[3]);
}

int main() {
    FILE *payloadFile = fopen("./bergen.bin", "rb");


    std::ofstream csvFile("./bergen_output.csv");


    if (payloadFile == nullptr) {
        printf("Error opening file\n");
        return 1;
    }

    if (!csvFile.is_open()) {
        std::cerr << "Error opening CSV File! " << std::endl;
        return 2;
    }

    csvFile << "block,lat,lon\n";

    fseek(payloadFile, 0, SEEK_END);


    uint size = ftell(payloadFile);




    uint countOfIDs = size/4;

    printf("countOfIDs = %d\n", countOfIDs);

    fseek(payloadFile, 0, SEEK_SET);

    int counter = 0;
    unsigned char buffer[4];
    char * row = (char *) malloc(256);

    while (counter < countOfIDs) {
        fread(&buffer, 4, 1, payloadFile);
        uint32_t value = readBigEndianUInt32(buffer);
        printf("-- Value: %u 0x%08x\n", value, value);



        uint decodedVaal = UnpackMonsterIDIntoMeshID(value);

        printf("%u 0x%08x\n", decodedVaal, decodedVaal);

        UnpackedMeshID mesh_id{};

        uint level = MeshID_DetermineLevel(decodedVaal);

        printf("Level: %u\n", level);

        UnpackMeshId(decodedVaal, &mesh_id);

        printf("Unpacked: %u %u %u %u %u %u\n", mesh_id.val1, mesh_id.val2, mesh_id.val3, mesh_id.val4, mesh_id.val5, mesh_id.val6);

        int x;
        int y;
        int result = ConvertToUTM(decodedVaal, 0, &x, &y);
        printf("ConvertToUTM: %u %u %u\n", result, x, y);

        if (result != 0) {
            printf("Error converting to UTM\n");
            free(payloadFile);
            free(row);
            csvFile.close();
            return 3;
        }

        MeshPoint mesh_point{};
        mesh_point.meshID = decodedVaal;
        mesh_point.x = 0;
        mesh_point.y = 0;

        MapPoint map_point{};
        result = MeshPoint_To_MapPoint(&mesh_point, &map_point);
        printf("MeshPoint_To_MapPoint: %u %u %u \n", result, map_point.lat, map_point.lon);

        float lat = (map_point.lat/512.0)/3600.0;
        float lon = (map_point.lon/512.0)/3600.0;

        printf("Lat: %f\n", lat);
        printf("Lon: %f\n", lon);

        bzero(row, 256);

        sprintf(row, "%d,%f,%f\n", counter, lat, lon);

        csvFile << row;

        counter++;
    }
    free(row);
    fclose(payloadFile);
    csvFile.close();
    return 0;
}