import org.eclipse.paho.client.mqttv3.*;

public class lidar_PointCloudConcatenator {
    private static final String BROKER = "tcp://192.168.1.89:1883";
    private static final String[] TOPICS = {"lidar/scan", "lidar/car2/scan", "lidar/car3/scan"};
    private static final String PUB_TOPIC = "lidar/java";

    // Per-cycle storage (first message per topic)
    private static final Object LOCK = new Object();
    private static byte[][] latest = new byte[3][];

    public static void main(String[] args) {
        try {
            MqttClient client = new MqttClient(BROKER, MqttClient.generateClientId());
            client.connect();
            System.out.println("Connected to broker. Waiting for messages...");

            for (String topic : TOPICS) {
                client.subscribe(topic, (t, message) -> {
                    byte[] payload = message.getPayload();
                    System.out.println("Java VNF Received payload from " + t + ", size = " + payload.length + " bytes");

                    synchronized (LOCK) {
                        int idx = indexOfTopic(t);
                        if (idx >= 0 && latest[idx] == null) {
                            // only capture the first message for this topic in the current cycle
                            latest[idx] = payload;
                        }

                        if (allReceived()) {
                            byte[] combined = joinWithNewlines(latest);
                            // publish combined bytes
                            client.publish(PUB_TOPIC, new MqttMessage(combined));
                            System.out.println("combined " + combined.length + " bytes");
                            clear();
                        }
                    }
                });
                System.out.println("Subscribed to: " + topic);
            }

        } catch (MqttException e) {
            System.err.println("Error: " + e.getMessage());
        }
    }

    private static int indexOfTopic(String t) {
        for (int i = 0; i < TOPICS.length; i++) {
            if (TOPICS[i].equals(t)) return i;
        }
        return -1;
    }

    private static boolean allReceived() {
        for (byte[] b : latest) {
            if (b == null) return false;
        }
        return true;
    }

    private static void clear() {
        for (int i = 0; i < latest.length; i++) latest[i] = null;
    }

    // Join payloads as: part0 + '\n' + part1 + '\n' + part2
    // (Matches your byte-count examples that include 2 newline bytes.)
    private static byte[] joinWithNewlines(byte[][] parts) {
        int total = 0;
        for (byte[] p : parts) total += p.length;
        total += (parts.length - 1); // two '\n' separators

        byte[] out = new byte[total];
        int pos = 0;
        for (int i = 0; i < parts.length; i++) {
            System.arraycopy(parts[i], 0, out, pos, parts[i].length);
            pos += parts[i].length;
            if (i < parts.length - 1) out[pos++] = (byte) '\n';
        }
        return out;
    }
}

