/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package gps;

import CommonFunctions.debugPrinter;
import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.OutputStreamWriter;
import java.net.InetAddress;
import java.net.Socket;
import java.util.TimerTask;
import mdot.Main;
import structures.Constants;

/**
 *
 * @author baba
 */
public class GPSClass {

    protected Socket gpsSocket;
    protected BufferedReader gpsBR;
    protected BufferedWriter gpsBW;
    protected String nmeaSentence;
    protected boolean gpsOK;
    protected String gpstext;
    protected String gpsDate;
    protected String gpsLatitude;
    protected String gpsLongitude;
    protected String gpsAltitude;
    protected String gpsMode;
    protected String gpsSatCount;
    protected String gpsErrPos;
    protected String gpsErrHoriz;
    protected String gpsErrVert;
    protected String gpsFix;
    protected String gpsTrack;
    protected String gpsSpeed;
    protected String gpsOnline;
    public String gpsOrientation;
    protected java.util.Timer timer;

    /* class GPSClass
     * Polls the attached GPS device every x seconds and populates the
     * context with the latitude, longitude and heading it returns.
     */
    public GPSClass() {

        gpsSocket = null;
        gpsBR = null;
        gpsOK = false;
        nmeaSentence = "Unititialised";
        gpsDate = "";
        gpsLatitude = "";
        gpsLongitude = "";
        gpsAltitude = "";
        gpstext = "";
        gpsMode = "";
        gpsSatCount = "";
        gpsErrPos = "";
        gpsErrHoriz = "";
        gpsErrVert = "";
        gpsFix = "";
        gpsTrack = "";
        gpsSpeed = "";
        gpsOnline = "";


        //Proper connection to GPS. Start polling thread.
        if(connectGPS()){
        timer = new java.util.Timer();
        timer.schedule(new GPSTask(), 0, 1); // poll every 5

        }
        //Error connecting to GPS.
        else
        {
            debugPrinter.printLine("ERROR: GPS connection error.");

            //give default latitude and longitude
            Main.getMe().setObjectAsString(Constants.HASH_LATITUDE, "42.251896533");
            Main.getMe().setObjectAsString(Constants.HASH_LONGITUDE, "-85.641268281");
        }



    }


    protected boolean connectGPS() {

        if (gpsSocket == null) {
            try {
                gpsSocket = new Socket("127.0.0.1", 2947);
                gpsOK = true;
                nmeaSentence = "gpsd Initialised";
                gpsBR = new BufferedReader(new InputStreamReader(gpsSocket.getInputStream()));
                gpsBW = new BufferedWriter(new OutputStreamWriter(gpsSocket.getOutputStream()));
                gpsBW.write("w+r-\n");
                gpsBW.flush();
            } catch (Exception e) {
                gpsOK = false;
                nmeaSentence = "gpsd socket failure " + e;
                debugPrinter.printLine("ERROR: gpsd socket failure");

            }
        }

        debugPrinter.printLine("Connected to GPS: " + gpsOK + "\n");
        return gpsOK;
    }

    protected void pollGPS() {

        if (gpsBR == null) {
            return;
        }

        try {
            while (gpsBR.ready()) {
                nmeaSentence = gpsBR.readLine();
                gpsOK = true;
                gpsDecode();
            }
        } catch (IOException ioe) {
            nmeaSentence = "I/O Error " + ioe;
            gpsOK = false;
        }

    }

