package com.example.fitbot.util.processing;

import android.util.Log;

import com.example.fitbot.util.server.IWebSocketHandler;
import com.example.fitbot.util.server.WebSocket;

import org.jetbrains.annotations.NotNull;

import java.util.ArrayList;
import java.util.List;
import java.util.stream.Collectors;

public class MotionProcessor {

    public static final String DELIMITER = ";";

    private final List<MotionData> preprocessedData = new ArrayList<>(); // Preprocessed motion data
    private final List<Vector3> relativePath = new ArrayList<>(); // Relative path of the motion data
    private Vector3 ZERO = new Vector3(0, 0, 0);
    private double sampleRate = 1.0D; // samples/second
    private DataConsumer motionDataConsumer = (p1, p2, p3, p4) -> {};
    private GesturePath path;
    private WebSocket socket;

    public MotionProcessor() {}


    /**
     * Function for starting the listening process
     * of the motion sensor. This function will create
     * a new WebSocket server and start listening for
     * incoming connections.
     */
    public void startListening() {
        // Create socket server
        this.socket = WebSocket.createServer();

        Log.i("MotionProcessor", "Listening for incoming connections.");

        // Check if the socket
        if (socket != null) {
            // Update event handler to match our functionality.
            socket.setEventHandler(new IWebSocketHandler() {
                @Override
                public void onMessageReceived(WebSocket.Message message, WebSocket.MessageReply replier) {
                    parsePacket(message.message);
                }
            });
            socket.startListening();
        }
    }

    /**
     * Function for stopping the listening process
     * of the motion sensor. This function will stop
     * the WebSocket server.
     */
    public void stopListening() {
        if (socket != null) {
            socket.stop();
        }
    }

    /**
     * Function for parsing arbitrary packet data.
     * @param data The data to parse.
     */
    public void parsePacket(@NotNull String data) {
        // If the message starts with 'data', it's a data packet.
        if ( data.startsWith("data")) {
            Log.i("MotionProcessor", "Received data packet: " + data.split(" ")[1]);
            MotionData parsedData = MotionData.decode(data.split(" ")[1]);
            if (parsedData != null) {
                addMotionData(parsedData);
            }
            // Otherwise check if it starts with 'calibrate', this is the ZERO point.
        } else if ( data.startsWith("zero")) { // message to calibrate device
            String[] vectorData = data.split(" ")[1].split(DELIMITER);
            ZERO = new Vector3(
                    Float.parseFloat(vectorData[0]),
                    Float.parseFloat(vectorData[1]),
                    Float.parseFloat(vectorData[2])
            );
            Log.i("MotionProcessor", "Device calibrated at " + ZERO.toString());
        } else if ( data.startsWith("sampleRate")) {
            this.sampleRate = Double.parseDouble(data.split(" ")[1]);
        }
    }

    /**
     * Function for setting the gesture path of the processor.
     *
     * @param path The path to set.
     */
    public void setGesturePath(GesturePath path) {
        this.path = path;
    }

    /**
     * Function for adding motion data to the processor.
     *
     * @param data The motion data to add.
     */
    public void addMotionData(MotionData data) {
        preprocessedData.add(data);
        Vector3 previous = this.relativePath.isEmpty() ? ZERO : this.relativePath.get(this.relativePath.size() - 1);
        Vector3 relativeVector = getRelativeVector(data).add(previous);
        this.relativePath.add(relativeVector);
        motionDataConsumer.accept(relativeVector, data, this.relativePath.size(), this.sampleRate);
    }

    /**
     * Function for updating the relative path.
     *
     * @param relativePath The new relative path.
     */
    public void setRelativePath(List<Vector3> relativePath) {
        this.relativePath.clear();
        this.relativePath.addAll(relativePath);
    }

    /**
     * Function for setting the motion data receiver.
     * @param consumer The consumer to set.
     */
    public void setMotionDataEventHandler(DataConsumer consumer) {
        if ( consumer != null)
            this.motionDataConsumer = consumer;
    }

