package hendrikschutter.com.externgnss;

import android.util.Log;

import java.io.BufferedReader;
import java.io.ByteArrayInputStream;
import java.io.ByteArrayOutputStream;
import java.io.IOException;
import java.io.InputStreamReader;
import java.nio.charset.StandardCharsets;
import java.util.Iterator;
import java.util.List;
import java.util.concurrent.BlockingQueue;

public class NMEAParser {
    private double latitude;
    private double longitude;
    private boolean fix;
    private int satCount;
    private int sentenceCounter;
    private BlockingQueue<Byte> rawByteData = null;

    public NMEAParser() {
        this.fix = false;
        this.satCount = 0;
        this.latitude = 0;
        this.longitude = 0;
        this.sentenceCounter = 0;
    }

    public void setReceiveByteStream(BlockingQueue<Byte> rawData) {
        this.rawByteData = rawData;
    }

    /**
     * find strings in raw data and sent them to parse
     */
    public void handleReceiveByteStream() {
        String nmeaSentence;
        ByteArrayOutputStream rawDataByteStream = new ByteArrayOutputStream();

        //wait for sufficient data in buffer TODO don't do this block wise, losing data in edge
        if (this.rawByteData.size() > 512) {
            for (Iterator<Byte> rawByte = this.rawByteData.iterator(); rawByte.hasNext(); ) {
                rawDataByteStream.write(rawByte.next());
                rawByte.remove(); //remove this data from stream
            }
            ByteArrayInputStream stream = new ByteArrayInputStream(rawDataByteStream.toByteArray());
            InputStreamReader streamReader = new InputStreamReader(stream, StandardCharsets.US_ASCII);
            BufferedReader bufferedReader = new BufferedReader(streamReader);

            try {
                while ((nmeaSentence = bufferedReader.readLine()) != null) {
                    //Log.i("NMEAParser", nmeaSentence);
                    this.parseSentence(nmeaSentence);
                }
            } catch (IOException e) {
                e.printStackTrace();
            }
        } else {
            //Log.i("NMEAParser", "wait for more data in buffer");
        }
    }

    public int getSentenceCounter(){
        return this.sentenceCounter;
    }

    public void parseSentence(String sentence) {
        this.sentenceCounter++;
        if (sentence.startsWith("$GNGLL")) {
            parseGNGLL(sentence);
        }
        if (sentence.startsWith("$GPGSV")) {
            parseGPGSV(sentence);
        }
    }

    private void parseGNGLL(String sentence) {
        if (sentence.length() >= 45) {
            if (sentence.substring(44, 45).equals("A")) {
                this.fix = true;
                this.latitude = (Double.parseDouble(sentence.substring(9, 17)) / 60) + Double.parseDouble(sentence.substring(7, 9));
                this.longitude = (Double.parseDouble(sentence.substring(23, 30)) / 60) + Double.parseDouble(sentence.substring(20, 23));
            } else {
                this.fix = false;
            }
        }
    }

    private void parseGPGSV(String sentence) {
        if (sentence.length() >= 13) {
            this.satCount = (Integer.parseInt(sentence.substring(11, 13)));
        }
    }


    public double calcDistance(double latitude, double longitude) {
        final int R = 6371; // Radius of the earth

        double latDistance = Math.toRadians(latitude - this.latitude);
        double lonDistance = Math.toRadians(longitude - this.longitude);
        double a = Math.sin(latDistance / 2) * Math.sin(latDistance / 2)
                + Math.cos(Math.toRadians(this.latitude)) * Math.cos(Math.toRadians(latitude))
                * Math.sin(lonDistance / 2) * Math.sin(lonDistance / 2);
        double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
        double distance = R * c * 1000; // convert to meters
        distance = Math.pow(distance, 2);
        return Math.sqrt(distance);
    }


    public double getLatitude() {
        return this.latitude;
    }

    public double getLongitude() {
        return this.longitude;
    }

    public boolean checkFix() {
        return this.fix;
    }

    public int getSatCount() {
        return this.satCount;
    }
}