Problem z zapisaniem tekstu w bitmapie

Co jest nie tak w poniższym kodzie? Potrzebuje zapisać tekst w bitmapie. Na pewno jest problem z jednym ifem w mainie. A raczej z funkcją read_bmp_type. Nie wiem, czy format BMP powinienem zapisywać jako P6?

#include <stdio.h>
#include <stdlib.h>

//Program killer
void die(char* reason){
  fprintf(stderr, "ERROR: %s\n", reason);
  exit(1);
}

//Returns 1 if P6, else returns 0
int read_bmp_type(FILE *fp) {
  char temp = fgetc(fp);
  if(temp == 'P' && fgetc(fp) == '6') {
    fgetc(fp);
    return 1;
  }

  return 0;
}

void skip_comments(FILE *fp) {
  char temp;
  while((temp = fgetc(fp)) == '#') {
    while(fgetc(fp) != '\n') {}
  }

  //Give back the first character of width that we took
  ungetc(temp, fp);

  return 0;
}

//Returns the width of the image
int get_width(FILE *fp) {
  int w;
  fscanf(fp, "%d", &w);
  return w;
}

//Returns the height of the image
int get_height(FILE *fp) {
  int h;
  fscanf(fp, "%d", &h);
  return h;
}
//Reads the color depth, returns 1 if 255, else returns 0
int read_color_depth(FILE *fp) {
  int c;
  fscanf(fp, "%d", &c);
  if(c == 255) {
    fgetc(fp);    //Get rid of the trailing whitespace
    return 1;
  }
  return 0;
}
void copy_header(FILE *, int, FILE *);
int get_message_length(char[]);
int message_fits(int, int, int);
int count_new_lines(FILE *);
void encode_length(FILE *, FILE *, int);
void encode_message(FILE *, FILE *, int, char *, int, int);

int main() {


  FILE *fp;
  if((fp = fopen("message.txt", "r+")) == NULL) {
    printf("Could not open file .\nAborting\n" );
    return 1;
  }

  if(read_bmp_type(fp)) {
    skip_comments(fp);

    char *myMessage = (char *)fp;
    int message_length = get_message_length(myMessage);
    int w = get_width(fp);
    int h = get_height(fp);

    if(message_fits(message_length, w, h)) {
      if(read_color_depth(fp)) {
      	FILE *fp_t = fopen("out.bmp","w");
      	int i = count_new_lines(fp);
      	copy_header(fp, i, fp_t);
        encode_length(fp, fp_t, (message_length - 8) / 8);
      	encode_message(fp, fp_t, (message_length - 8), myMessage, w, h);
      	printf("Encoding Process Complete. Take a look at out.ppm\n");

      	fclose(fp);
      	fclose(fp_t);
      } else {
	printf("\nError: Image color depth invalid. Must be 255.\n");
	return 1;
      }
    } else {
      printf("\nError: Image file is not large enough to hold the message.\nAborting.\n");
      return 1;
    }
  } else {
    printf("Error: Wrong file format.\nAborting\n");
    return 1;
  }

  return 0;
}

void copy_header(FILE *fp1, int numLines, FILE *fp2) {
  int i;
  char temp;

  rewind(fp1); //Goes back to the beginning of the file

  for(i = 0; i < numLines; i++) {
    while((temp = fgetc(fp1)) != EOF && temp != '\n') {
      fputc(temp, fp2);
    }
    fputc('\n', fp2);
  }

  return;
}
int get_message_length(char my_msg[]) {
  int i = 0;
  while(my_msg[i] != '\0') {
    i++;
  }
  return i * 8 + 8;
}
int message_fits(int length, int w, int h) {
  return length < w * h * 3;
}
int count_new_lines(FILE *fp) {
  char temp; int count = 0;

  rewind(fp);

  while((temp = fgetc(fp)) != EOF)
    if(temp == '\n')
      count++;

  return count;
}

void encode_length(FILE *in, FILE *out, int length) {
  char temp;
  int i, l;
  for(i = 0; i < 8; i++) {
    temp = fgetc(in);
      l = length;
      l >>= 7 - i;
      if((l & 1) == 1) {
      	if((temp & 1) == 0) {
      	  temp++;
	       }
      } else {
      	if((temp & 1) == 1) {
      	  temp--;
      	}
      }
    fputc(temp, out);
  }

  return;
}

void encode_message(FILE *in, FILE *out, int num_to_encode, char* my_message, int w, int h) {
  int encoded = 0;
  unsigned char temp;
  int idx = 0, num_coppied = 0;
  char current;

  int fileSize = (w * h * 3) - 8; //Number of bytes after first 8
  int i;

  for(i = 0; i < fileSize; i++) {
    temp = fgetc(in);
    current = my_message[idx];

    current >>= 7 - num_coppied;
    num_coppied++;

    if(encoded <= num_to_encode) {
      encoded++;
      if((current & 1) == 1) {
      	if((temp & 1) == 0) {
      	  temp++;
      	}
      } else {
      	if((temp & 1) == 1) {
      	  temp--;
      	}
      }
      if(num_coppied == 8) {
      	idx++;
      	num_coppied = 0;
      }
    }

    fputc(temp, out);
  }

  return 0;
}