public override byte[] GetBytes()
        {
            Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
            Node ignore = f.RearNode;

            ForkliftPath fp = new ForkliftPath(GetPath(), ignore);

            return Encoding.ASCII.GetBytes(fp.getDirections());
        }
        public void TestForkliftPathGraphSixNodesFromAToBBackToB()
        {
            Graph g = Database.Instance.Data.Graphs.FirstOrDefault().Value;

            Path p = g.ShortestPath(g.getNode("a"), g.getNode("b"), g.getNode("b"));

            ForkliftPath fp = new ForkliftPath(p, g.getNode("b"));

            string actual = fp.getDirections();
            string expected = "NNLSNSNNLNL";

            Assert.IsTrue(actual == expected);

        }
        // Consumes one package 
        private void ConsumePackage(byte[] package)
        {
            // Allocates data from the package
            byte[] data = new byte[package.Length - 3];

            // Finds the type of pakcage
            byte byteType = package[1];

            // Fills the data array with the corresponding data
            for (int i = 2, j = 0; i < package.Length - 1; i++, j++)
            {
                data[j] = package[i];
            }

            // Convert the data to a string 
            string dataString = Encoding.UTF8.GetString(data, 0, data.Length);

            // Find the correct method to consume the package
            switch (byteType)
            {
                case TYPE_UPDATE_COLOR:
                    // Tell the user that the color is being fetched 
                    // dataString contains the id of the color
                    ConsoleHandler.AddMessage(MessageType.BLUETOOTH, "Sending color #" + dataString + "...");

                    // Get the color from the database
                    Color requestedColor = Database.Instance.Data.Colors.FirstOrDefault(i => i.Value.Identifier == int.Parse(dataString)).Value;

                    // Convert the RGB-values of the color to byte array
                    byte[] returnData = requestedColor.ToRGBBytes();

                    // Send the package back to the NXT
                    SendPackageBT(TYPE_FETCHED_COLOR, returnData);
                    
                    break;

                case TYPE_SAVE_COLOR:
                      
                    // Split the data into to values that needs to be saved
                    int colorId = int.Parse(dataString.Substring(0,3)); // Use the first char as color identifer
                    int red = int.Parse(dataString.Substring(3,3)); // Use the first char as red value
                    int green = int.Parse(dataString.Substring(6,3)); // Use the first char as green value
                    int blue = int.Parse(dataString.Substring(9,3)); // Use the first char as blue value

                    // Tell the user that the color is being saved 
                    ConsoleHandler.AddMessage(MessageType.BLUETOOTH, "Received color #" + colorId + ". (" + red + ", " + green + ", " + blue + ")");

                    // Find the old color
                    Color color = Database.Instance.Data.Colors.FirstOrDefault(i => i.Value.Identifier == colorId).Value;

                    // Update the values
                    color.Red = red;
                    color.Green = green;
                    color.Blue = blue;

                    // Save the updated color
                    Database.Instance.Data.AddColor(color);
                    break;

                case TYPE_REPORT_OBSTACLE:

                    ConsoleHandler.AddMessage(MessageType.ERROR, "Obstacle encountered!");
                    
                    int directionsIndex = int.Parse(dataString);

                    if (!(CurrentJob is TurnJob) && !(CurrentJob is DebugJob))
                    {
                        if (CurrentJob != null)
                        {
                            Path currentPath = CurrentJob.GetPath();

                            String directions = new ForkliftPath(currentPath, forklift.RearNode).getDirections();

                            if (CurrentJob is PalletJob)
                            {
                                PalletJob pj = (PalletJob)CurrentJob;

                                if (pj.Type == PalletJobType.fetch)
                                {
                                    directions = directions.Insert(0, "TUND");
                                }
                                else if (pj.Type == PalletJobType.deliver)
                                {
                                    directions = directions.Insert(0, "TUBDN");
                                }
                            }

                            int max = directions.Length;
                            int i = directionsIndex;

                            if (directions[directions.Length - 1] == 'L' || directions[directions.Length - 1] == 'R')
                            {
                                max--;
                            }

                            double nodeRearIndex = (max - 1.0 - i) / 2.0;

                            if (Math.Abs(nodeRearIndex % 1 - 0.5) < 0.4)
                            {
                                ConsoleHandler.AddMessage(MessageType.ERROR, "Currently inside intersection - aborting all jobs");

                                forklift.FrontNode = null;
                                forklift.FrontNode = null;

                                // Inform the user to update the position
                                ConsoleHandler.AddMessage(MessageType.ERROR, "Please update the position of the forklift...");

                                while (forklift.FrontNode == null || forklift.RearNode == null)
                                {
                                    ;
                                }
                            }
                            else
                            {
                                Node newRearNode = null;
                                Node newFrontNode = null;
                                int roundedIndex = (int)nodeRearIndex;

                                if (roundedIndex == 0)
                                {
                                    newFrontNode = forklift.FrontNode;
                                    newRearNode = forklift.RearNode;
                                }
                                else
                                {
                                    newFrontNode = currentPath.Nodes[roundedIndex];
                                    newRearNode = currentPath.Nodes[roundedIndex - 1];
                                }

                                // Update the visitied count on edges
                                Node previousNode = null;

                                foreach (Node n in currentPath.Nodes)
                                {
                                    if (previousNode == null)
                                    {
                                        previousNode = n;
                                        continue;
                                    }

                                    // Find edge to the previous node and increment the visitied count
                                    KeyValuePair<Node, Edge> nodeEdgePair = previousNode.Neighbours.Single(x => x.Key != null && x.Key.Equals(n));
                                    nodeEdgePair.Value.Visited++;

                                    if (n.Equals(newFrontNode))
                                    {
                                        break;
                                    }

                                    previousNode = n;
                                }

                                // Update forklift nodes
                                forklift.FrontNode = newFrontNode;
                                forklift.RearNode = newRearNode;

                                // Update edge the NXT is standing on
                                Database.Instance.Data.Graphs.FirstOrDefault().Value.BlockEdge(newFrontNode, newRearNode);
                            }
                            
                        }
                        else
                        {
                            Commands.PrintError("No current job - Continuing...");
                        }
                    }
                    else if(CurrentJob is TurnJob)
                    {
                        ConsoleHandler.AddMessage(MessageType.REGULAR, "Already performing turn-job... (ignoreing)");
                    }
                    else if (CurrentJob is DebugJob)
                    {
                        ConsoleHandler.AddMessage(MessageType.REGULAR, "Performing debug-job... (ignoreing debugjobs)");
                    }
                    
                    break;

                // Check if the NXT updated its status
                case TYPE_UPDATE_STATUS:
                {
                    if (dataString[0] != GetStatusByte(forklift))
                    {
                        // Tell the user what the status the NXT updated to
                        ConsoleHandler.AddMessage(MessageType.BLUETOOTH, "Received " + ConsoleHandler.DNS + "-status: " + dataString);
                    }

                    // Check what status the NXT told us
                    switch (dataString[0])
                    {
                        // The NXT was idle
                        case STATUS_IDLE:

                            // Check if the nxt just completed a job
                            if (forklift.Status == Status.BUSY && CurrentJob != null)
                            {
                                Path p = CurrentJob.GetPath();

                                // Update the visited count on traversed edges
                                Node previousNode = null;

                                foreach (Node n in p.Nodes)
                                {
                                    if (previousNode == null)
                                    {
                                        previousNode = n;
                                        continue;
                                    }

                                    // Find edge to the previous node and increment the visitied count
                                    KeyValuePair<Node, Edge> nodeEdgePair = previousNode.Neighbours.Single(x => x.Key != null && x.Key.Equals(n));
                                    nodeEdgePair.Value.Visited++;

                                    previousNode = n;
                                }

                                // Update the position of the forklift
                                if (p.Nodes.Count >= 2)
                                {
                                    Node frontNode = p.Nodes.ElementAt(p.Nodes.Count - 1);
                                    Node rearNode = p.Nodes.ElementAt(p.Nodes.Count - 2);

                                    Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
                                    f.UpdateNodes(frontNode, rearNode);
                                }

                                if (!(CurrentJob is TurnJob))
                                {
                                    Graph g = Database.Instance.Data.Graphs.FirstOrDefault().Value;

                                    // Unblock all edges
                                    for (int i = g.BlockedEdges.Count - 1; i >= 0; i--)
                                    {
                                        Tuple<Node, Node> edge = g.BlockedEdges[i];
                                        g.UnblockEdge(edge.Item1, edge.Item2);
                                    }

                                    // Decay the graph
                                    g.Decay(0.90);
                                }

                                if (CurrentJob is PalletJob)
                                {
                                    PalletJob job = (PalletJob) CurrentJob;

                                    // Check if the forklift just finished a deliver pallet job
                                    if (job.Type == PalletJobType.deliver)
                                    {
                                        Pallet pallet = forklift.Payload;

                                        // Update the location of the pallet
                                        pallet.Location = p.Nodes.Last();

                                        // Update the payload of the forklift
                                        forklift.Payload = null;
                                    }
                                    else if (job.Type == PalletJobType.fetch)
                                    {
                                        Node n = p.Nodes.Last();

                                        // Update the payload (Which will also update the location of the pallet)
                                        forklift.Payload = n.Pallet;
                                    }

                                    // Swap front and rear node
                                    forklift.UpdateNodes(forklift.RearNode, forklift.FrontNode);
                                }

                                CurrentJob = null;
                            }

                            // Update the internal status
                            forklift.Status = Status.IDLE;

                            // Check if PALL-E should get another job
                            if (CurrentJob == null && forklift.Status == Status.IDLE)
                            {
                                Job nextJob = null;

                                while ((nextJob == null || nextJob.GetPath() != null) && JobList.Count > 0)
                                {
                                    // Get the next job (job with lowest id)
                                    nextJob = JobList.Aggregate((l, r) => l.Key < r.Key ? l : r).Value;

                                    try
                                    {
                                        nextJob.GetPath();
                                        break;
                                    }
                                    catch (PathException e)
                                    {
                                        Commands.PrintError("Remove job #" + nextJob.ID() + ": '" + e.Message + "'");
                                    }
                                    catch (JobException e)
                                    {
                                        Commands.PrintError("Remove job #" + nextJob.ID() + ": '" + e.Message + "'");
                                    }

                                    Database.Instance.Data.RemoveJob(nextJob);
                                    nextJob = null;
                                }

                                // We removed all the jobs, nothing left to do!
                                if (JobList.Count < 1)
                                {
                                    break;
                                }

                                try
                                {
                                    // Test if the path can be generated
                                    nextJob.GetPath();

                                    // Tell the user what job was sent
                                    Commands.PrintSuccess("Sending Job #" + nextJob.ID() + " to PALL-E. " + (JobList.Count - 1) + " jobs left");

                                    // Send the job to the NXT
                                    SendPackageBT(nextJob.GetJobTypeBytes(), nextJob.GetBytes());
                                }
                                catch (JobException e)
                                {
                                    Commands.PrintError("Job #" + nextJob.ID() + " cancelled: '" + e.Message + "'");
                                    Database.Instance.Data.RemoveJob(nextJob);
                                }
                            }

                            break;

                        // The NXT was busy
                        case STATUS_BUSY:

                            // If the NXT was just idle, you know you have given it a job
                            if (forklift.Status == Status.IDLE)
                            {
                                // Debug jobs has highe priority
                                if(JobList.Count > 0)
                                {
                                    // Remove the job that is being executed currently
                                    CurrentJob = JobList.Aggregate((l, r) => l.Key < r.Key ? l : r).Value;
                                    Database.Instance.Data.Jobs.Remove(CurrentJob.Identifier);
                                }   
                            }

                            // Update the internal status
                            forklift.Status = Status.BUSY;
                            break;

                        case STATUS_OBSTACLE:
                            if (CurrentJob != null)
                            {
                                if (!(CurrentJob is DebugJob))
                                {

                                    if (!(CurrentJob is TurnJob))
                                    {
                                        ConsoleHandler.AddMessage(MessageType.REGULAR, "Calculating alternative path...");

                                        Database.Instance.Data.AddJob(CurrentJob);
                                        CurrentJob = new TurnJob(Database.Instance.Data.Jobs.Keys.Min() - 1);

                                        Node oldFrontNode = forklift.FrontNode;
                                        Node oldRearNode = forklift.RearNode;

                                        forklift.FrontNode = oldRearNode;
                                        forklift.RearNode = oldFrontNode;

                                        ConsoleHandler.AddMessage(MessageType.REGULAR, "Sending turn-job to PALL-E...");
                                    }
                                    else
                                    {
                                        ConsoleHandler.AddMessage(MessageType.REGULAR, "Resending turn-job to " + ConsoleHandler.DNS + "...");
                                    }

                                    // Send turn job
                                    SendPackageBT(CurrentJob.GetJobTypeBytes(), CurrentJob.GetBytes());
                                }
                                else
                                {
                                    // Send empty package (so that obstacle status will be avoided)
                                    DebugJob dj = new DebugJob(-1, "");
                                    SendPackageBT(dj.GetJobTypeBytes(), dj.GetBytes());
                                }
                            }
                            else
                            {
                                ConsoleHandler.AddMessage(MessageType.ERROR, "Tried to recover from obstacle, but there are no jobs left");
                                // throw new Exception("No current jobs");
                            }

                            // The status should not be updated if the current job is a debugjob
                            if (!(CurrentJob is DebugJob))
                            {
                                // Update the internal status
                                forklift.Status = Status.OBSTACLE;
                            }

                            break;

                        // The NXT encoutered an error
                        case STATUS_ERROR:
                            // Update the internal status
                            forklift.Status = Status.ERROR;
                            
                            // Tell the user that the NXT encountered an error
                            ConsoleHandler.AddMessage(MessageType.ERROR, ConsoleHandler.DNS + " reported an error");
                            break;

                        default:
                            forklift.Status = Status.UNKNOWN;
                            break;
                    }
                    break;
                }
            }
            return;
        }
        public override byte[] GetBytes()
        {
            Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
            Node ignore = f.RearNode;

            Path p = GetPath();

            if (p == null)
            {
                throw new NullReferenceException("Path is empty (either at destination, or impossible)");
            }

            ForkliftPath fp = new ForkliftPath(p, ignore);

            string directions = fp.getDirections();

            // We add some special instructions to handle the different job types
            // D - forklift down
            // N - move to next red tape
            // U - forklift up
            // T - Turn around
            // B - Back off small step

            // The instructions is inserted in reversed order
            if (Type == PalletJobType.fetch)
            {
                // Removes the extra N
                directions = directions.Substring(1, directions.Length - 1);

                // Add directions to fetch the pallet
                directions = directions.Insert(0, "TUNDR");
            }
            else if (Type == PalletJobType.deliver)
            {
                // Removes the extra N
                directions = directions.Substring(1, directions.Length - 1);

                // Add directions to deliver the pallet
                directions = directions.Insert(0, "TUBDNR");
            }

            return Encoding.ASCII.GetBytes(directions);
        }