    /**
     * Function for getting the relative vector of the motion data.
     * This function will calculate the relative position of the motion data
     * based on its acceleration and rotation vectors. This has to be done since
     * the acceleration vector is relative to its own rotation vector.
     *
     * @param motionData The motion data to calculate the relative vector for.
     * @return The relative vector of the motion data.
     */
    public Vector3 getRelativeVector(MotionData motionData) {

        // Rotate the acceleration vector back by the rotation vector to make it
        // perpendicular to the gravity vector, then apply double integration to get the relative position.
        // s = 1/2 * a * t^2
        return motionData.acceleration
                .rotate(motionData.rotation.negate())
                .divide(2)
                .multiply(sampleRate * sampleRate);
    }

    /**
     * Function for getting the error offsets of the provided path and the
     * received motion data.
     *
     * @param referencePath The reference path to compare the motion data to.
     * @return A list of error offsets of the motion data compared to the reference path.
     */
    public List<Double> getErrors(GesturePath referencePath) {

        // Return the errors of the relative path compared to the reference path.
        return relativePath
                .stream()
                .map(referencePath::getError)
                .collect(Collectors.toList());
    }

    /**
     * Function for getting the error offsets of the motion data compared to the
     * reference path.
     *
     * @return A list of error offsets of the motion data compared to the reference path.
     *        If no path is set, an empty list will be returned.
     */
    public List<Double> getErrors() {
        if ( path == null)
            return new ArrayList<>();
        return getErrors(path);
    }

    /**
     * Function for getting the error of the motion data compared to the reference path.
     *
     * @param path The path to compare the motion data to.
     * @param referencePoint The reference point to compare the motion data to.
     * @return The error of the motion data compared to the reference path.
     */
    public double getError(GesturePath path, Vector3 referencePoint)
    {
        return path.getError(referencePoint);
    }

    /**
     * Function for getting the error of the provided vector and the set path.
     * If no path is set, the error will be 0.
     *
     * @param referencePoint The reference point to compare the path data to.
     * @return The error of the motion data compared to the reference path.
     */
    public double getError(Vector3 referencePoint) {
        if ( path == null)
            return 0;
        return path.getError(referencePoint);
    }

    /**
     * Function for calculating the average error of the motion data
     * compared to the reference path.
     *
     * @param referencePath The reference path to compare the motion data to.
     * @return The average error of the motion data compared to the reference path.
     */
    public double getAverageError(GesturePath referencePath) {
        return getErrors(referencePath)
                .stream()
                .mapToDouble(Double::doubleValue)
                .average()
                .orElse(0.0D);
    }

    /**
     * Function for calculating the average error of the motion data
     * compared to the reference path.
     *
     * @return The average error of the motion data compared to the reference path.
     */
    public double getAverageError() {
        if ( path == null)
            return 0;
        return getAverageError(path);
    }

    /**
     * Function for logging the statistics of the motion data.
     *
     * @param referencePath The reference path to compare the motion data to.
     */
    public void logStatistics(GesturePath referencePath) {
        Log.i("MotionProcessor", "Average path error: " + getAverageError(referencePath));
        Log.i("MotionProcessor", "Path length: " + relativePath.size());
        Log.i("MotionProcessor", "Sample rate: " + sampleRate);
        Log.i("MotionProcessor", "Calibration point: " + ZERO.toString());
    }

    /**
     * Interface that accepts motion data and the transformed vector.
     */
    public interface DataConsumer {

        /**
         * Function for accepting motion data and the transformed vector.
         * @param transformedVector The transformed vector.
         * @param motionData The input motion data.
         * @param sampleIndex The index of the current sample
         * @param sampleRate The sample rate.
         */
        void accept(Vector3 transformedVector, MotionData motionData, int sampleIndex, double sampleRate);
    }
}
