public static void writeBytesToImage(BufferedImage bimg, byte[] msg, String filename, String key) throws ImageWriteException, IOException {

        int w = bimg.getWidth();
        int h = bimg.getHeight();

        //System.out.println("[Key] " + key);

        RC4 prng = new RC4(EncryptionProvider.sha256(key.getBytes()));
        Set<Integer> usedBytes = new HashSet<>();

        // Encode message length
        int length = msg.length;
        //System.out.println("[Actual length] " + length);
        byte[] lengthBytes = new byte[4];
        lengthBytes[0] = (byte) ((length >> 24) & 0xFF);
        lengthBytes[1] = (byte) ((length >> 16) & 0xFF);
        lengthBytes[2] = (byte) ((length >> 8) & 0xFF);
        lengthBytes[3] = (byte) (length & 0xFF);

        byte[] data = new byte[1 + 4 + length];

        // Add a header byte to the nanopost to be able to check validity
        byte[] header = new byte[1];
        header[0] = prng.getByte();
        
        //System.out.println("[Validation byte]: " + header[0]);
        
        // Write header byte
        System.arraycopy(header, 0, data, 0, 1);
        
        // Write length to data array
        System.arraycopy(lengthBytes, 0, data, 1, 4);

        // Write data to data array
        System.arraycopy(msg, 0, data, 5, msg.length);

        //System.out.println("[Data array] " + Arrays.toString(data));

        ByteArrayInputStream bais = new ByteArrayInputStream(data);
        BitInputStream bis = new BitInputStream(bais);

        // Write data bits as LSB for pixel bytes
        int bitNumber = 0;
        //System.out.println("[Index] ");
        byte bit;
        
        while ((bit = (byte) bis.readBits(1)) != -1 && bitNumber < data.length * 8) {
            // Get image pixel
            int index = nextPixelIndex(prng, usedBytes, h, w);

            int x = getPixelX(index, h, w);
            int y = getPixelY(index, h, w);
            int byteNumber = getPixelZ(index, h, w);

            ////System.out.print(index + " x: " + x + " y: " + y + " b: " + byteNumber + "  ");            
            
            int pixel = 0;
            try {
                pixel = bimg.getRGB(x, y);
            } catch (ArrayIndexOutOfBoundsException e) {
                System.err.println("x: " + x + " y: " + y + " z: " + byteNumber);
            }

            // Write LSB to pixel
            if (bit != 0) {
                pixel |= 1 << byteNumber * 8;
            } else {
                pixel &= ~(1 << byteNumber * 8);
            }
            
            
            //System.out.println("lsb: " + bit + " = " + ((pixel >> (8 * byteNumber)) & 0x01));            
            
            bimg.setRGB(x, y, pixel);

            bitNumber++;
        }

        try {
            File outputfile = new File(filename);
            ImageIO.write(bimg, "png", outputfile);
        } catch (IOException e) {
            throw new ImageWriteException("Error writing to file: " + e.getMessage());
        }
    }