picar/app/src/main/java/com/example/carcontroller/LIDAR/LidarView.java

package com.example.carcontroller.LIDAR;

import android.content.Context;
import android.content.SharedPreferences;
import android.graphics.Bitmap;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.util.AttributeSet;
import android.view.MotionEvent;
import android.view.SurfaceHolder;
import android.view.SurfaceView;

import androidx.preference.PreferenceManager;

import com.example.carcontroller.Empty;
import com.example.carcontroller.Int32Value;
import com.example.carcontroller.PersonTrackingGrpc;
import com.example.carcontroller.PointScan;
import com.example.carcontroller.Updaters.AbstractUpdater;
import com.example.carcontroller.Updaters.ZmqUpdater;

import java.util.stream.Collectors;

import io.grpc.ManagedChannel;
import io.grpc.ManagedChannelBuilder;
import io.grpc.stub.StreamObserver;

public class LidarView extends SurfaceView implements AbstractUpdater.MapChangedListener<PointScan> {

    private static final String LIDAR_TOPIC = "lidar_map";

    private AbstractUpdater<PointScan> lidar;
    private Thread lidarThread;
    private String port;
    private SurfaceHolder surfaceHolder;
    PersonTrackingGrpc.PersonTrackingStub stub;

    private int mBitmapX, mBitmapY, mViewWidth, mViewHeight;
    private Bitmap mBitmap;

    public LidarView(Context context) {
        super(context);
        init();
    }

    public LidarView(Context context, AttributeSet attrs) {
        super(context, attrs);
        init();
    }

    public LidarView(Context context, AttributeSet attrs, int defStyleAttr) {
        super(context, attrs, defStyleAttr);
        init();
    }

    private void init() {
        SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(getContext());
        String host = prefs.getString("host", "10.0.0.53");
        port = prefs.getString("zmqPort", "5050");
        String gRPCPort = prefs.getString("port", "50051");
        lidar = new ZmqUpdater<>(PointScan.getDefaultInstance().getParserForType(), LIDAR_TOPIC, host, port);
        lidar.addMapChangedListener(this);
        surfaceHolder = getHolder();
        lidarThread = new Thread(lidar);
        ManagedChannel channel = ManagedChannelBuilder.forAddress(host, Integer.parseInt(gRPCPort)).usePlaintext().build();
        stub = PersonTrackingGrpc.newStub(channel);
    }

    /**
     * Called by MainActivity.onResume() to start a thread.
     */
    public void resume() {
        StreamObserver<Empty> response = new StreamObserver<Empty>() {
            @Override
            public void onNext(Empty value) {
                lidarThread.start();
            }

            @Override
            public void onError(Throwable t) {
                // TODO: close the activity,
                System.out.println(t.getMessage());
            }

            @Override
            public void onCompleted() {
                // Don't care.
            }
        };
        // use async grpc method, ZMQ doesn't need to connect straight away.
        stub.startTracking(Int32Value.newBuilder().setValue(Integer.parseInt(port)).build(), response);
    }

    @Override
    protected void onSizeChanged(int w, int h, int oldw, int oldh) {
        super.onSizeChanged(w, h, oldw, oldh);
    }

    @Override
    public boolean onTouchEvent(MotionEvent event) {
        float x = event.getX();
        float y = event.getY();

        // TODO: Get the group that was selected and select on the grpc controller.
        switch (event.getAction()) {
            case MotionEvent.ACTION_DOWN:
                break;
        }
        return true;
    }


    public void stop() {
        // TODO: Use grpc to tell zmq to stop.
        lidar.stop();
        try {
            lidarThread.join(1000);
        } catch (InterruptedException e) {
        }
    }

    @Override
    public void mapChanged(PointScan points) {
        if (surfaceHolder.getSurface().isValid()) {
            Canvas canvas = surfaceHolder.lockCanvas();
            canvas.save();
            canvas.drawColor(Color.WHITE);
            for (Point point : points.getPointsList().stream().map(Point::fromProtoPoint).collect(Collectors.toList())) {
                // Now for each point, draw a circle for the point (so it's big enough) in the correct spot,
                // and create a colour for that point to paint it correctly.
                // TODO: Dynamically change the colour of the paint object based on the point group number.
                canvas.drawCircle((float) point.x, (float) point.y, 5, new Paint());
            }
            canvas.restore();
            surfaceHolder.unlockCanvasAndPost(canvas);
        }
    }

    private static class Point {
        private double x;
        private double y;

        private Point(double x, double y) {
            this.x = x;
            this.y = y;
        }

        static Point fromProtoPoint(com.example.carcontroller.Point point) {
            return fromHist(point.getDistance(), point.getAngle());
        }

        static Point fromHist(double distance, double angle) {
            return fromHist(distance, angle, new Point(0, 0));
        }

        static Point fromHist(double distance, double angle, Point offset) {
            return new Point(distance * Math.sin(angle) + offset.x, distance * Math.cos(angle) + offset.y);
        }

    }
}