    protected void gpsDecode() {
        String[] toks;
        String[] subtoks;
        char ctl;
        int i;
        int n;

        if (nmeaSentence.startsWith("GPSD")) {
            toks = nmeaSentence.split(",");
            n = toks.length;
            for (i = 1; i < n; i++) {
                ctl = toks[i].charAt(0);
                if (ctl == 'A') {
                    // Altitude
                    gpsAltitude = (toks[i].substring(1)).trim();
                } else if (ctl == 'D') {
                    // date and time
                    gpsDate = (toks[i].substring(1)).trim();
                } else if (ctl == 'M') {
                    // mode
                    ctl = toks[i].charAt(2);
                    if (ctl == '0') {
                        gpsMode = "Unknown";
                    } else if (ctl == '1') {
                        gpsMode = "No fix";
                    } else if (ctl == '2') {
                        gpsMode = "2D";
                    } else if (ctl == '3') {
                        gpsMode = "3D";
                    }
                } else if (ctl == 'O') {
                    //GPSD,O=75 1255478190.000 0.005 42.277853064 -85.624092692 208.807 37.116 65.418 351.2577 0.038 0.031 ? 74.13 ? 3
                    //          ??              ??   lat            Lon         Alti     ??     ??     heading                       Distance
                    //      0   1               2    3            4             5       6       7       8       9    10    11 12   13   14

                    //GPSD,Y=75 1255478190.000 9:2 66 45 37 1:13 18 49 30 1:5 61 200 18 1:12 38 230 23 1:4 34 72 35 1:29 25 310 41 1:10 69 186 31 1:30 37 273 36 1:1 0 0 33 0:


                    // Position
                    toks[i] = (toks[i].substring(1)).trim();
                    subtoks = toks[i].split(" ");
                    if (subtoks.length <= 9) {
                        gpsLatitude = Constants.NOT_NUMBER;
                        gpsLongitude = Constants.NOT_NUMBER;
                        gpsOrientation = Constants.NOT_NUMBER;
                        if (Main.getMe().ContainsKey(Constants.HASH_LATITUDE)) {
                            Main.getMe().removeObject(Constants.HASH_LATITUDE);
                        }
                        if (Main.getMe().ContainsKey(Constants.HASH_LONGITUDE)) {
                            Main.getMe().removeObject(Constants.HASH_LONGITUDE);
                        }
                        if (Main.getMe().ContainsKey(Constants.HASH_HEADING)) {
                            Main.getMe().removeObject(Constants.HASH_HEADING);
                        }

                        try {
                            Main.treeView.UpdateTree();
                        } catch (Exception ex) {
                        }

                    } else {
                        gpsLatitude = subtoks[3];
                        gpsLongitude = subtoks[4];
                        String orientation = subtoks[8];
                        double doubleOrientation = Double.parseDouble(orientation);

                        //Change bearing degrees to 8 point heading
                        double approx = Math.round(doubleOrientation/45);

                        if (approx == 0) {
                            gpsOrientation = Constants.GPS_ORIENTATION_NORTH;
                        } else if (approx == 1) {
                            gpsOrientation = Constants.GPS_ORIENTATION_NORTHEAST;
                        } else if (approx == 2) {
                            gpsOrientation = Constants.GPS_ORIENTATION_EAST;
                        } else if (approx == 3) {
                            gpsOrientation = Constants.GPS_ORIENTATION_SOUTHEAST;
                        } else if (approx == 4) {
                            gpsOrientation = Constants.GPS_ORIENTATION_SOUTH;
                        } else if (approx == 5) {
                            gpsOrientation = Constants.GPS_ORIENTATION_SOUTHWEST;
                        } else if (approx == 6) {
                            gpsOrientation = Constants.GPS_ORIENTATION_WEST;
                        } else if (approx == 7) {
                            gpsOrientation = Constants.GPS_ORIENTATION_NORTHWEST;
                        } else if (approx == 8) {
                            gpsOrientation = Constants.GPS_ORIENTATION_NORTH;
                        }
                    }


                    //Replace Latitude, Longitude and heading in context.
                    if (Main.getMe().ContainsKey(Constants.HASH_LATITUDE)) {
                        Main.getMe().replaceObjectAsString(Constants.HASH_LATITUDE, gpsLatitude);
                    } else {
                        Main.getMe().setObjectAsString(Constants.HASH_LATITUDE, gpsLatitude);
                    }
                    if (Main.getMe().ContainsKey(Constants.HASH_LONGITUDE)) {
                        Main.getMe().replaceObjectAsString(Constants.HASH_LONGITUDE, gpsLongitude);
                    } else {
                        Main.getMe().setObjectAsString(Constants.HASH_LONGITUDE, gpsLongitude);
                    }
                    if (Main.getMe().ContainsKey(Constants.HASH_HEADING)) {
                        Main.getMe().replaceObjectAsString(Constants.HASH_HEADING, gpsOrientation);
                    } else {
                        if (gpsOrientation != null) {
                            Main.getMe().setObjectAsString(Constants.HASH_HEADING, gpsOrientation);
                        }
                    }

                    //Update the debug window
                    try {
                        Main.treeView.UpdateTree();
                    } catch (Exception ex) {
                    }
                
                } else if (ctl == 'Q') {
                    // Satellite count and position error
                    toks[i] = (toks[i].substring(1)).trim();
                    subtoks = toks[i].split(" ");
                    gpsSatCount = subtoks[0];
                    gpsErrPos = subtoks[1];
                    gpsErrHoriz = subtoks[2];
                    gpsErrVert = subtoks[3];
                } else if (ctl == 'S') {
                    // DGPS - rather academic for UK
                    ctl = toks[i].charAt(2);
                    if (ctl == '0') {
                        gpsFix = "No fix";
                    } else if (ctl == '1') {
                        gpsFix = "Fix";
                    } else if (ctl == '2') {
                        gpsFix = "DGPS fix";
                    }
                } else if (ctl == 'T') {
                    // Track
                    gpsTrack = (toks[i].substring(1)).trim();
                } else if (ctl == 'V') {
                    // Speed over Ground
                    gpsSpeed = (toks[i].substring(1)).trim();
                } else if (ctl == 'X') {
                    // GPS online
                    ctl = toks[i].charAt(2);
                    if (ctl == '0') {
                        gpsOnline = "Offline";
                    } else if (ctl == '1') {
                        gpsOnline = "Online";
                    }
                    debugPrinter.printLine("GPSD:  " + gpsOnline + "\n");
                } else if (ctl == 'Y') {
                   
                } else {
                    
                }
            }
        }
    }

    class GPSTask extends TimerTask {

        public void run() {
            pollGPS();
        }
    }
}
