public class BaseApp extends Application {
    public static Context applicationContext;
    private static QuadCopterXIOIO drone;


    public BaseApp() {
        super();

        FlyverMQ.start();
        SensorsList.create();
        OutputsList.create();
        drone = new QuadCopterXIOIO(34,35,36,37);
        try {
            new MainController(drone).start();
        } catch (MainControllerInstanceExisting mainControllerInstanceExisting) {
            mainControllerInstanceExisting.printStackTrace();
        }
        ServerRunner.run(NanoHTTPDServer.class);
    }

    @Override
    public void onCreate() {
        super.onCreate();
        applicationContext = getApplicationContext();
        new SensorsWrapper(applicationContext).start();
        WebpageUtils.deployWebpage(applicationContext.getResources().getAssets());
        new LocationServicesProvider();
        new AirQualitySensor("sensors.airquality", 45);
        new AirQualityMonitorProducer();
        new BatteryStatus("battery.status",46, BatteryStatus.BatteryCells.THREE);
    }
} public class PIDSettings {

    public enum PIDKeys {
        PID_YAW_PROPORTIONAL("pid_yaw_p"),
        PID_YAW_INTEGRAL("pid_yaw_i"),
        PID_YAW_DERIVATIVE("pid_yaw_d"),
        PID_PITCH_PROPORTIONAL("pid_pitch_p"),
        PID_PITCH_INTEGRAL("pid_pitch_i"),
        PID_PITCH_DERIVATIVE("pid_pitch_d"),
        PID_ROLL_PROPORTIONAL("pid_roll_p"),
        PID_ROLL_INTEGRAL("pid_roll_i"),
        PID_ROLL_DERIVATIVE("pid_roll_d");
        private String key;

        private PIDKeys(String key) {
            this.key = key;
        }

        public String getValue() {
            return key;
        }
    }

    public static void setPIDSettings() {

        /* Yaw PID Controller preferences */
        if(FlyverPreferences.getPreference(PIDKeys.PID_YAW_PROPORTIONAL.getValue()) == null) {
//            changePIDSharedPreference(PIDKeys.PID_YAW_PROPORTIONAL, "2.2", context);
            FlyverPreferences.setPreference(PIDKeys.PID_YAW_PROPORTIONAL.getValue(), "2.2");
        }
        if(FlyverPreferences.getPreference(PIDKeys.PID_YAW_INTEGRAL.getValue()) == null) {
            FlyverPreferences.setPreference(PIDKeys.PID_YAW_INTEGRAL.getValue(), "0");
        }
        if(FlyverPreferences.getPreference(PIDKeys.PID_YAW_DERIVATIVE.getValue()) == null) {
            FlyverPreferences.setPreference(PIDKeys.PID_YAW_DERIVATIVE.getValue(), "0.2");
        }
        /* End of Yaw PID Controller preferences */

        /* Pitch PID Controller preferences */

        if(FlyverPreferences.getPreference(PIDKeys.PID_PITCH_PROPORTIONAL.getValue()) == null) {
            FlyverPreferences.setPreference(PIDKeys.PID_PITCH_PROPORTIONAL.getValue(), "2.4");
        }

        if(FlyverPreferences.getPreference(PIDKeys.PID_PITCH_INTEGRAL.getValue()) == null) {
            FlyverPreferences.setPreference(PIDKeys.PID_PITCH_INTEGRAL.getValue(), "0.2");
        }

        if(FlyverPreferences.getPreference(PIDKeys.PID_PITCH_DERIVATIVE.getValue()) == null) {
            FlyverPreferences.setPreference(PIDKeys.PID_PITCH_DERIVATIVE.getValue(), "0.4");
        }

        /* End of Pitch PID controller preferences */

        /* Roll PID controller preferences */
        if(FlyverPreferences.getPreference(PIDKeys.PID_ROLL_PROPORTIONAL.getValue()) == null) {
            FlyverPreferences.setPreference(PIDKeys.PID_ROLL_PROPORTIONAL.getValue(), "2.4");
        }

        if(FlyverPreferences.getPreference(PIDKeys.PID_ROLL_INTEGRAL.getValue()) == null) {
            FlyverPreferences.setPreference(PIDKeys.PID_ROLL_INTEGRAL.getValue(), "0.2");
        }

        if(FlyverPreferences.getPreference(PIDKeys.PID_ROLL_DERIVATIVE.getValue()) == null) {
            FlyverPreferences.setPreference(PIDKeys.PID_ROLL_DERIVATIVE.getValue(), "0.4");
        }
        FlyverPreferences.commit();

        /* End of Roll PID controller preferences */

        float[][] pidValues = getPidValues();
        Server.sCurrentStatus.getPidYaw().setP(pidValues[0][0]);
        Server.sCurrentStatus.getPidYaw().setI(pidValues[0][1]);
        Server.sCurrentStatus.getPidYaw().setD(pidValues[0][2]);
        Server.sCurrentStatus.getPidPitch().setP(pidValues[1][0]);
        Server.sCurrentStatus.getPidPitch().setI(pidValues[1][1]);
        Server.sCurrentStatus.getPidPitch().setD(pidValues[1][2]);
        Server.sCurrentStatus.getPidRoll().setP(pidValues[2][0]);
        Server.sCurrentStatus.getPidRoll().setI(pidValues[2][1]);
        Server.sCurrentStatus.getPidRoll().setD(pidValues[2][2]);
    }

    /**
     * returns 3x3 array of PID values
     * [0][n] - Yaw values
     * [1][n] - Pitch values
     * [2][n] - Roll values
     * [n][0] - proportional values
     * [n][1] - integral values
     * [n][2] - derivative values
     * @return array of PID values
     */
    public static float[][] getPidValues() {
        String preference;
        float p;
        float i;
        float d;
        float[][] values = new float[3][3];

        preference = FlyverPreferences.getPreference(PIDSettings.PIDKeys.PID_YAW_INTEGRAL.getValue());
        p = Float.parseFloat(preference);
        preference = FlyverPreferences.getPreference(PIDSettings.PIDKeys.PID_YAW_INTEGRAL.getValue());
        i = Float.parseFloat(preference);
        preference = FlyverPreferences.getPreference(PIDSettings.PIDKeys.PID_YAW_DERIVATIVE.getValue());
        d = Float.parseFloat(preference);
        values[0][0] = p;
        values[0][1] = i;
        values[0][2] = d;

        preference = FlyverPreferences.getPreference(PIDSettings.PIDKeys.PID_PITCH_PROPORTIONAL.getValue());
        p = Float.parseFloat(preference);
        preference = FlyverPreferences.getPreference(PIDSettings.PIDKeys.PID_PITCH_INTEGRAL.getValue());
        i = Float.parseFloat(preference);
        preference = FlyverPreferences.getPreference(PIDSettings.PIDKeys.PID_PITCH_DERIVATIVE.getValue());
        d = Float.parseFloat(preference);
        values[1][0] = p;
        values[1][1] = i;
        values[1][2] = d;

        preference = FlyverPreferences.getPreference(PIDSettings.PIDKeys.PID_ROLL_PROPORTIONAL.getValue());
        p = Float.parseFloat(preference);
        preference = FlyverPreferences.getPreference(PIDSettings.PIDKeys.PID_ROLL_INTEGRAL.getValue());
        i = Float.parseFloat(preference);
        preference = FlyverPreferences.getPreference(PIDSettings.PIDKeys.PID_ROLL_DERIVATIVE.getValue());
        d = Float.parseFloat(preference);
        values[2][0] = p;
        values[2][1] = i;
        values[2][2] = d;

        return values;
    }

} public class SharedIPCKeys {
    public static final String COORDINATES = "coordinates";
    public static final String YAW = "yaw";
    public static final String THROTTLE = "throttle";
    public static final String INCREASE = "increase";
    public static final String DECREASE = "decrease";
    public static final String EMERGENCY = "emergency";
    public static final String PIDYAW = "pidy";
    public static final String PIDPITCH = "pidp";
    public static final String PIDROLL = "pidr";
    public static final String PROPORTIONAL = "proportional";
    public static final String INTEGRAL = "integral";
    public static final String DERIVATIVE = "derivative";
    public static final String PICTURE = "picture";
    public static final String PICREADY = "picready";
    public static final String HEARTBEAT = "heartbeat";
} public class Server extends IntentService {

    private static final String SERVER = "SERVER";
    private static final String EV_DEBUG = "DEBUG";
    private static final String EV_RAWDATA = "RAWDATA";

    public static Status sCurrentStatus = new Status();
    Gson mGson = new Gson();
    static BufferedReader mStreamFromClient;
    static PrintWriter mStreamToClient;
    Quadruple<String, Float, Float, Float> mJsonCoordinates = new Quadruple<>();
    Triple<String, String, Float> mJsonAction = new Triple<>();
    Quadruple<String, Float, Float, Float> mJsonPid = new Quadruple<>();
    Tuples.Tuple<String, String> mTuple = new Tuples.Tuple<>();
    static HashMap<String, ServerCallback> mCallbacks = new HashMap<>();
    IBinder mBinder = new LocalBinder();
    CameraProvider mCameraProvider;
    byte[] mPicture;
    ServerSocket mServerSocket;
    Socket mConnection;
    Timer mHeartbeat = new Timer();
    LoggerService logger;

    public Server() {
        super("Server");
    }

    public void setCameraProvider(CameraProvider cameraProvider) {
        this.mCameraProvider = cameraProvider;
    }

    @Override
    public IBinder onBind(Intent intent) {
        return mBinder;
    }

    public class LocalBinder extends Binder {
        public Server getServerInstance() {
            return Server.this;
        }
    }

    /**
     * Supply a runnable to be executed when a JSON with the appropriate key is received
     * Refer to the IPC/IPCKeys class for the valid keys
     * @param key - String, key associated with the runnable
     * @param callback - Runnable to be executed
     */
    public static void registerCallback(String key, ServerCallback callback) {
        if(callback == null) {
            Log.d(SERVER, "Runnable provided as callback is null");
            return;
        }
        mCallbacks.put(key, callback);
    }

    private void initLogger() {
        logger = new LoggerService(this.getApplicationContext());
        logger.Start();
        logger.LogData("EV_DEBUG", "Server", "Server initialized the Logger.");
    }

    @Override
    public int onStartCommand(Intent intent, int flags, int startId) {
        Log.d(SERVER, "OnStart executed");
        super.onStartCommand(intent, flags, startId);

        //initLogger();
        return START_NOT_STICKY;
    }

    @Override
    /**
     * Entry point of the server, starts a socket server, initializes the connection and starts an event loop
     * Provides a pictureReady callback to the CameraProvider
     */
    protected void onHandleIntent(Intent intent) {
        try {
            Log.d(SERVER, "Server Started");
            openSockets();
            initConnection(mConnection);
            loop();
        } catch (IOException e) {
            e.printStackTrace();
        }
    }

    /**
     * Opens a socket associated with the ServerSocket
     * @throws IOException
     */
    private void openSockets() throws IOException {
        if(mServerSocket != null && mServerSocket.isBound()) {
            mServerSocket.close();
        }
        mServerSocket = new ServerSocket(51342);
        mServerSocket.setSoTimeout(0);
        mServerSocket.setReuseAddress(true);
        mConnection = mServerSocket.accept();
        Log.d(SERVER, "Socket opened");
    }

    /**
     * Opens the input/output streams with a Socket
     * as a BufferedReader/PrintWriter
     * @param connection - Socket
     * @throws IOException
     */
    private void initConnection(Socket connection) throws IOException {
        Log.d(SERVER, "Streams opened");

        //logger.LogData(EV_DEBUG, SERVER, "Streams opened");

        mStreamFromClient = new BufferedReader(new InputStreamReader(connection.getInputStream()));
        mStreamToClient = new PrintWriter(connection.getOutputStream(), true);
        sCurrentStatus.setEmergency(new Tuples.Tuple<>("emergency", "stop"));
        //initialize a heartbeat to the client
        mHeartbeat.scheduleAtFixedRate(new TimerTask() {
            @Override
            public void run() {
                long currentTime = System.currentTimeMillis();
                Tuples.Tuple<String, String> tuple = new Tuples.Tuple<>(SharedIPCKeys.HEARTBEAT,
                        String.valueOf(currentTime / 1000));
                Type type = new TypeToken<Tuples.Tuple<String, String>>() {}.getType();
                String json = mGson.toJson(tuple, type);
                mStreamToClient.println(json);
                mStreamToClient.flush();
            }
        }, 1, 1000);
        onClientConnected();
    }

    /**
     * Deserializes a JSON string in a JSONCoordinates/JsonAction class, depending on the key of the JSON
     * Checks if the key has a callback associated with it, and runs it.
     * @param json - String describing a JSON object
     */
    private String deserialize(String json) {

        if(json.isEmpty()) {
            Log.w(SERVER, "JSON is empty");
            return null;
        }
        String mKey;
        Log.d(SERVER, json);
        //returns the next string encountered after "key" in the JSON
        Pattern mPattern = Pattern.compile("\\{.+(?=key)\\w+\":(\"\\w+\")\\}?.+\\}?$");
        Matcher matcher = mPattern.matcher(json);
        if(JSONUtils.validateJson(json) && matcher.matches()) {
            mKey = matcher.group(1);
        } else {
            Log.e(SERVER, "Invalid JSON!");
            mStreamToClient.println("Invalid JSON!");
            mStreamToClient.flush();
            return null;
        }

        mKey = mKey.replace('"', ' ').trim();
        switch (mKey) {
            case SharedIPCKeys.COORDINATES: {
                //TypeToken must be passed to the fromJson method to avoid type erasure problems
                Type type = new TypeToken<Quadruple<String, Float, Float, Float>>() {}.getType();
                mJsonCoordinates = JSONUtils.deserialize(json, type);
                sCurrentStatus.setAzimuth(mJsonCoordinates.getValue1());
                sCurrentStatus.setPitch(mJsonCoordinates.getValue2());
                sCurrentStatus.setRoll(mJsonCoordinates.getValue3());
                fireCallbacks(mKey, json);
            }
            break;
            case SharedIPCKeys.YAW: {
                Type type = new TypeToken<Triple<String, String, Float>>() {}.getType();
                mJsonAction = JSONUtils.deserialize(json, type);
                sCurrentStatus.setYaw(mJsonAction);
                fireCallbacks(mKey, json);
            }
            break;
            case SharedIPCKeys.THROTTLE: {
                Type type = new TypeToken<Triple<String, String, Float>>() {}.getType();
                mJsonAction = JSONUtils.deserialize(json, type);
                sCurrentStatus.setThrottle(mJsonAction);
                fireCallbacks(mKey, json);
            }
            break;
            case SharedIPCKeys.EMERGENCY: {
                Type type = new TypeToken<Tuples.Tuple<String, String>>() {}.getType();
                mTuple = JSONUtils.deserialize(json, type);
                sCurrentStatus.setEmergency(mTuple);
                fireCallbacks(mKey, json);
                mStreamToClient.println("Emergency " + mTuple.value);
                mStreamToClient.flush();
            }
            break;
            case SharedIPCKeys.PIDYAW: {
                Type type = new TypeToken<Quadruple<String, Float, Float, Float>>() {}.getType();
                mJsonPid = JSONUtils.deserialize(json, type);
                sCurrentStatus.mPidYaw.setP(mJsonPid.getValue1());
                sCurrentStatus.mPidYaw.setI(mJsonPid.getValue2());
                sCurrentStatus.mPidYaw.setD(mJsonPid.getValue3());
                fireCallbacks(mKey, json);
            }
            break;
            case SharedIPCKeys.PIDPITCH: {
                Type type = new TypeToken<Quadruple<String, Float, Float, Float>>() {}.getType();
                mJsonPid = JSONUtils.deserialize(json, type);
                sCurrentStatus.mPidPitch.setP(mJsonPid.getValue1());
                sCurrentStatus.mPidPitch.setI(mJsonPid.getValue2());
                sCurrentStatus.mPidPitch.setD(mJsonPid.getValue3());
                fireCallbacks(mKey, json);
            }
            break;
            case SharedIPCKeys.PIDROLL: {
                Type type = new TypeToken<Quadruple<String, Float, Float, Float>>() {}.getType();
                mJsonPid = JSONUtils.deserialize(json, type);
                sCurrentStatus.mPidRoll.setP(mJsonPid.getValue1());
                sCurrentStatus.mPidRoll.setI(mJsonPid.getValue2());
                sCurrentStatus.mPidRoll.setD(mJsonPid.getValue3());
                fireCallbacks(mKey, json);
            }
            break;
            case SharedIPCKeys.PICTURE: {
                mCameraProvider.snapIt();
                fireCallbacks(mKey, json);
            }
            break;
            default: {
                if(mCallbacks.containsKey(mKey)) {
                    fireCallbacks(mKey, json);
                } else {
                    mStreamToClient.println("Unknown key " + mKey);
                    mStreamToClient.flush();
                }
            }
            break;
        }
        return mKey;
    }

    /**
     * Waits for input on the socket, and returns it as a string
     * If the string representing the JSON object is null
     * Resets the socketServer and waits for a new connection
     * Also resets the heartbeat
     * @return - String in JSON notation, describing a command
     * @throws java.io.IOException
     */
    private String readStream() throws IOException {
        String mJson;
        if((mJson = mStreamFromClient.readLine()) == null) {
            mHeartbeat.cancel();
            mHeartbeat.purge();
            mHeartbeat = new Timer();
            sCurrentStatus.setEmergency(new Tuples.Tuple<>("emergency", "start"));
            mConnection = mServerSocket.accept();
            initConnection(mConnection);
            loop();
        }
        return mJson;
    }

    /**
     * Camera provider callback, called every time a new picture is ready
     * Sends a JSON with picture metadata and base64 encoded byte array of the picture
     */
    private void newPictureReady() {
        Log.d(SERVER, "New picture is ready");
        mPicture = mCameraProvider.getLastPicture();
        String base64Pic = Base64.encodeToString(mPicture, Base64.DEFAULT);
        Triple<String, String, String> mJsonBitmap = new Triple<>(SharedIPCKeys.PICTURE, SharedIPCKeys.PICREADY, base64Pic);
        String mJson = JSONUtils.serialize(mJsonBitmap, new TypeToken<Triple<String, String, String>>() {}.getType());
        mStreamToClient.println(mJson);
        Log.d(SERVER, mJson);
        mStreamToClient.flush();
    }

    private void fireCallbacks(String key, String json) {
        if(mCallbacks.containsKey(key)) {
            mCallbacks.get(key).run(json);
        } else {
            Log.w(SERVER, "Key: " + key + " has no associated callback");
        }
    }

    /**
     * Used to send custom messages to the client app
     * Intended for usage with custom callbacks.
     * @param msg - String
     * @return - true, if successful, false otherwise
     */
    public static boolean sendMsgToClient(String msg) {
        if(msg != null && !msg.isEmpty() && mStreamToClient != null) {
            Log.d(SERVER, "Sent to client: " + msg);
            mStreamToClient.println(msg);
            mStreamToClient.flush();
            return true;
        } else {
            return false;
        }
    }

    private void onClientConnected() {

        String json;
        Type type = new TypeToken<Quadruple<String, Float, Float, Float>>() {}.getType();

        mJsonPid = new Quadruple<>("pidyaw", sCurrentStatus.getPidYaw().getP(), sCurrentStatus.getPidYaw().getI(), sCurrentStatus.getPidYaw().getD());
        json = mGson.toJson(mJsonPid, type);
        mStreamToClient.println(json);
        mStreamToClient.flush();

        mJsonPid = new Quadruple<>("pidpitch", sCurrentStatus.getPidPitch().getP(), sCurrentStatus.getPidPitch().getI(), sCurrentStatus.getPidPitch().getD());
        json = mGson.toJson(mJsonPid, type);
        mStreamToClient.println(json);
        mStreamToClient.flush();

        mJsonPid = new Quadruple<>("pidroll", sCurrentStatus.getPidRoll().getP(), sCurrentStatus.getPidRoll().getI(), sCurrentStatus.getPidRoll().getD());
        json = mGson.toJson(mJsonPid, type);
        mStreamToClient.println(json);
        mStreamToClient.flush();
    }

    /**
     * Main worker, deserializes the JSON data and notifies the registered components for changes
     * @throws java.io.IOException
     */
    private void loop() throws IOException {
        while(true) {
            Log.d(SERVER, "Waiting for input");
            String mJson = readStream();
            String mKey;
            if(mJson == null) {
                Log.w(SERVER, "JSON is null");
                break;
            }
            mKey = deserialize(mJson);
//            fireCallbacks(mKey, mJson);
        }
    }
} public class Status {

    private float MAX_THROTTLE = 1023;
    private float MAX_YAW = 180;
    private float MIN_YAW = -180;

    float mAzimuth = 0;
    float mPitch = 0;
    float mRoll = 0;
    float mYaw = 0;
    float mThrottle = 0;
    boolean mEmergency = false;
    PID mPidYaw = new PID();
    PID mPidPitch = new PID();
    PID mPidRoll = new PID();

    Status() {
        //empty on purpose
    }

    public PID getPidRoll() {
        return mPidRoll;
    }

    public PID getPidPitch() {
        return mPidPitch;
    }

    public PID getPidYaw() {
        return mPidYaw;
    }

    public class PID {
        float mP;
        float mI;
        float mD;

        public float getP() {
            return mP;
        }

        public void setP(float mP) {
            this.mP = mP;
        }

        public float getI() {
            return mI;
        }

        public void setI(float mI) {
            this.mI = mI;
        }

        public float getD() {
            return mD;
        }

        public void setD(float mD) {
            this.mD = mD;
        }

        private PID() {
            //empty constructor
        }

        private PID(float p, float i, float d) {
            this.mP = p;
            this.mI = i;
            this.mD = d;
        }

    }

    public boolean isEmergency() {
        return mEmergency;
    }

    public void setEmergency(Tuples.Tuple JSONAction) {
        if(JSONAction.value.equals("stop")) {
            this.mEmergency = false;
        } else if (JSONAction.value.equals("start")) {
            this.mRoll = 0;
            this.mPitch = 0;
            this.mThrottle = 0;
            this.mYaw = 0;
            this.mEmergency = true;
        }
    }

    public float getAzimuth() {
        return mAzimuth;
    }

    public void setAzimuth(float mAzimuth) {
        this.mAzimuth = mAzimuth;
    }

    public float getPitch() {
        return mPitch;
    }

    public void setPitch(float mPitch) {
        if(mEmergency) {
            return;
        }
        this.mPitch = mPitch / 5;
    }

    public float getRoll() {
        return mRoll;
    }

    public void setRoll(float mRoll) {
        if(mEmergency) {
            return;
        }
        //TODO: division by five for testing purposes
        this.mRoll = (mRoll * (-1)) / 5;
    }

    public float getYaw() {
        return mYaw;
    }

    /**
     * Changes the current yaw based on steps
     * If the yaw exceeds 180 deg, it overflows to -180 deg or lower, and vice versa
     * @param triple - JSONTriple<String, String, Float>
     */
    public void setYaw(Tuples.Triple<String, String, Float> triple) {
        if(mEmergency) {
            return;
        }
        float newYaw = this.mYaw + (MAX_YAW * (triple.getValue2() / 100));
        if(newYaw > MAX_YAW) {
            newYaw *= (-1);
            if(newYaw < MIN_YAW) {
                newYaw = MIN_YAW;
            }
            this.mYaw = 0;
        }
        if(triple.value1.equals(SharedIPCKeys.INCREASE)) {
            this.mYaw = newYaw;
        } else if(triple.value1.equals(SharedIPCKeys.DECREASE)) {
            if(newYaw < MIN_YAW) {
                newYaw *= 1;
                if(newYaw > MAX_YAW) {
                    newYaw = MAX_YAW;
                }
            }
            this.mYaw = newYaw;
        }
    }

    public float getThrottle() {
        return mThrottle;
    }

    /**
     * Changes the throttle based on steps
     * Steps can vary between 0 and 100, percentage based
     * Floating point steps are allowed
     * @param triple - JSONTriple<String, String, Float>
     */
    public void setThrottle(Tuples.Triple<String, String, Float> triple) {
        if(mEmergency) {
            return;
        }
        float newThrottle = mThrottle;
        if(triple.value1.equals(SharedIPCKeys.INCREASE)) {
            newThrottle += MAX_THROTTLE * (triple.getValue2() / 100);
            if(newThrottle > MAX_THROTTLE) {
                this.mThrottle = MAX_THROTTLE;
            } else {
                this.mThrottle = newThrottle;
            }
        } else if(triple.value1.equals(SharedIPCKeys.DECREASE)) {
            newThrottle -= MAX_THROTTLE * (triple.getValue2() / 100);
            if(newThrottle < 0) {
                this.mThrottle = 0;
            } else {
                this.mThrottle = newThrottle;
            }
        }
    }
} public interface ServerCallback {
    public void run(String json);
} public class LoggerService implements Runnable, FlyverMQConsumer {
    protected static final String STOP_COMMAND = "stop";
    protected static final String THREAD_NAME = "LoggerThread";

    protected static final int THREAD_COUNT = 3;

    protected int iThreadNumber = 0;

    protected FileLogger m_flLogger;
    protected String m_sFilename = null;
    protected BlockingQueue m_bqQueue;
    protected BlockingQueue m_bqFileQueue;
    protected Thread m_thThreads[];
    protected Thread m_thFileThread;

    //protected HttpClient m_hcClient;
    protected String eventURL, eventMethod, eventToken;

    protected FlyverMQ m_mqQueue;

    public LoggerService(Context context, FlyverMQ queue) {
        this(context);

        m_mqQueue = queue;
    }

    public LoggerService(Context context) {
        m_bqQueue = new LinkedBlockingQueue(100);
        m_bqFileQueue = new LinkedBlockingQueue(100);
        InitProperties(context);

        m_mqQueue = null;

        m_thFileThread = new Thread(new Runnable() {
            @Override
            public void run() {
                runFileThread();
            }
        });

        m_thFileThread.start();
    }

    public void run() {
        int myThreadNumber = iThreadNumber++;

        HttpClient hcClient = CreateKeepAliveHttpClient();

        try {
            Log.i("LoggerService", "Thread " + myThreadNumber + " started.");

            while (true) {
                //Log.d("LoggerService", "" + myThreadNumber + ": Waiting for an item from the queue.");
                Object obj = m_bqQueue.take();

                if (obj.getClass() == SimpleEvent.class) {
                    //Log.d("LoggerService", "" + myThreadNumber + ": Processing a SimpleEvent.");
                    HandleEventLog(hcClient, (SimpleEvent) obj);
                }
                else if (obj.getClass() == CommandEvent.class) {
                    Log.d("LoggerService", "" + myThreadNumber + ": Processing a CommandEvent.");
                    if (((CommandEvent) obj).EventData.equals(STOP_COMMAND)) {
                        throw new InterruptedException();
                    }
                else
                    Log.d("LoggerService", "" + myThreadNumber + ": Unknown Object received: " + obj.toString());
                }
            }
        } catch(InterruptedException ie) {
            Log.w("LoggerService", "" + myThreadNumber + ": INTERRUPTED!");
        } catch(Exception ex) {
            Log.w("LoggerService", "" + myThreadNumber + ": Generic ERROR: " + ex.toString());
            ex.printStackTrace();
        } finally {
            if (hcClient != null) {
                hcClient = null;
            }
        }
    }

    public void runFileThread() {
        try {
            Log.i("LoggerService", "Thread FileThread started.");

            while (true) {
                //Log.d("LoggerService", "" + myThreadNumber + ": Waiting for an item from the queue.");
                Object obj = m_bqFileQueue.take();

                if (obj.getClass() == SimpleEvent.class) {
                    //Log.d("LoggerService", "FileThread: Processing a SimpleEvent.");
                    m_flLogger.WriteLogEntry((SimpleEvent) obj);
                }
                else if (obj.getClass() == CommandEvent.class) {
                    Log.d("LoggerService", "FileThread: Processing a CommandEvent.");
                    if (((CommandEvent) obj).EventData.equals(STOP_COMMAND)) {
                        throw new InterruptedException();
                    }
                    else
                        Log.d("LoggerService", "FileThread: Unknown Object received: " + obj.toString());
                }
            }
        } catch(InterruptedException ie) {
            Log.w("LoggerService", "FileThread: INTERRUPTED!");
        } catch(Exception ex) {
            Log.w("LoggerService", "FileThread: Generic ERROR: " + ex.toString());
            ex.printStackTrace();
        } finally {
        }
    }

    public boolean Start() {
        m_thThreads = new Thread[3];

        for (int i = 0; i < LoggerService.THREAD_COUNT; i++)
        {
            m_thThreads[i] = new Thread(this, THREAD_NAME + i);
            m_thThreads[i].start();
        }

        if (m_mqQueue != null)
            m_mqQueue.registerConsumer(this, "sensors.raw");

        return true;
    }

    public boolean Stop() {
        return this.Stop(false);
    }

    public boolean Stop(boolean bImmediately) {
        if (m_thThreads == null)
            return true;

        for (int i = 0; i < LoggerService.THREAD_COUNT; i++) {
            if (m_thThreads[i].getState() == Thread.State.TERMINATED) {
                continue;
            }

            if (bImmediately) {
                m_thThreads[i].interrupt();
                continue;
            }

            if (m_thThreads[i].getState() == Thread.State.RUNNABLE && m_bqQueue != null) {
                m_bqQueue.add(CommandEvent.CreateStopCommand());
            }
        }

        if (m_thFileThread.getState() == Thread.State.RUNNABLE) {
            if (bImmediately)
                m_thFileThread.interrupt();
            else if (m_bqFileQueue != null)
                m_bqFileQueue.add(CommandEvent.CreateStopCommand());
        }

        m_thFileThread = null;
        m_thThreads = null;

        return true;
    }

    public boolean LocalReadFromStart()
    {
        return m_flLogger.ReadFromStart();
    }

    public SimpleEvent LocalReadLogEntry()
    {
        return m_flLogger.ReadLogEntry();
    }

    public boolean LocalGoToEnd()
    {
        return m_flLogger.GoToEnd();
    }

    protected void InitProperties(Context context) {
        if (m_sFilename != null)
            return;

        try
        {
            SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(context);

            if (sharedPref.getString(IntCfg.internal_storage_path, null) == null) {
                SharedPreferences.Editor edit = sharedPref.edit();
                edit.putString(IntCfg.internal_storage_path, context.getFilesDir().getAbsolutePath());
                edit.apply();
            }

            Log.i("LoggerService", "Internal Storage Path: " + sharedPref.getString(IntCfg.internal_storage_path, null));

            boolean bInternalStorage = new Boolean(sharedPref.getString(IntCfg.save_internally, "true")).booleanValue();
            if (!isExternalStorageWritable() && !bInternalStorage) {
                bInternalStorage = true;
            }

            if (bInternalStorage)
                m_sFilename = sharedPref.getString(IntCfg.internal_storage_path, null) + File.separator + sharedPref.getString(IntCfg.local_filename, null);
            else
                m_sFilename = Environment.getExternalStoragePublicDirectory(Environment.DIRECTORY_DOWNLOADS) + File.separator + sharedPref.getString(IntCfg.local_filename, null);

            Log.i("LoggerService", "Filename: " + m_sFilename);

            m_flLogger = new FileLogger(m_sFilename);

            eventURL = sharedPref.getString(IntCfg.KEY_LOG_URL, "").toLowerCase();
            eventToken = sharedPref.getString(IntCfg.KEY_ACCESS_TOKEN, "");
            eventMethod = sharedPref.getString(IntCfg.KEY_LOG_METHOD, "").toLowerCase();

            Log.i("LoggerService", "eventURL: " + eventURL + ", eventToken: " + eventToken + ", eventMethod: " + eventMethod);
        } catch(Exception ex) {
            Log.w("LoggerService", "ERROR!");
            ex.printStackTrace();
        }
    }

    protected void InitHttpClient() {
        //m_hcClient = CreateKeepAliveHttpClient();
    }

    protected HttpClient CreateKeepAliveHttpClient() {
        HttpClient hcClient = new DefaultHttpClient();
        ClientConnectionManager mgr = hcClient.getConnectionManager();
        HttpParams hp = hcClient.getParams();
        HttpConnectionParams.setConnectionTimeout(hp, 7500);
        HttpConnectionParams.setSoTimeout(hp, 7500);
        hcClient = new DefaultHttpClient(
                new ThreadSafeClientConnManager(hp,mgr.getSchemeRegistry()), hp);

        return hcClient;
    }

    protected String GetResponseText(HttpResponse response) {
        BufferedReader in = null;
        StringBuffer sb = null;

        try {
            in = new BufferedReader
                    (new InputStreamReader(response.getEntity().getContent()));
            sb = new StringBuffer("");
            String line = "";
            String NL = System.getProperty("line.separator");
            while ((line = in.readLine()) != null) {
                sb.append(line + NL);
            }
            //in.close();

            // TODO: handle any errors gracefully, storing the data in a file somewhere
        } catch (Exception ex) {
            Log.w("LoggerService", ex.getMessage());
            ex.printStackTrace();
        } finally {
            if (in != null) {
                try {
                    in.close();
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }
        }

        if (sb != null)
            return sb.toString();

        return null;
    }

    protected boolean isExternalStorageWritable() {
        String state = Environment.getExternalStorageState();
        if (Environment.MEDIA_MOUNTED.equals(state)) {
            return true;
        }
        return false;
    }

    public void Dispose() {
        if (m_flLogger != null) {
            m_flLogger.Dispose();
            m_flLogger = null;
        }

        if (m_mqQueue != null) {
            //TODO: need to implement unregisterConsumer
            //m_mqQueue.unregisterConsumer(this, "sensors.raw");
        }

        //if (m_hcClient != null) {
        //    m_hcClient = null;
        //}

        this.Stop();
    }

    public void LogData(SimpleEvent sEvent) {
        //Log.d("LoggerService", "LogData: " + sEvent.getEventType());

        try{
            if (m_bqQueue.remainingCapacity() > 0)
                m_bqQueue.add(sEvent);
            else
                Log.d("LoggerService", "Queue FULL, Event DROPPED!");

            if (m_bqFileQueue.remainingCapacity() > 0)
                m_bqFileQueue.add(sEvent);
            else
                Log.d("LoggerService", "FILE Queue FULL, Event DROPPED!");
        } catch (Exception ex) {
            Log.d("LoggerService", "LogData error: " + ex.toString());
        } finally {

        }
    }

    public void LogData(String eType, String eTags, String eData) {
        LogData(new SimpleEvent(eType, eTags, eData));
    }

    protected void HandleEventLog(HttpClient hcClient, SimpleEvent se) {
        //Log.i("LoggerService", "HandleEventLog: eURL: " + eventURL + ", eMethod: " + eventMethod + ", eventType: " + se.getEventType());

        //try {
        //    m_flLogger.WriteLogEntry(se);
        //} catch(Exception ex) {
        //    Log.w("LoggerService", "Generic error!");
        //    ex.printStackTrace();
        //}

        if (eventMethod.equals("get"))
            httpGET(hcClient, eventURL, eventToken, se);
        else if (eventMethod.equals("post"))
            httpPOST(hcClient, eventURL, eventToken, se);
    }

    protected void HandleEventLog(HttpClient hcClient, String eURL, String eMethod, String eToken, String eType, String eTags, String eData) {
        HandleEventLog(hcClient, new SimpleEvent(eType, eTags, eData));
    }

    protected String httpGET(HttpClient hcClient, String eURL, String eToken, SimpleEvent se) {
        //Log.i("LoggerService", "httpGET " + eURL);
        String sb = null;

        try {
            HttpGet request = new HttpGet();
            request.setURI(new URI(eURL + eToken + "/"
                    + URLEncoder.encode(se.getEventType(), "UTF-8") + "/"
                    + URLEncoder.encode(se.getEventTags(), "UTF-8") + "/"
                    + URLEncoder.encode(se.getEventData(), "UTF-8") + "/"
                    + URLEncoder.encode("" + se.getEventTimeStamp(), "UTF-8")));
            request.setHeader(HTTP.CONN_DIRECTIVE, HTTP.CONN_KEEP_ALIVE);
            HttpResponse response = hcClient.execute(request);
            sb = GetResponseText(response);
        } catch (Exception ex) {
            Log.w("LoggerService", ex.getMessage());
            ex.printStackTrace();
            sb = null;
        } finally {
        }

        return sb;
    }

    protected String httpGET(HttpClient hcClient, String eURL, String eToken, String eType, String eTags, String eData) {
        return httpGET(hcClient, eURL, eToken, new SimpleEvent(eType, eTags, eData));
    }

    protected String httpPOST(HttpClient hcClient, String eURL, String eToken, SimpleEvent se) {
        //Log.d("LoggerService", "STARTED httpPOST");

        String sb = null;
        // Create a new HttpClient and Post Header
        HttpPost request = new HttpPost(eURL);
        request.setHeader(HTTP.CONN_DIRECTIVE, HTTP.CONN_KEEP_ALIVE);

        try {
            // Add your data
            List<NameValuePair> nameValuePairs = new ArrayList<NameValuePair>(4);
            nameValuePairs.add(new BasicNameValuePair("token", eToken));
            nameValuePairs.add(new BasicNameValuePair("entrytype", se.getEventType()));
            nameValuePairs.add(new BasicNameValuePair("entrytags", se.getEventTags()));
            nameValuePairs.add(new BasicNameValuePair("entrydata", se.getEventData()));
            nameValuePairs.add(new BasicNameValuePair("entrytimestamp", "" + se.getEventTimeStamp()));

            request.setEntity(new UrlEncodedFormEntity(nameValuePairs));
            request.setHeader(HTTP.CONN_DIRECTIVE, HTTP.CONN_KEEP_ALIVE);

            // Execute HTTP Post Request
            HttpResponse response = hcClient.execute(request);
            sb = GetResponseText(response);
        } catch(Exception ex)
        {
            Log.w("LoggerService", ex.toString());
            ex.printStackTrace();
            sb = null;
        }
        finally {
        }

        Log.d("LoggerService", "ENDED httpPOST " + sb);

        return sb;
    }

    protected String httpPOST(HttpClient hcClient, String eURL, String eToken, String eType, String eTags, String eData) {
        return httpPOST(hcClient, eURL, eToken, new SimpleEvent(eType, eTags, eData));
    }

    protected int count = 0;

    @Override
    public void dataReceived(FlyverMQMessage message) {
        if (count++ > 5) {
            LogData(new SimpleEvent(message.topic, tagsFromMessage(message), message.data.toString(), message.creationTime));
            count = 0;
        }
    }

    @Override
    public void unregistered() {
        Log.d("LoggerService", "has been unregistered from the Message Queue.");
    }

    @Override
    public void paused() {

    }

    @Override
    public void resumed() {

    }

    protected String tagsFromMessage(FlyverMQMessage message) {
        JSONObject json = new JSONObject();
        String sRet = "";

        try {
            json.put("messageid", message.messageId);
            json.put("priority", message.priority);
            json.put("ttl", message.ttl);
            sRet = json.toString();
        } catch (JSONException e) {
            e.printStackTrace();
            sRet = "";
        }

        return sRet;
    }
} public interface Drone{

    public void updateSpeeds(float yawForce, float pitchForce, float rollForce, float altitudeForce);
    public void setToZero();
    public String getDebugText();
    public float[] getMotorPowers();
    class MotorPowers {};
    // TODO: Fix working only with IOIO
} public interface MicroController {
    /**
     * Initialization Setup
     * @throws ConnectionLostException
     */
    public void setup() throws  ConnectionLostException;

    /**
     * Loops the microcontroller.
     * @throws InterruptedException
     * @throws ConnectionLostException
     */
    public void loop() throws  InterruptedException, ConnectionLostException;
    // TODO: Fix the excetions

} public class PIDAngleController {
    public PIDAngleController(float kp, float ki, float kd, float smoothingStrength) {
        this.kp = kp;
        this.ki = ki;
        this.kd = kd;
        this.smoothingStrength = smoothingStrength;
        previousDifference = 0.0f;

        integrator = 0.0f;
        differencesMean = 0.0f;
    }

    public float getInput(float targetAngle, float currentAngle, float dt) {
        // The complete turn can be done, so we have to be careful around the
        // "+180°, -180°" limit.
        float rawDifference = targetAngle - currentAngle;
        float difference = DroneState.getMainAngle(rawDifference);
        boolean differenceJump = (difference != rawDifference);

        // Now, the PID computation can be done.
        float input = 0.0f;

        // Proportional part.
        input += difference * kp;

        // Integral part.
        integrator += difference * ki * dt;
        input += integrator;
        // Derivative part, with filtering.
        if (!differenceJump) {
            differencesMean = differencesMean * smoothingStrength
                    + difference * (1 - smoothingStrength);
            float derivative = (differencesMean - previousDifference) / dt;
            previousDifference = differencesMean;
            input += derivative * kd;
        } else {
            // Erase the history, because we are not reaching the target from
            // the "same side".
            differencesMean = 0.0f;
        }

        return input;
    }

    public void setCoefficients(float kp, float ki, float kd) {
        this.kp = kp;
        this.ki = ki;
        this.kd = kd;
    }

    public void resetIntegrator() {
        integrator = 0.0f;
    }

    private float kp, ki, kd, integrator, smoothingStrength, differencesMean,
            previousDifference;
} public class DroneState implements  FlyverMQConsumer, FlyverMQCallback {

    public static final float PI = 3.14159265359f;
    public static final float RAD_TO_DEG = 180.0f / PI;
    public static final double DEG_TO_RAD = PI / 180.0;
    public static final float ALTITUDE_SMOOTHING = 0.95f;
    public static final String TAG = "drone_state";
    public static final String TOPIC = "dronestate.raw";

    private DroneStateData droneStateData;
    private float[] yawPitchRollVec = new float[3];
    private float yawZero;
    private float pitchZero;
    private float rollZero;
    private float elevationZero;
    private float latitudeZero;
    private float longitudeZero;
    private float absoluteLongitude;
    private float absoluteLatitude;
    private float absoluteElevation;
    private SensorsWrapper sensors;
    private FlyverMQProducer dronestateProducer;

    static int count = 0;
    @Override
    public void dataReceived(FlyverMQMessage message) {
        // Make the measurements relative to the user-defined zero orientation.
        count++;
        assert message != null;
        String data = (String) message.data;
        String[] nums = data.replace('[', ' ').replace(']', ' ').split(",");
        //Log.d(TAG, String.valueOf(count).concat(" received msg ").concat(message.topic));

        float[] orientationMatrix = new float[nums.length];
        for (int i = 0; i < nums.length ; i++) {
            orientationMatrix[i] = Float.parseFloat(nums[i]);
        }
        System.arraycopy(orientationMatrix, 0, yawPitchRollVec, 0, 3);
        droneStateData.yaw = getMainAngle(-(yawPitchRollVec[0] - yawZero) * RAD_TO_DEG);
        droneStateData.pitch = getMainAngle(-(yawPitchRollVec[1] - pitchZero) * RAD_TO_DEG);

        if (yawPitchRollVec[2] * RAD_TO_DEG < -150f) { // If the roll angle is less than -150 add 180
            droneStateData.roll = getMainAngle((yawPitchRollVec[2] - rollZero) * RAD_TO_DEG + 180f);
        } else if (yawPitchRollVec[2] * RAD_TO_DEG > +150f) {
            droneStateData.roll = getMainAngle((yawPitchRollVec[2] - rollZero) * RAD_TO_DEG - 180f);
        } else {
            droneStateData.roll = getMainAngle((yawPitchRollVec[2] - rollZero) * RAD_TO_DEG);
        }
//        Log.d(TAG, "YAW: " + droneStateData.yaw + " PITCH: " + droneStateData.pitch + " ROLL: " + droneStateData.roll);
        droneStateData.time = System.nanoTime();
        FlyverMQMessage msg = new FlyverMQMessage.MessageBuilder().setCreationTime(1).setMessageId(2).setTtl(4).setPriority((short) 1).setTopic(TOPIC).setData(droneStateData).build();
        dronestateProducer.addMessage(msg);
    }

    @Override
    public void unregistered() {

    }

    @Override
    public void paused() {

    }

    @Override
    public void resumed() {

    }

    @Override
    public void producerRegistered(String topic) {

    }

    @Override
    public void producerUnregistered(String topic) {

    }

    public float[] getYawPitchRollVec() {
        return yawPitchRollVec;
    }

    public DroneState() {
        droneStateData = new DroneStateData();
        yawZero = 0.0f;
        pitchZero = 0.0f;
        rollZero = 0.0f;

        dronestateProducer = new FlyverMQProducer(TOPIC) {

            @Override
            public void registered() {

            }

            @Override
            public void unregistered() {

            }

            @Override
            public void onPause() {

            }

            @Override
            public void onResume() {

            }
        };
        MainController.getInstance().getMessageQueue().registerConsumer(this, "sensors.raw");
        try {
            dronestateProducer.register(false);
        } catch (ProducerAlreadyRegisteredException e) {
            e.printStackTrace();
        }
    }

    public class DroneStateData {
        public float yaw, pitch, roll; // [degrees].
        public long time; // [nanoseconds].

        // TODO: Other state data could be added

    }

    public DroneStateData getState() {
        return droneStateData;
    }

    public void setCurrentStateAsZero() {
        yawZero = yawPitchRollVec[0];
//        pitchZero = yawPitchRollVec[1];
//        rollZero = yawPitchRollVec[2];
        elevationZero = absoluteElevation;
        longitudeZero = absoluteLongitude;
        latitudeZero = absoluteLatitude;
    }

    // Return the smallest angle between two segments.
    public static float getMainAngle(float angle) {
        while (angle < -180.0f)
            angle += 360.0f;
        while (angle > 180.0f)
            angle -= 360.0f;

        return angle;
    }

    public float[] getZeroStates() {
        float[] zeroStates = new float[3];

        zeroStates[0] = yawZero;
        zeroStates[1] = pitchZero;
        zeroStates[2] = rollZero;

        return zeroStates;
    }
} public class FlyverMQ {


    /**
     * Helper class
     * Used to easily associate producers with an internal queue for their events
     * Constructor can be passed only producer, and the queue will be dynamic LinkedList,
     * or producer and size, and the queue will be CapacityQueue, which automatically removes
     * the oldest objects, when it's capacity is reached
     */
    private class SimpleMQProducerWrapper {
        private FlyverMQProducer producer;

        public FlyverMQProducer getProducer() {
            return producer;
        }

        public BlockingQueue<FlyverMQMessage> getQueue() {
            return queue;
        }

        private BlockingQueue<FlyverMQMessage> queue;

        SimpleMQProducerWrapper(FlyverMQProducer producer) {
            this.producer = producer;
            queue = new LinkedBlockingQueue<>();
        }

        SimpleMQProducerWrapper(FlyverMQProducer producer, int size) {
            this.producer = producer;
            queue = new CapacityQueue<>(size);
        }

        //prevent instantiating with an empty constructor
        private SimpleMQProducerWrapper() {
        }

        public int getEventCount() {
            return queue.size();
        }
    }


    /**
     * Helper class wrapping around asynchronous consumers
     * Used to associate them with an internal semaphore
     * as to avoid concurency issues
     */
    protected class SimpleMQAsyncConsumerWrapper {
        private Semaphore semaphore = new Semaphore(1);
        private FlyverMQConsumer consumer;
        private String topic;

        public Semaphore getSemaphore() {
            return semaphore;
        }

        public FlyverMQConsumer getConsumer() {
            return consumer;
        }

        public SimpleMQAsyncConsumerWrapper(FlyverMQConsumer consumer, String topic) {
            this.consumer = consumer;
            this.topic = topic;
        }

        public String getTopic() {
            return topic;
        }
    }

    /**
     * Helper class, that wraps the real-time consumers
     * with an internal queue for the messages they are subscribed to,
     * as to guarantee delivery and processing of every message received
     */
    private class SimpleMQRTConsumerWrapper {
        private FlyverMQConsumer consumer;
        private Thread worker;
        private LinkedList<FlyverMQMessage> queue;
        private ExecutorService executorService = Executors.newSingleThreadExecutor();
        boolean empty = true;

        private SimpleMQRTConsumerWrapper() {
            //prevent instantiating with empty constructor
        }

        SimpleMQRTConsumerWrapper(FlyverMQConsumer consumer) {
            this.consumer = consumer;
            worker = new Thread(new Runnable() {
                @Override
                public void run() {
                    doWork();
                }
            });
            worker.start();
        }

        /**
         * Adds a message to the queue, and if it's empty,
         * notifies the worker thread, associated with the wrapper,
         * that there are messages ready for processing
         *
         * @param message
         */
        public void addMessage(FlyverMQMessage message) {
            queue.add(message);
            if (empty) {
                empty = false;
                this.notify();
            }
        }

        public int getEventCount() {
            return queue.size();
        }

        /**
         * Loops until all messages in the queue have been processed
         * If no more messages are left, it waits until notified
         * that more messages have been added
         */
        private void doWork() {
            //noinspection InfiniteLoopStatement
            while (true) {
                final FlyverMQMessage message = queue.pop();
                executorService.execute(new Runnable() {
                    @Override
                    public void run() {
                        consumer.dataReceived(message);
                    }
                });
                if (queue.isEmpty()) {
                    empty = true;
                }
                try {
                    this.wait();
                } catch (InterruptedException e) {
                    e.printStackTrace();
                }
            }
        }
    }

    /**
     * Helper class intended for running in a worker thread
     * it polls the producers and gets
     * the messages that are on top of their respective queue,
     * then broadcasts them to every client, that is registered
     * for the message's topic. The client's dataReceived(SimpleMQMessage)
     * method is executed in a different thread, managed by a ThreadPool implemented
     * via ExecutorService
     */
    private class SimpleMQWorker implements Runnable {
        FlyverMQMessage message;
        ExecutorService executorService = Executors.newFixedThreadPool(50);

        @Override
        public void run() {
            //noinspection InfiniteLoopStatement
            while (true) {
                //loop over all registered producers
                try {
                    topicsGuard.acquire();
                } catch (InterruptedException e) {
                    e.printStackTrace();
                }
                for (String topic : topics.keySet()) {
                    //get the topmost event of the queue, associated with the current producer
                    //sometimes tries to poll past the index of the queue
                    //swallow the exception, and try on the next iteration again
                    try {
                        message = topics.get(topic).poll();
                        if (message == null) {
                            continue;
                        }
                    } catch (NoSuchElementException e) {
                        //e.printStackTrace();
                        continue;
                    }
                    //get the linked list containing all consumers registered for the topic
                    //and broadcast the message to all of them
                    if (asyncConsumers.get(message.topic) == null) {
                        Log.e("MQ", "No consumers present for topic ".concat(message.topic));
                        continue;
                    }
                    for (SimpleMQAsyncConsumerWrapper wrapper : asyncConsumers.get(message.topic)) {
                        //post the client's dataReceived method to the ThreadPool
                        if(!wrapper.topic.equals(message.topic)) {
                            Log.e(TAG, "message intended for topic ".concat(message.topic).concat(" is sent to ".concat(wrapper.getTopic())));
                        }
                        sendMsg(wrapper, message);
                    }
                }
                topicsGuard.release();
            }
        }

        private void sendMsg(final SimpleMQAsyncConsumerWrapper wrapper, final FlyverMQMessage msg) {
            executorService.execute(new Runnable() {
                @Override
                public void run() {
                    try {
                        wrapper.getSemaphore().acquire();
                    } catch (InterruptedException e) {
                        e.printStackTrace();
                    }
                    wrapper.getConsumer().dataReceived(msg);
                    wrapper.getSemaphore().release();
                }
            });
        }
    }

    private static FlyverMQ instance;
    private final String TAG = "SimpleMQ";
    private final String REGISTEREDMSG = "Producer already registered: ";
    private final String NOTOPICMSG = "No such topic exists : ";
    protected Map<String, LinkedList<FlyverMQProducer>> producers = new HashMap<>();
    protected Map<String, LinkedList<SimpleMQAsyncConsumerWrapper>> asyncConsumers = new HashMap<>();
    protected Map<String, LinkedList<FlyverMQConsumer>> rtConsumers = new HashMap<>();
    protected LinkedList<FlyverMQCallback> registeredComponentCallbacks = new LinkedList<>();
    protected Map<String, BlockingQueue<FlyverMQMessage>> topics = new HashMap<>();
    private FlyverMQSocketServer socketServer;
    private Semaphore topicsGuard = new Semaphore(1);

    public FlyverMQ() {
    }

    public static FlyverMQ getInstance() throws FlyverMQException {
        if (instance == null) {
            throw new FlyverMQException("FlyverMQ is not instantiated");
        }
        return instance;
    }

    public static void start() {
        if(instance == null) {
            instance = new FlyverMQ().init();
        }
    }

    /**
     * Initializes the internals of the message queue and starts it's worker threads
     *
     * @return this, as to allow chaining
     */
    private FlyverMQ init() {
        final FlyverMQ flyverMQref = this;
        Thread networkIO = new Thread(new Runnable() {
            @Override
            public void run() {
                socketServer = new FlyverMQSocketServer(flyverMQref);
            }
        });
        networkIO.start();
        Thread worker = new Thread(new SimpleMQWorker());
        worker.setDaemon(true);
        worker.start();
        return this;
    }

    /**
     * Register an object, implementing the SimpleMQCallback interface
     * which is used to monitor changes in the producers registration and deregistration
     * from the components
     *
     * @param callback SimpleMQCallback, implementing onRegistered and onUnregistered methods
     * @return this, as to allow chaining
     */
    public FlyverMQ producersStatusCallback(FlyverMQCallback callback) {
        registeredComponentCallbacks.add(callback);
        return this;
    }

    /**
     * Register an Object implementing the SimpleMQProducer interface to a topic
     * All messages by this producer are enqueued for later processing
     *
     * @param producer       Class implementing SimpleMQProducer interface
     * @param topic          String defining the topic of the producer
     * @param removeExisting Set to true to remove previously registered producer for the topic
     * @return this, to allow chaining
     * @throws ProducerAlreadyRegisteredException - Throws the already registered exception if a producer is already registered,
     *                                            and the removeExisting flag is set to false
     */
    public FlyverMQ registerProducer(FlyverMQProducer producer, String topic, boolean removeExisting) throws ProducerAlreadyRegisteredException {
        if (topicExists(topic) && !removeExisting) {
            throw new ProducerAlreadyRegisteredException(REGISTEREDMSG.concat(topic));
        }
        if (topicExists(topic)) {
            try {
                unregisterProducer(topic);
            } catch (NoSuchTopicException e) {
                e.printStackTrace();
            }
        }

        if (!producers.containsKey(topic)) {
            producers.put(topic, new LinkedList<FlyverMQProducer>());
        } else {
            producers.get(topic).add(producer);
        }
        if (!topics.containsKey(topic)) {
            try {
                topicsGuard.acquire();
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
            topics.put(topic, new LinkedBlockingQueue<FlyverMQMessage>());
            topicsGuard.release();
        }
        for (FlyverMQCallback callback : registeredComponentCallbacks) {
            callback.producerRegistered(topic);
        }
        Log.d(TAG, "Producer registered ".concat(topic));
        return this;
    }

    /**
     * Register an Object implementing the SimpleMQProducer interface to a topic
     * All messages by this producer are enqueued for later processing
     * The internal queue of the SimpleMQ implementing object is limited to the number
     * passed as the fourth parameter. Any events that are passed over that limit
     * discard the oldest events, and are enqueued
     *
     * @param producer       Class implementing SimpleMQProducer interface
     * @param topic          String defining the topic of the producer
     * @param removeExisting Set to true to remove previously registered producer for the topic
     * @param maxQueueEvents int, the limit of the queue
     * @return this, to allow chaining
     * @throws ProducerAlreadyRegisteredException - Throws the already registered exception if a producer is already registered,
     *                                            and the removeExisting flag is set to false
     */
    public FlyverMQ registerProducer(FlyverMQProducer producer, String topic, boolean removeExisting, int maxQueueEvents) throws ProducerAlreadyRegisteredException {
        if (!removeExisting && topicExists(topic)) {
            throw new ProducerAlreadyRegisteredException(REGISTEREDMSG.concat(topic));
        }
        if (topicExists(topic)) {
            try {
                unregisterProducer(topic);
            } catch (NoSuchTopicException e) {
                e.printStackTrace();
            }
        }
        if (!producers.containsKey(topic)) {
            producers.put(topic, new LinkedList<FlyverMQProducer>());
        } else {
            producers.get(topic).add(producer);
        }
        if (!topics.containsKey(topic)) {
            try {
                topicsGuard.acquire();
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
            topics.put(topic, new LinkedBlockingQueue<FlyverMQMessage>());
            topicsGuard.release();
        }
        for (FlyverMQCallback callback : registeredComponentCallbacks) {
            callback.producerRegistered(topic);
        }
        Log.d(TAG, "Producer registered ".concat(topic));
        return this;
    }

    /**
     * Removes the producer for a supplied topic from the list of producers
     * If no producer is present for the supplied topic, an exception is thrown
     *
     * @param topic the topic for which the producer is to be removed
     * @return this, to allow chaining
     * @throws NoSuchTopicException - thrown if no producer is registered for the requested topic
     */
    public FlyverMQ unregisterProducer(String topic) throws NoSuchTopicException {
        if (!topicExists(topic)) {
            throw new NoSuchTopicException(NOTOPICMSG.concat(topic));
        } else {
//            producers.get(topic).unregistered();
            for (FlyverMQProducer producer : producers.get(topic)) {
                producer.unregistered();
            }
            producers.remove(topic);
            for (FlyverMQCallback callback : registeredComponentCallbacks) {
                callback.producerUnregistered(topic);
            }
            return this;
        }
    }

    /**
     * Register a consumer for a topic.
     * More than one consumer may be registered for a single topic
     *
     * @param consumer Object implementing the SimpleMQConsumer interface
     * @param topic    String defining the topic that the consumer should receive
     * @return - this, to allow chaining
     */
    public FlyverMQ registerConsumer(FlyverMQConsumer consumer, String topic) {
        if (!asyncConsumers.containsKey(topic)) {
            asyncConsumers.put(topic, new LinkedList<SimpleMQAsyncConsumerWrapper>());
            asyncConsumers.get(topic).add(new SimpleMQAsyncConsumerWrapper(consumer, topic));
        } else {
            asyncConsumers.get(topic).add(new SimpleMQAsyncConsumerWrapper(consumer, topic));
        }
        return this;
    }

    /**
     * Removes all enqueued message for the supplied topic.
     * Throws exception if no producers are registered for the supplied topic
     *
     * @param topic string defining the topic that is to be cleared
     * @return this, to allow chaining
     * @throws NoSuchTopicException - thrown if the topic has no registered producers
     */
    public FlyverMQ clearTopic(String topic) throws NoSuchTopicException {
        if (topicExists(topic)) {
            topics.get(topic).clear();
        } else {
            throw new NoSuchTopicException(NOTOPICMSG.concat(topic));
        }
        return this;
    }

    /**
     * Returns the count of all events in the queue, associated with the topic
     * Throws exception if no such topic is present
     *
     * @param topic String defining the topic for which the event count is requested
     * @return int, count of the events in the queue
     * @throws NoSuchTopicException - thrown if no such topic is registered
     */
    public int getEventCount(String topic) throws NoSuchTopicException {
        if (!topicExists(topic)) {
            throw new NoSuchTopicException(NOTOPICMSG.concat(topic));
        } else {
            return topics.get(topic).size();
        }
    }

    /**
     * Adds message to the message queue associated with a topic
     *
     * @param producer - producer of the data
     * @param data     string with the data contained in the message
     * @return boolean, true if adding the message to the queue is successful, false otherwise
     */
    protected boolean addMessage(FlyverMQProducer producer, FlyverMQMessage data) throws NoSuchTopicException {
        if (!topics.containsKey(producer.topic)) {
            throw new NoSuchTopicException(NOTOPICMSG + producer.topic);
        }
        return topics.get(producer.topic).add(data);
    }

    /**
     * Get the first message of the queue for a topic
     *
     * @param topic    String defining the topic that should provide the message
     * @param blocking boolean, if processing the message is synchronous or not
     * @return SimpleMQMessage, the message on top of the queue
     * @throws NoSuchTopicException - thrown if no such  topic is in existence
     */
    public FlyverMQMessage getMessage(String topic, boolean blocking) throws NoSuchTopicException {
        if (!topicExists(topic)) {
            throw new NoSuchTopicException(NOTOPICMSG.concat(topic));
        }
        return topics.get(topic).poll();
    }

    /**
     * Get the first N messages on the top of the queue
     *
     * @param topic    String defining the topic that should provide the messages
     * @param blocking boolean, if processing the message is synchronous or not
     * @param count    int, the number N of the requested messages
     * @return LinkedList containing the requested messages
     * @throws NoSuchTopicException - thrown if no such topic is in existence
     */
    public LinkedList<FlyverMQMessage> getMessages(String topic, boolean blocking, int count) throws NoSuchTopicException {
        if (!topicExists(topic)) {
            throw new NoSuchTopicException(NOTOPICMSG + topic);
        }
        LinkedList<FlyverMQMessage> messages = new LinkedList<>();
        for (int i = 0; i < count; i++) {
            FlyverMQMessage message = topics.get(topic).poll();
            if (message == null) {
                return messages;
            }
            messages.add(message);
        }
        return messages;
    }

    /**
     * Get all the topics, that have registered producers to them
     * can match against strings and java-formatted regular expressions alike
     *
     * @param topic String for the topic that is being searched.
     *              If passed null value or wildcard "*" returns all registered topics
     *              else returns all topics starting with the passed string
     * @return - LinkedList containing the matched topics
     */
    protected LinkedList<String> listTopics(String topic) {
        LinkedList<String> topics = new LinkedList<>();
        if (topic == null || topic.equals("*")) {
            for (String s : producers.keySet()) {
                topics.add(s);
            }
        } else {
            Pattern pattern = Pattern.compile("\\^".concat(topic).concat(".+"));
            Matcher matcher;
            for (String s : producers.keySet()) {
                matcher = pattern.matcher(s);
                if (matcher.matches()) {
                    topics.add(s);
                }
            }
        }
        return topics;
    }

    /**
     * Check if the supplied topic has any registered producers to it
     *
     * @param topic string defining the topic
     * @return boolean, true if the topic has any active producers, false otherwise
     */
    public boolean topicExists(String topic) {
        return producers.containsKey(topic);
    }

} public class FlyverMQMessage {

    public static class MessageBuilder {

        public MessageBuilder setTtl(long ttl) {
            this.ttl = ttl;
            return this;
        }

        public MessageBuilder setPriority(short priority) {
            this.priority = priority;
            return this;
        }

        public MessageBuilder setMessageId(long messageId) {
            this.messageId = messageId;
            return this;
        }

        public MessageBuilder setTopic(String topic) {
            this.topic = topic;
            return this;
        }

        public MessageBuilder setCreationTime(long creationTime) {
            this.creationTime = creationTime;
            return this;
        }

        public MessageBuilder setData(Object data) {
            this.data = data;
            return this;
        }

        public FlyverMQMessage build() {
            return new FlyverMQMessage(ttl, priority, messageId, topic, creationTime, data);
        }

        Long ttl;
        Short priority;
        Long messageId;
        String topic;
        Long creationTime;
        Object data;
    }

    /**
     * Time to live of the message in milliseconds
     * milliseconds = seconds / 1000
     * Compare with the creationTime timestamp,
     * and discard the message if the difference is
     * above a predefined threshold
     */
    public Long ttl;

    /**
     * Priority of the message
     * Dispatch messages with higher priorities first
     */
    public Short priority;

    /**
     * Message ID
     */
    public Long messageId;

    /**
     * Topic/subject of the message
     * Defined by the producer
     * Clients subscribe for messages based on this topic
     */
    public String topic;

    /**
     * Creation time in milliseconds.
     * milliseconds = seconds / 1000
     * Used to discard outlived messages
     * based on the ttl
     */
    public Long creationTime;

    /**
     * User-provided data carried by the message
     */
    public Object data;

    /**
     * Private constructor, message should be created via Builder
     */
    private FlyverMQMessage(long ttl, short priority, long messageId, String topic, long creationTime, Object data) {
        this.ttl = ttl;
        this.priority = priority;
        this.messageId = messageId;
        this.topic = topic;
        this.creationTime = creationTime;
        this.data = data;
    }
} public class FlyverMQException extends Exception {
    public FlyverMQException(String detailMessage) {
        super(detailMessage);
    }
} public interface FlyverMQConsumer {
    /**
     * Data recieved callback for the consumer.
     * Called when a message with the associated topic
     * for the consumer is received in the message queue
     * @param message SimpleMQMessage data container
     */
    public void dataReceived(FlyverMQMessage message);

    /**
     * Consumer unregister hook
     * Called when the consumer is removed from the message queue
     */
    public void unregistered();

    //TODO: clarify if consumers need onPause/onResume functionallity
    public void paused();
    public void resumed();
} public class FlyverPreferences {
    private final static String PATH = Environment.getExternalStorageDirectory()
                                         .toString().concat("/co.flyver/config/");
    private final static String FILENAME = "config.properties";

    private static Properties properties = new Properties();
    private static HashMap<String, String> staged = null;

    // prevent instantiation as all methods are static
    private FlyverPreferences() {
    }


    /**
     * Commits all staged changes to the properties
     * After calling this method all staged changes are cleared
     */
    public static void commit() {
        File file = new File(PATH);

        if(!file.exists()) {
            file.mkdirs();
        }
        if(staged == null) {
            return;
        }

        for(String key : staged.keySet()) {
            properties.setProperty(key, staged.get(key));
        }
        OutputStream outputStream;
        try {
            outputStream = new FileOutputStream(PATH.concat(FILENAME));
            properties.store(outputStream, null);
        } catch (IOException e) {
            e.printStackTrace();
        }
        staged = null;
    }

    /**
     * Sets the property denoted by the key
     * Also creates new preferences, if they are not present
     * @param key
     * @param value
     */
    public static void setPreference(String key, String value) {
        if(null == staged) {
            staged = new HashMap<>();
        }
        staged.put(key, value);
    }

    /**
     * Returns the value associated with the preference key
     * @param key
     * @return String
     */
    public static String getPreference(String key) {
        File file = new File(PATH.concat(FILENAME));
        if(!file.exists() || file.isDirectory()) {
            return null;
        }
        InputStream inputStream = null;
        try {
            inputStream = new FileInputStream(file);
            properties.load(inputStream);
        } catch (IOException e) {
            e.printStackTrace();
        } finally {
            if (inputStream != null) {
                try {
                    inputStream.close();
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }
        }
        return properties.getProperty(key);
    }
}
import android.app.Activity;
import co.flyver.flyvercore.baseapp.BaseApp;
import co.flyver.flyvercore.pidcontrollers.PIDSettings;
import co.flyver.utils.containers.SharedIPCKeys;
import co.flyver.androidrc.server.Server;
import co.flyver.androidrc.server.Status;
import co.flyver.androidrc.server.interfaces.ServerCallback;
import co.flyver.dataloggerlib.LoggerService;
import co.flyver.flyvercore.dronetypes.Drone;
import co.flyver.flyvercore.microcontrollers.MicroController;
import co.flyver.flyvercore.pidcontrollers.PIDAngleController;
import co.flyver.flyvercore.statedata.DroneState;
import co.flyver.utils.flyvermq.FlyverMQ;
import co.flyver.utils.flyvermq.FlyverMQMessage;
import co.flyver.utils.flyvermq.exceptions.FlyverMQException;
import co.flyver.utils.flyvermq.interfaces.FlyverMQConsumer;
import co.flyver.utils.settings.FlyverPreferences;
import ioio.lib.spi.Log;
package co.flyver.flyvercore.maincontrollers;





/**
 * The MainController is the heart of Flyver
 * The MainController initialized all the controllers and control algorithms
 */

public class MainController extends Activity {

    /* CONSTANTS*/
    public static final double MAX_MOTOR_POWER = 1023.0;
    public static final long INT_MAX = Integer.MAX_VALUE;
    public static final float MAX_SAFE_PITCH_ROLL = 45; // [deg].
    public static final float PID_DERIV_SMOOTHING = 0.5f;
    private static final String CONTROLLER = "MainController";
    /* END OF*/

    private static MainController instance;
    private Drone drone;
    private float meanThrust;
    private float yawAngleTarget;
    private float pitchAngleTarget;
    private float rollAngleTarget;
    private float altitudeTarget;
    private float timeWithoutPcRx;
    private float timeWithoutAdkRx;
    private DroneState droneState;
    private boolean regulatorEnabled;
    private boolean altitudeLockEnabled;
    PIDAngleController yawController;
    PIDAngleController pitchController;
    PIDAngleController rollController;
    private DroneState.DroneStateData droneStateData;
    private long previousTime;
    private boolean zeroStateFlag = false; // Flag indicate that droneState should be zeroed
    private FlyverMQ messageQueue;


    private LoggerService logger;

    public static MainController getInstance() {
        return instance;
    }

    public FlyverMQ getMessageQueue() {
        return messageQueue;
    }

    public float getYawAngleTarget() {
        return yawAngleTarget;
    }

    public void setYawAngleTarget(float yawAngleTarget) {
        this.yawAngleTarget = yawAngleTarget;
    }

    public float getPitchAngleTarget() {
        return pitchAngleTarget;
    }

    public void setPitchAngleTarget(float pitchAngleTarget) {
        this.pitchAngleTarget = pitchAngleTarget;
    }

    public float getRollAngleTarget() {
        return rollAngleTarget;
    }

    public void setRollAngleTarget(float rollAngleTarget) {
        this.rollAngleTarget = rollAngleTarget;
    }

    public float getMeanThrust() {
        return meanThrust;
    }

    public void setMeanThrust(float meanThrust) {
        this.meanThrust = meanThrust;
    }


    private void initLogger() {
        logger = new LoggerService(BaseApp.applicationContext, messageQueue);
        logger.Start();
        logger.LogData("EV_DEBUG", "MainController", "MainController initialized the Logger.");
    }

    public MainController(Drone drone) throws MainControllerInstanceExisting {
        if(instance != null) {
            throw new MainControllerInstanceExisting("Only one instance of the MainController can be existing at a time");
        }
        regulatorEnabled = true;
        this.drone = drone;

        try {
            messageQueue = FlyverMQ.getInstance();
        } catch (FlyverMQException e) {
            e.printStackTrace();
        }

        // altitudeRegulator = new PidRegulator(0.0f,  0.0f,  0.0f, PID_DERIV_SMOOTHING, 0.0f);

        yawAngleTarget = 0.0f;
        pitchAngleTarget = 0.0f;
        rollAngleTarget = 0.0f;
        altitudeTarget = 0.0f;
        instance = this;

    }

    public void start() {
        // Initializations.
        regulatorEnabled = true;
        altitudeLockEnabled = false;
        meanThrust = 0.0f;

        // Start the sensors.
        droneState = new DroneState();
        droneStateData = droneState.new DroneStateData();

        //initialize the controller parameters
        PIDSettings.setPIDSettings();
        startPIDControllers();

        try {
            new MainControllerConsumers().start();
        } catch (FlyverMQException e) {
            e.printStackTrace();
        }

        registerCallbacks();
        registerConsumers();

        //initLogger();
    }

    public void stop() {
        // Stop the main controller thread.
        // Stop the sensors.
        droneState.paused();
    }

    public DroneState.DroneStateData getSensorsData() {
        return droneStateData;
    }

    public void setMeanTrust(float trust) {
        meanThrust = trust;
    }


    /**
     * onConnectionEstablished is called when the whole systems starts
     * and int working order
     */
    public void onConnectionEstablished() {
        // Reset the orientation.
        droneState.setCurrentStateAsZero();
        // TODO: droneState.setCurrentStateAsZero works only on initialization, but can't be accessed after
    }

    /**
     * Called when the communication link between the drone and the RC is lost.
     * TODO: Break it into cases and implement algorithms such as Return To Home
     */
    public void onConnectionLost() {
        // Emergency stop of the quadcopter.
        emergencyStop("Connection Lost");
    }

    public void onIoioConnect(MicroController microController) {
        Log.e(CONTROLLER, "IOIO Connection established");

    }

    public void onIoioDisconnect() {
        Log.e(CONTROLLER, "IOIO Connection lost");
    }

    /**
     * Stops the drone.
     *
     * @param reason for logging the cause of the emergency
     */
    public void emergencyStop(String reason) {
        // TODO: Smart Landing
        Log.w("Emergency", reason);
        yawController.resetIntegrator();
        pitchController.resetIntegrator();
        rollController.resetIntegrator();
        setMeanThrust(0);
    }

    private void startPIDControllers() {
        float[][] pidValues = PIDSettings.getPidValues();
        yawController = new PIDAngleController(pidValues[0][0], pidValues[0][1], pidValues[0][2], PID_DERIV_SMOOTHING);
        pitchController = new PIDAngleController(pidValues[1][0], pidValues[1][1], pidValues[1][2], PID_DERIV_SMOOTHING);
        rollController = new PIDAngleController(pidValues[2][0], pidValues[2][1], pidValues[2][2], PID_DERIV_SMOOTHING);
    }

    private void registerCallbacks() {
        ServerCallback onValuesChange = new ServerCallback() {
            @Override
            public void run(String json) {
                setMeanTrust(Server.sCurrentStatus.getThrottle());