~jacqueline/neon-genesis-ovengeleon

903cc6a0bc65fbe059bb465a99f9bc138e635b4a — jacqueline 6 months ago c7ac826
squeeky clean

 - fix indentation
 - debug stuff behind a build flag
1 files changed, 813 insertions(+), 810 deletions(-)

M src/main.cpp
M src/main.cpp => src/main.cpp +813 -810
@@ 14,6 14,8 @@

#include <pico/util/queue.h>

//#define DEBUG

#define DISPLAY_CS (17)
#define DISPLAY_DC (16)
#define DISPLAY_SCLK (18)


@@ 51,55 53,55 @@ Adafruit_MAX31855 thermocouple(TEMP_CLK, TEMP_CS, TEMP_DO);

class NoArgsType {};
class RectType {
	public:
		int x,y,w,h;
		uint16_t color;
  public:
    int x,y,w,h;
    uint16_t color;
};
enum Justification {
	LEFT,
	CENTER,
	RIGHT,
  LEFT,
  CENTER,
  RIGHT,
};
class TextType {
	public:
		int x,y;
		uint16_t fg_color, bg_color;
		Justification justify;
  public:
    int x,y;
    uint16_t fg_color, bg_color;
    Justification justify;
};
class CursorType {
	public:
		int x,y;
  public:
    int x,y;
};
class ConfigType {
	public:
		int textSize;
		uint16_t textColor;
		const GFXfont *font;
  public:
    int textSize;
    uint16_t textColor;
    const GFXfont *font;
};
class LineType {
	public:
		uint16_t x0,y0,x1,y1;
		uint16_t color;
  public:
    uint16_t x0,y0,x1,y1;
    uint16_t color;
};
class PixelType {
	public:
		uint16_t x,y;
		uint16_t color;
  public:
    uint16_t x,y;
    uint16_t color;
};
class DrawMessage {
	public:
		enum { CLEAR,RECT,TEXT,CURSOR,PRINT,CONFIG,LINE,PIXEL } type;
		union {
			NoArgsType nothing;
			RectType rect;
			TextType text;
			CursorType cursor;
			ConfigType config;
			LineType line;
			PixelType pixel;
		};
		// Delcared separately due to containing a string.
		string *str = nullptr;
  public:
    enum { CLEAR,RECT,TEXT,CURSOR,PRINT,CONFIG,LINE,PIXEL } type;
    union {
      NoArgsType nothing;
      RectType rect;
      TextType text;
      CursorType cursor;
      ConfigType config;
      LineType line;
      PixelType pixel;
    };
    // Delcared separately due to containing a string.
    string *str = nullptr;
};




@@ 110,14 112,14 @@ uint16_t text_bg_color = 0x0000;

// State machine
enum State {
	MAIN_MENU,
	CALIBRATE_1,
	CALIBRATE_2,
	CALIBRATE_3,
	PICK_PROFILE,
	BAKE,
	FINISHED_BAKE,
	FINISHED_CALIBRATE
  MAIN_MENU,
  CALIBRATE_1,
  CALIBRATE_2,
  CALIBRATE_3,
  PICK_PROFILE,
  BAKE,
  FINISHED_BAKE,
  FINISHED_CALIBRATE
};
State current_state = MAIN_MENU;
State next_state = MAIN_MENU;


@@ 139,10 141,10 @@ int calibration_lag_degrees = -1;

// Baking state machine
enum ReflowState {
	PREHEAT,
	SOAK,
	REFLOW,
	COOL
  PREHEAT,
  SOAK,
  REFLOW,
  COOL
};
ReflowState reflow_state = PREHEAT;



@@ 171,214 173,214 @@ int num_items = 0;
// ** UTIL FUNCTIONS ** //

void send_clear() {
	DrawMessage msg{ DrawMessage::CLEAR, NoArgsType{} };
	queue_add_blocking(&drawing_queue, &msg);
  DrawMessage msg{ DrawMessage::CLEAR, NoArgsType{} };
  queue_add_blocking(&drawing_queue, &msg);
}

void send_text(string text, int x, int y, Justification j, uint16_t fg=0xFFFF, uint16_t bg=0x0000) {
	DrawMessage msg{
		DrawMessage::TEXT,
		{
			.text=TextType{
				x, y,
				fg, bg,
				j,
			}
		},
		new string(text)
	};
	queue_add_blocking(&drawing_queue, &msg);
  DrawMessage msg{
    DrawMessage::TEXT,
      {
	.text=TextType{
	  x, y,
	  fg, bg,
	  j,
	}
      },
      new string(text)
  };
  queue_add_blocking(&drawing_queue, &msg);
}

void send_config(int textSize=1, uint16_t textColor=0xFFFF, const GFXfont *font=NULL) {
	DrawMessage msg{
		DrawMessage::CONFIG,
		{
			.config=ConfigType{
				textSize,
				textColor,
				font
			}
		}
	};
	queue_add_blocking(&drawing_queue, &msg);
  DrawMessage msg{
    DrawMessage::CONFIG,
      {
	.config=ConfigType{
	  textSize,
	  textColor,
	  font
	}
      }
  };
  queue_add_blocking(&drawing_queue, &msg);
}

void send_rect(int x, int y, int w, int h, uint16_t color) {
	DrawMessage msg{
		DrawMessage::RECT,
		{
			.rect=RectType{x,y,w,h,color}
		}
	};
	queue_add_blocking(&drawing_queue, &msg);
  DrawMessage msg{
    DrawMessage::RECT,
      {
	.rect=RectType{x,y,w,h,color}
      }
  };
  queue_add_blocking(&drawing_queue, &msg);
}

void send_print(string text, int x=-1, int y=-1) {
	if (x != -1 && y != -1) {
		DrawMessage msg{
			DrawMessage::CURSOR,
			{
				.cursor=CursorType{x, y}
			}
		};
		queue_add_blocking(&drawing_queue, &msg);
  if (x != -1 && y != -1) {
    DrawMessage msg{
      DrawMessage::CURSOR,
	{
	  .cursor=CursorType{x, y}
	}

	DrawMessage msg{
		DrawMessage::PRINT,
		{ NoArgsType{} },
		new string(text)
	};
	queue_add_blocking(&drawing_queue, &msg);
    };
    queue_add_blocking(&drawing_queue, &msg);
  }

  DrawMessage msg{
    DrawMessage::PRINT,
      { NoArgsType{} },
      new string(text)
  };
  queue_add_blocking(&drawing_queue, &msg);
}

void send_line(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t color) {
	DrawMessage msg{
		DrawMessage::LINE,
		{
			.line=LineType{x0,y0,x1,y1,color}
		}
	};
	queue_add_blocking(&drawing_queue, &msg);
  DrawMessage msg{
    DrawMessage::LINE,
      {
	.line=LineType{x0,y0,x1,y1,color}
      }
  };
  queue_add_blocking(&drawing_queue, &msg);
}

void send_pixel(uint16_t x, uint16_t y, uint16_t color) {
	DrawMessage msg{
		DrawMessage::PIXEL,
		{
			.pixel=PixelType{x,y,color}
		}
	};
	queue_add_blocking(&drawing_queue, &msg);
  DrawMessage msg{
    DrawMessage::PIXEL,
      {
	.pixel=PixelType{x,y,color}
      }
  };
  queue_add_blocking(&drawing_queue, &msg);
}

string get_time_string(unsigned long millis) {
	ostringstream str;
	str << std::setfill('0') << std::setw(2) << millis / 1000 / 60;
	str << ":";
	str << std::setfill('0') << std::setw(2) << millis / 1000 % 60;
	return str.str();
  ostringstream str;
  str << std::setfill('0') << std::setw(2) << millis / 1000 / 60;
  str << ":";
  str << std::setfill('0') << std::setw(2) << millis / 1000 % 60;
  return str.str();
}

void draw_temperature() {
	string temp_sign = "";
	if (current_temp != -1 && last_temp != -1) {
		if (current_temp > last_temp) temp_sign = "+++";
		if (current_temp < last_temp) temp_sign = "---";
	}

	ostringstream temp_string;
	temp_string << "TEMP: ";
	if (current_temp == -1) temp_string << "???";
	else temp_string << current_temp;
	temp_string << "C " << temp_sign;
	string text = temp_string.str();

	int x = (320 / 2);
	int y = 240 - HEADER_FOOTER_SIZE + 2;

	send_config();
	send_text(text, x, y, CENTER, 0xFFFF, current_temp_color);
  string temp_sign = "";
  if (current_temp != -1 && last_temp != -1) {
    if (current_temp > last_temp) temp_sign = "+++";
    if (current_temp < last_temp) temp_sign = "---";
  }

  ostringstream temp_string;
  temp_string << "TEMP: ";
  if (current_temp == -1) temp_string << "???";
  else temp_string << current_temp;
  temp_string << "C " << temp_sign;
  string text = temp_string.str();

  int x = (320 / 2);
  int y = 240 - HEADER_FOOTER_SIZE + 2;

  send_config();
  send_text(text, x, y, CENTER, 0xFFFF, current_temp_color);
}

void draw_header() {
	uint16_t color_fg = ST77XX_WHITE;
	uint16_t color_bg = current_temp_color;
	text_bg_color = color_bg;

	send_rect(0, 0, 320, HEADER_FOOTER_SIZE, color_bg);
	send_config();

	string l_action = "";
	switch (current_state) {
		case MAIN_MENU:
		case PICK_PROFILE:
			l_action = "PICK";
			break;
		case FINISHED_BAKE:
		case FINISHED_CALIBRATE:
			l_action = "DONE";
			break;
		default:
			break;
	}
	send_text(l_action, 0, 2, LEFT, color_fg, color_bg);

	string title = "";
	switch (current_state) {
		case MAIN_MENU:
			title = "";
			break;
		case CALIBRATE_1:
		case CALIBRATE_2:
		case CALIBRATE_3:
			title = "CALIBRATING";
			break;
		case PICK_PROFILE:
			title = "PROFILE?";
			break;
		case BAKE:
			title = "REFLOWING";
			// TODO: use profile name?
			break;
		case FINISHED_BAKE:
		case FINISHED_CALIBRATE:
			title = "FINISHED";
			break;
		default:
			break;
	}
	send_text(title, 320 / 2, 2, CENTER, color_fg, color_bg);

	string r_action = "";
	switch (current_state) {
		case MAIN_MENU:
		case PICK_PROFILE:
			r_action = "UP";
			break;
		default:
			break;
	}
	send_text(r_action, 320, 2, RIGHT, color_fg, color_bg);
  uint16_t color_fg = ST77XX_WHITE;
  uint16_t color_bg = current_temp_color;
  text_bg_color = color_bg;

  send_rect(0, 0, 320, HEADER_FOOTER_SIZE, color_bg);
  send_config();

  string l_action = "";
  switch (current_state) {
    case MAIN_MENU:
    case PICK_PROFILE:
      l_action = "PICK";
      break;
    case FINISHED_BAKE:
    case FINISHED_CALIBRATE:
      l_action = "DONE";
      break;
    default:
      break;
  }
  send_text(l_action, 0, 2, LEFT, color_fg, color_bg);

  string title = "";
  switch (current_state) {
    case MAIN_MENU:
      title = "";
      break;
    case CALIBRATE_1:
    case CALIBRATE_2:
    case CALIBRATE_3:
      title = "CALIBRATING";
      break;
    case PICK_PROFILE:
      title = "PROFILE?";
      break;
    case BAKE:
      title = "REFLOWING";
      // TODO: use profile name?
      break;
    case FINISHED_BAKE:
    case FINISHED_CALIBRATE:
      title = "FINISHED";
      break;
    default:
      break;
  }
  send_text(title, 320 / 2, 2, CENTER, color_fg, color_bg);

  string r_action = "";
  switch (current_state) {
    case MAIN_MENU:
    case PICK_PROFILE:
      r_action = "UP";
      break;
    default:
      break;
  }
  send_text(r_action, 320, 2, RIGHT, color_fg, color_bg);
}

void draw_footer() {
	uint16_t color_fg = ST77XX_WHITE;
	uint16_t color_bg = current_temp_color;
	text_bg_color = color_bg;

	send_rect(0, 240 - HEADER_FOOTER_SIZE, 320, HEADER_FOOTER_SIZE, color_bg);
	send_config();

	string l_action = "";
	switch (current_state) {
		case CALIBRATE_1:
		case CALIBRATE_2:
		case CALIBRATE_3:
		case BAKE:
			l_action = "CANCEL";
			break;
		case PICK_PROFILE:
			l_action = "BACK";
			break;
		default:
			break;
	}
	send_text(l_action, 0, 240 - HEADER_FOOTER_SIZE + 2, LEFT, color_fg, color_bg);

	string r_action = "";
	switch (current_state) {
		case MAIN_MENU:
		case PICK_PROFILE:
			r_action = "DOWN";
			break;
		default:
			break;
	}
	send_text(r_action, 320, 240 - HEADER_FOOTER_SIZE + 2, RIGHT, color_fg, color_bg);

	draw_temperature();
  uint16_t color_fg = ST77XX_WHITE;
  uint16_t color_bg = current_temp_color;
  text_bg_color = color_bg;

  send_rect(0, 240 - HEADER_FOOTER_SIZE, 320, HEADER_FOOTER_SIZE, color_bg);
  send_config();

  string l_action = "";
  switch (current_state) {
    case CALIBRATE_1:
    case CALIBRATE_2:
    case CALIBRATE_3:
    case BAKE:
      l_action = "CANCEL";
      break;
    case PICK_PROFILE:
      l_action = "BACK";
      break;
    default:
      break;
  }
  send_text(l_action, 0, 240 - HEADER_FOOTER_SIZE + 2, LEFT, color_fg, color_bg);

  string r_action = "";
  switch (current_state) {
    case MAIN_MENU:
    case PICK_PROFILE:
      r_action = "DOWN";
      break;
    default:
      break;
  }
  send_text(r_action, 320, 240 - HEADER_FOOTER_SIZE + 2, RIGHT, color_fg, color_bg);

  draw_temperature();
}

bool test_elements_state = false;


@@ 386,92 388,93 @@ int test_temperature = 24;
unsigned long last_temp_time = 0;

void set_elements_state(bool on_or_off) {
	test_elements_state = on_or_off;
	if (on_or_off) {
		digitalWrite(TOP_ELEMENT, HIGH);
		digitalWrite(BOTTOM_ELEMENT, HIGH);
	} else {
		digitalWrite(TOP_ELEMENT, LOW);
		digitalWrite(BOTTOM_ELEMENT, LOW);
	}
  test_elements_state = on_or_off;
  if (on_or_off) {
#ifndef DEBUG
    digitalWrite(TOP_ELEMENT, HIGH);
    digitalWrite(BOTTOM_ELEMENT, HIGH);
#endif
  } else {
    digitalWrite(TOP_ELEMENT, LOW);
    digitalWrite(BOTTOM_ELEMENT, LOW);
  }
}

uint16_t get_temperature_color() {
	if (current_temp == -1 || last_temp == -1) {
		return 0x0000;
	}
	if (current_temp > TEMPERATURE_HOT) {
		return 0x8082;
	}
	if (current_temp > TEMPERATURE_WARM) {
		return 0xBDA3;
	}
	return 0x1423;
  if (current_temp == -1 || last_temp == -1) {
    return 0x0000;
  }
  if (current_temp > TEMPERATURE_HOT) {
    return 0x8082;
  }
  if (current_temp > TEMPERATURE_WARM) {
    return 0xBDA3;
  }
  return 0x1423;
}

void update_temperature() {
	last_temp = current_temp;

	double raw_value = thermocouple.readCelsius();
	if (isnan(raw_value)) {
		current_temp = -1;
	} else {
		current_temp = (int) (raw_value + 0.5);
	}

	/*
	unsigned long t = millis();
	if (t - last_temp_time > 250) {
		if (test_elements_state) {
			test_temperature++;
		} else if (test_temperature > 24) {
			test_temperature--;
		}
		last_temp_time = t;
	}
	current_temp = test_temperature;
	*/
  last_temp = current_temp;
#ifndef DEBUG
  double raw_value = thermocouple.readCelsius();
  if (isnan(raw_value)) {
    current_temp = -1;
  } else {
    current_temp = (int) (raw_value + 0.5);
  }
#else
  unsigned long t = millis();
  if (t - last_temp_time > 250) {
    if (test_elements_state) {
      test_temperature++;
    } else if (test_temperature > 24) {
      test_temperature--;
    }
    last_temp_time = t;
  }
  current_temp = test_temperature;
#endif
}

void main_menu_setup() {
	set_elements_state(false);
  set_elements_state(false);

	send_config(1, 0xFFFF, &FreeSerif18pt7b);
	send_print("NEON\nGENESIS\nOVENGELION", 0, 50);
  send_config(1, 0xFFFF, &FreeSerif18pt7b);
  send_print("NEON\nGENESIS\nOVENGELION", 0, 50);

	send_config();
	if (is_calibrated) {
		send_text("CALIBRATION OK", 320, 220, RIGHT, 0x0000, 0x0F00);
	} else {
		send_text("NO CALIBRATION", 320, 220, RIGHT, 0x0000, 0xF000);
	}
  send_config();
  if (is_calibrated) {
    send_text("CALIBRATION OK", 320, 220, RIGHT, 0x0000, 0x0F00);
  } else {
    send_text("NO CALIBRATION", 320, 220, RIGHT, 0x0000, 0xF000);
  }

	num_items = 2;
  num_items = 2;
}

void main_menu_loop() {
	send_config(2);
  send_config(2);

	if (last_drawn_selection != selection) {
		int fg, bg;
  if (last_drawn_selection != selection) {
    int fg, bg;

		if (selection == 0) { fg=0x0000; bg=0xFFFF; }
		else { fg=0xFFFF; bg=0x0000; }
		send_text("BAKE", 320 / 2, 165, CENTER, fg, bg);
    if (selection == 0) { fg=0x0000; bg=0xFFFF; }
    else { fg=0xFFFF; bg=0x0000; }
    send_text("BAKE", 320 / 2, 165, CENTER, fg, bg);

		if (selection == 1) { fg=0x0000; bg=0xFFFF; }
		else { fg=0xFFFF; bg=0x0000; }
		send_text("CALIBRATE", 320 / 2, 195, CENTER, fg, bg);
    if (selection == 1) { fg=0x0000; bg=0xFFFF; }
    else { fg=0xFFFF; bg=0x0000; }
    send_text("CALIBRATE", 320 / 2, 195, CENTER, fg, bg);

		last_drawn_selection = selection;
	}
    last_drawn_selection = selection;
  }
}

void calibrate_1_setup() {
	calibrate_1_start_time = millis();
  calibrate_1_start_time = millis();

	send_config(2);
	send_print("STAGE 1: HEATING to 240C", 0, 20);
  send_config(2);
  send_print("STAGE 1: HEATING to 240C", 0, 20);
}

/**


@@ 479,37 482,37 @@ void calibrate_1_setup() {
 * temperature is high.
 */
void calibrate_1_loop() {
	if (current_temp == -1) {
		// Wait for the temperature to be available. Shouldn't normally
		// happen.
		return;
	}

	// Keep the elements on until we get to a reasonable reflow temp.
	set_elements_state(true);

	unsigned long current_time = millis();
	if (current_temp >= 240) {
		calibrate_2_start_time = current_time;
		calibration_lag_degrees = current_temp;
		next_state = CALIBRATE_2;
		return;
	}

	if (last_drawn_time == 0 || current_time - last_drawn_time >= 1000) {
		send_config(3);
		send_text(get_time_string(current_time - calibrate_1_start_time),
				320 / 2,
				240 / 2,
				CENTER);
		last_drawn_time = current_time;
	}
  if (current_temp == -1) {
    // Wait for the temperature to be available. Shouldn't normally
    // happen.
    return;
  }

  // Keep the elements on until we get to a reasonable reflow temp.
  set_elements_state(true);

  unsigned long current_time = millis();
  if (current_temp >= 240) {
    calibrate_2_start_time = current_time;
    calibration_lag_degrees = current_temp;
    next_state = CALIBRATE_2;
    return;
  }

  if (last_drawn_time == 0 || current_time - last_drawn_time >= 1000) {
    send_config(3);
    send_text(get_time_string(current_time - calibrate_1_start_time),
	320 / 2,
	240 / 2,
	CENTER);
    last_drawn_time = current_time;
  }
}

void calibrate_2_setup() {
	// Start time was set by stage 1 already.
	send_config(2);
	send_print("STAGE 2: WAIT FOR COOL", 0, 20);
  // Start time was set by stage 1 already.
  send_config(2);
  send_print("STAGE 2: WAIT FOR COOL", 0, 20);
}

/**


@@ 518,34 521,34 @@ void calibrate_2_setup() {
 * we end up reaching.
 */
void calibrate_2_loop() {
	// Disable both heaters.
	set_elements_state(false);

	unsigned long current_time = millis();
	if (last_temp > current_temp) {
		// Temperature is falling! Record things.
		calibration_cool_lag_time = current_time - calibrate_2_start_time;
		calibration_lag_degrees = last_temp - calibration_lag_degrees;

		calibrate_3_start_time = current_time;
		next_state = CALIBRATE_3 ;
		return;
	}

	if (last_drawn_time == 0 || current_time - last_drawn_time >= 1000) {
		send_config(3);
		send_text( get_time_string(current_time - calibrate_2_start_time),
				320 / 2,
				240 / 2,
				CENTER);
		last_drawn_time = current_time;
	}
  // Disable both heaters.
  set_elements_state(false);

  unsigned long current_time = millis();
  if (last_temp > current_temp) {
    // Temperature is falling! Record things.
    calibration_cool_lag_time = current_time - calibrate_2_start_time;
    calibration_lag_degrees = last_temp - calibration_lag_degrees;

    calibrate_3_start_time = current_time;
    next_state = CALIBRATE_3 ;
    return;
  }

  if (last_drawn_time == 0 || current_time - last_drawn_time >= 1000) {
    send_config(3);
    send_text( get_time_string(current_time - calibrate_2_start_time),
	320 / 2,
	240 / 2,
	CENTER);
    last_drawn_time = current_time;
  }
}

void calibrate_3_setup() {
	// Start time was set by stage 3 already.
	send_config(2);
	send_print("STAGE 3: WAIT FOR REHEAT", 0, 20);
  // Start time was set by stage 3 already.
  send_config(2);
  send_print("STAGE 3: WAIT FOR REHEAT", 0, 20);
}

/**


@@ 553,533 556,533 @@ void calibrate_3_setup() {
 * how long until the temperature starts rising again.
 */
void calibrate_3_loop() {
	// Enable both heaters.
	set_elements_state(true);

	unsigned long current_time = millis();
	if (last_temp < current_temp) {
		// Temperature is rising!
		calibration_heat_lag_time = current_time - calibrate_3_start_time;

		next_state = FINISHED_CALIBRATE;
		return;
	}

	if (last_drawn_time == 0 || current_time - last_drawn_time >= 1000) {
		send_config(3);
		send_text( get_time_string(current_time - calibrate_3_start_time),
				320 / 2,
				240 / 2,
				CENTER);
		last_drawn_time = current_time;
	}
  // Enable both heaters.
  set_elements_state(true);

  unsigned long current_time = millis();
  if (last_temp < current_temp) {
    // Temperature is rising!
    calibration_heat_lag_time = current_time - calibrate_3_start_time;

    next_state = FINISHED_CALIBRATE;
    return;
  }

  if (last_drawn_time == 0 || current_time - last_drawn_time >= 1000) {
    send_config(3);
    send_text( get_time_string(current_time - calibrate_3_start_time),
	320 / 2,
	240 / 2,
	CENTER);
    last_drawn_time = current_time;
  }
}

void finished_calibrate_setup() {
	set_elements_state(false);

	unsigned long current_time = millis();

	send_config(2);
	send_print("CALIBRATION COMPLETE!\n", 0, 20);
	send_print("TOTAL TIME: ");
	send_print(get_time_string(current_time - calibrate_1_start_time));
	send_print("\nCOOL LAG TIME: ");
	send_print(get_time_string(calibration_cool_lag_time));
	send_print("\nHEAT LAG TIME: ");
	send_print(get_time_string(calibration_heat_lag_time));
	send_print("\nLAG DEGREES: ");
	send_print(std::to_string(calibration_lag_degrees));

	send_print("\nWRITING TO FLASH... ");
	
	bool r = LittleFS.begin();
	if (!r) {
		digitalWrite(LED_BLUE, LOW);
	} else {
		File f = LittleFS.open("CALIBRATION", "w");
		if (!f) {
			digitalWrite(LED_RED, LOW);
		} else {
			f.println(calibration_cool_lag_time);
			f.println(calibration_heat_lag_time);
			f.println(calibration_lag_degrees);
			f.close();
		}
		LittleFS.end();
	}

	// Hooray! :)
	is_calibrated = true;
	send_print("OK!");
  set_elements_state(false);

  unsigned long current_time = millis();

  send_config(2);
  send_print("CALIBRATION COMPLETE!\n", 0, 20);
  send_print("TOTAL TIME: ");
  send_print(get_time_string(current_time - calibrate_1_start_time));
  send_print("\nCOOL LAG TIME: ");
  send_print(get_time_string(calibration_cool_lag_time));
  send_print("\nHEAT LAG TIME: ");
  send_print(get_time_string(calibration_heat_lag_time));
  send_print("\nLAG DEGREES: ");
  send_print(std::to_string(calibration_lag_degrees));

  send_print("\nWRITING TO FLASH... ");

  bool r = LittleFS.begin();
  if (!r) {
    digitalWrite(LED_BLUE, LOW);
  } else {
    File f = LittleFS.open("CALIBRATION", "w");
    if (!f) {
      digitalWrite(LED_RED, LOW);
    } else {
      f.println(calibration_cool_lag_time);
      f.println(calibration_heat_lag_time);
      f.println(calibration_lag_degrees);
      f.close();
    }
    LittleFS.end();
  }

  // Hooray! :)
  is_calibrated = true;
  send_print("OK!");
}

int get_desired_temperature(ReflowState state, unsigned long time_in_state) {
	// Easy states first.
	if (state == PREHEAT) {
		return preheat_temp;
	}
	if (state == COOL) {
		return 0;
	}

	int start = 0;
	int end = 0;
	long duration = 1;
	switch (state) {
		case SOAK:
			start = preheat_temp;
			end = soak_temp;
			duration = soak_duration;
			break;
		case REFLOW:
			start = soak_temp;
			end = reflow_temp;
			duration = reflow_duration;
			break;
		default:
			break;
	}

	double progress = time_in_state / (float) duration;
	double desired_temp = ((end - start) * progress) + start;
	// Safety first!
	if (desired_temp > end) desired_temp = end;

	return (int) (desired_temp + 0.5);
  // Easy states first.
  if (state == PREHEAT) {
    return preheat_temp;
  }
  if (state == COOL) {
    return 0;
  }

  int start = 0;
  int end = 0;
  long duration = 1;
  switch (state) {
    case SOAK:
      start = preheat_temp;
      end = soak_temp;
      duration = soak_duration;
      break;
    case REFLOW:
      start = soak_temp;
      end = reflow_temp;
      duration = reflow_duration;
      break;
    default:
      break;
  }

  double progress = time_in_state / (float) duration;
  double desired_temp = ((end - start) * progress) + start;
  // Safety first!
  if (desired_temp > end) desired_temp = end;

  return (int) (desired_temp + 0.5);
}

void bake_setup() {
	int graph_width = 280;
	int graph_height = 140;
	int graph_x = 20;
	int graph_y = 80;

	// First draw the axises!
	send_line(graph_x-1,graph_y+graph_height+1,graph_x+graph_width+1,graph_y+graph_height+1,0xF000);
	send_line(graph_x-1,graph_y-1,graph_x-1,graph_y+graph_height+1,0xF000);

	// Now draw the pretty ideal curve!
	unsigned long total_time =
		preheat_duration +
		soak_duration +
		reflow_duration +
		cool_duration;
	for (int x = graph_x; x < graph_x + graph_width; x++) {
		double progress = (x - graph_x) / (float) graph_width;
		unsigned long current_time = total_time * progress;

		int desired_temp;
		if (current_time < preheat_duration) {
			progress = current_time / (float) preheat_duration;
			desired_temp = ((preheat_temp - current_temp) * progress) + current_temp;
		} else if (current_time > total_time - cool_duration) {
			unsigned long preheat_start = total_time - cool_duration;
			progress = 1 - ((current_time - preheat_start) / (float) preheat_duration);
			desired_temp = ((reflow_temp - current_temp) * progress) + current_temp;
		} else {
			unsigned long time_in_state = current_time - preheat_duration;
			ReflowState state = SOAK;
			if (time_in_state > soak_duration) {
				time_in_state -= soak_duration;
				state = REFLOW;
			}
			desired_temp = get_desired_temperature(state, time_in_state);
		}
		double scaled_temp = ((float) graph_height / 275) * desired_temp;
		int y = graph_y + graph_height - scaled_temp;
		send_pixel(x, y, 0xFFFF);
	}
  int graph_width = 280;
  int graph_height = 140;
  int graph_x = 20;
  int graph_y = 80;

  // First draw the axises!
  send_line(graph_x-1,graph_y+graph_height+1,graph_x+graph_width+1,graph_y+graph_height+1,0xF000);
  send_line(graph_x-1,graph_y-1,graph_x-1,graph_y+graph_height+1,0xF000);

  // Now draw the pretty ideal curve!
  unsigned long total_time =
    preheat_duration +
    soak_duration +
    reflow_duration +
    cool_duration;
  for (int x = graph_x; x < graph_x + graph_width; x++) {
    double progress = (x - graph_x) / (float) graph_width;
    unsigned long current_time = total_time * progress;

    int desired_temp;
    if (current_time < preheat_duration) {
      progress = current_time / (float) preheat_duration;
      desired_temp = ((preheat_temp - current_temp) * progress) + current_temp;
    } else if (current_time > total_time - cool_duration) {
      unsigned long preheat_start = total_time - cool_duration;
      progress = 1 - ((current_time - preheat_start) / (float) preheat_duration);
      desired_temp = ((reflow_temp - current_temp) * progress) + current_temp;
    } else {
      unsigned long time_in_state = current_time - preheat_duration;
      ReflowState state = SOAK;
      if (time_in_state > soak_duration) {
	time_in_state -= soak_duration;
	state = REFLOW;
      }
      desired_temp = get_desired_temperature(state, time_in_state);
    }
    double scaled_temp = ((float) graph_height / 275) * desired_temp;
    int y = graph_y + graph_height - scaled_temp;
    send_pixel(x, y, 0xFFFF);
  }
}

void pick_profile_loop() {
	// TODO: redraw items if selection changed
  // TODO: redraw items if selection changed
}

void reflow_loop() {
	unsigned long current_time = millis();
	switch (reflow_state) {
		case PREHEAT:
			if (current_temp > preheat_temp) {
				reflow_state = SOAK;
				reflow_state_start_time = current_time;
				return;
			}
			break;
		case SOAK:
			if (current_time - reflow_state_start_time > soak_duration) {
				reflow_state = REFLOW;
				reflow_state_start_time = current_time;
				return;
			}
			break;
		case REFLOW:
			if (reflow_rise_time == 0) {
				if (current_temp >= reflow_temp - calibration_lag_degrees) {
					reflow_rise_time = current_time - reflow_state_start_time;
				}
			} else {
				// The reflow stage is defined as a duration above the soak
				// temp, with a peak of the reflow temp. Therefore, once we've
				// hit our peak and are at least the rise time through our
				// desired temp, we transition to COOL.
				// This ensures that we hit our peak, whilst also keeping things
				// hot for the amount of time we're after.
				if (reflow_duration - (current_time - reflow_state_start_time) <= reflow_rise_time
						// Alternatively, if we've completely blown through our
						// duration without ever hitting the peak, then just
						// kill the elements so that we don't risk damaging
						// anything.
						|| current_time - reflow_state_start_time > reflow_duration) {
					reflow_state = COOL;
					reflow_state_start_time = current_time;
				}
			}
			break;
		case COOL:
			if (current_temp < 150) {
				next_state = FINISHED_BAKE;
				return;
			}
			break;
  unsigned long current_time = millis();
  switch (reflow_state) {
    case PREHEAT:
      if (current_temp > preheat_temp) {
	reflow_state = SOAK;
	reflow_state_start_time = current_time;
	return;
      }
      break;
    case SOAK:
      if (current_time - reflow_state_start_time > soak_duration) {
	reflow_state = REFLOW;
	reflow_state_start_time = current_time;
	return;
      }
      break;
    case REFLOW:
      if (reflow_rise_time == 0) {
	if (current_temp >= reflow_temp - calibration_lag_degrees) {
	  reflow_rise_time = current_time - reflow_state_start_time;
	}

	unsigned long time_in_state = current_time - reflow_state_start_time;
	int desired_temp = get_desired_temperature(reflow_state, time_in_state);

	if (desired_temp - current_temp < calibration_lag_degrees && holding_at_time == -1) {
		// We are within lag_temp of the temperature we should be aiming for.
		// Time to start turning the elements off!
		holding_at_temp = desired_temp;
		holding_at_time = current_time;
	}

	if (holding_at_temp == desired_temp) {
		// We are currently holding the elements off. But what's the state of
		// things?
		if (current_temp > desired_temp) {
			// We overshot! Keep holding the elements off.
			set_elements_state(false);
		} else if (current_temp < desired_temp - calibration_lag_degrees) {
			// We have held off for too long. This hopefully shouldn't happen,
			// but if it does we immediately stop holding.
			holding_at_temp = -1;
			holding_at_time = -1;
			holding_at_reheat_time = -1;
			set_elements_state(true);
		} else if (current_time - holding_at_time >= calibration_cool_lag_time
				|| last_temp > current_temp) {
			// We have been holding the element off for long enough that it
			// should be dropping very soon, or temp is already dropping.
			if (holding_at_reheat_time == -1) {
				// Turn the elements back on for a bit.
				set_elements_state(true);
				holding_at_reheat_time = current_time;
			} else if (current_time - holding_at_reheat_time < calibration_heat_lag_time) {
				// We have held them on for long enough. Turn them back off,
				// and begin the cycle again.
				set_elements_state(false);
				holding_at_time = current_time;
				holding_at_reheat_time = -1;
			}
		}
		// Otherwise, we are holding the element off and everything looks okay.
		// Stay the course.
      } else {
	// The reflow stage is defined as a duration above the soak
	// temp, with a peak of the reflow temp. Therefore, once we've
	// hit our peak and are at least the rise time through our
	// desired temp, we transition to COOL.
	// This ensures that we hit our peak, whilst also keeping things
	// hot for the amount of time we're after.
	if (reflow_duration - (current_time - reflow_state_start_time) <= reflow_rise_time
	    // Alternatively, if we've completely blown through our
	    // duration without ever hitting the peak, then just
	    // kill the elements so that we don't risk damaging
	    // anything.
	    || current_time - reflow_state_start_time > reflow_duration) {
	  reflow_state = COOL;
	  reflow_state_start_time = current_time;
	}
      }
      break;
    case COOL:
      if (current_temp < 150) {
	next_state = FINISHED_BAKE;
	return;
      }
      break;
  }

  unsigned long time_in_state = current_time - reflow_state_start_time;
  int desired_temp = get_desired_temperature(reflow_state, time_in_state);

  if (desired_temp - current_temp < calibration_lag_degrees && holding_at_time == -1) {
    // We are within lag_temp of the temperature we should be aiming for.
    // Time to start turning the elements off!
    holding_at_temp = desired_temp;
    holding_at_time = current_time;
  }

  if (holding_at_temp == desired_temp) {
    // We are currently holding the elements off. But what's the state of
    // things?
    if (current_temp > desired_temp) {
      // We overshot! Keep holding the elements off.
      set_elements_state(false);
    } else if (current_temp < desired_temp - calibration_lag_degrees) {
      // We have held off for too long. This hopefully shouldn't happen,
      // but if it does we immediately stop holding.
      holding_at_temp = -1;
      holding_at_time = -1;
      holding_at_reheat_time = -1;
      set_elements_state(true);
    } else if (current_time - holding_at_time >= calibration_cool_lag_time
	|| last_temp > current_temp) {
      // We have been holding the element off for long enough that it
      // should be dropping very soon, or temp is already dropping.
      if (holding_at_reheat_time == -1) {
	// Turn the elements back on for a bit.
	set_elements_state(true);
	holding_at_reheat_time = current_time;
      } else if (current_time - holding_at_reheat_time < calibration_heat_lag_time) {
	// We have held them on for long enough. Turn them back off,
	// and begin the cycle again.
	set_elements_state(false);
	holding_at_time = current_time;
	holding_at_reheat_time = -1;
      }
    }
    // Otherwise, we are holding the element off and everything looks okay.
    // Stay the course.
  }
}

bool read_debounced(int pin) {
	delay(10);
	return digitalRead(pin);
  delay(10);
  return digitalRead(pin);
}

void top_left_pushed() {
	if (read_debounced(BUTTON_TOP_LEFT)) return;
	switch (current_state) {
		case MAIN_MENU:
			if (selection == 0) {
				next_state = PICK_PROFILE;
			}
			if (selection == 1) {
				next_state = CALIBRATE_1;
			}
			break;
		case FINISHED_BAKE:
		case FINISHED_CALIBRATE:
			next_state = MAIN_MENU;
			break;
		default:
			break;
	}
  if (read_debounced(BUTTON_TOP_LEFT)) return;
  switch (current_state) {
    case MAIN_MENU:
      if (selection == 0) {
	next_state = PICK_PROFILE;
      }
      if (selection == 1) {
	next_state = CALIBRATE_1;
      }
      break;
    case FINISHED_BAKE:
    case FINISHED_CALIBRATE:
      next_state = MAIN_MENU;
      break;
    default:
      break;
  }
}

void top_right_pushed() {
	if (read_debounced(BUTTON_TOP_RIGHT)) return;
	switch (current_state) {
		case MAIN_MENU:
		case PICK_PROFILE:
			selection = (selection + num_items - 1) % num_items;
			break;
		default:
			break;
	}
  if (read_debounced(BUTTON_TOP_RIGHT)) return;
  switch (current_state) {
    case MAIN_MENU:
    case PICK_PROFILE:
      selection = (selection + num_items - 1) % num_items;
      break;
    default:
      break;
  }
}

void bottom_left_pushed() {
	if (read_debounced(BUTTON_BOTTOM_LEFT)) return;
	switch (current_state) {
		case FINISHED_BAKE:
		case FINISHED_CALIBRATE:
			next_state = MAIN_MENU;
			break;
		default:
			break;
	}
  if (read_debounced(BUTTON_BOTTOM_LEFT)) return;
  switch (current_state) {
    case FINISHED_BAKE:
    case FINISHED_CALIBRATE:
      next_state = MAIN_MENU;
      break;
    default:
      break;
  }
}

void bottom_right_pushed() {
	if (read_debounced(BUTTON_BOTTOM_RIGHT)) return;
	switch (current_state) {
		case MAIN_MENU:
		case PICK_PROFILE:
			selection = (selection + 1) % num_items;
			break;
		default:
			break;
	}
  if (read_debounced(BUTTON_BOTTOM_RIGHT)) return;
  switch (current_state) {
    case MAIN_MENU:
    case PICK_PROFILE:
      selection = (selection + 1) % num_items;
      break;
    default:
      break;
  }
}

void change_state(State new_state) {
	current_state = new_state;

	send_clear();
	draw_header();
	draw_footer();
	text_bg_color = 0x0000;

	// Any menu would want to be reset anyway.
	last_drawn_selection = -1;
	selection = 0;

	unsigned long current_time = millis();
	File f;
	switch (current_state) {
		case MAIN_MENU:
			main_menu_setup();
			break;
		case PICK_PROFILE:
			bake_setup();
			break;
		case CALIBRATE_1:
			calibrate_1_setup();
			break;
		case CALIBRATE_2:
			calibrate_2_setup();
			break;
		case CALIBRATE_3:
			calibrate_3_setup();
			break;
		case FINISHED_CALIBRATE:
			finished_calibrate_setup();
			break;
		default:
			break;
	}
  current_state = new_state;

  send_clear();
  draw_header();
  draw_footer();
  text_bg_color = 0x0000;

  // Any menu would want to be reset anyway.
  last_drawn_selection = -1;
  selection = 0;

  unsigned long current_time = millis();
  File f;
  switch (current_state) {
    case MAIN_MENU:
      main_menu_setup();
      break;
    case PICK_PROFILE:
      bake_setup();
      break;
    case CALIBRATE_1:
      calibrate_1_setup();
      break;
    case CALIBRATE_2:
      calibrate_2_setup();
      break;
    case CALIBRATE_3:
      calibrate_3_setup();
      break;
    case FINISHED_CALIBRATE:
      finished_calibrate_setup();
      break;
    default:
      break;
  }
}

void setup() {
	// Ensure the elements are off immediately, for safety.
	pinMode(TOP_ELEMENT, OUTPUT);
	pinMode(BOTTOM_ELEMENT, OUTPUT);
	digitalWrite(TOP_ELEMENT, LOW);
	digitalWrite(BOTTOM_ELEMENT, LOW);

	// Next set up our multicore comms, then signal to the other core that
	// it can proceed.
	queue_init(&drawing_queue, sizeof(DrawMessage), 16);
	rp2040.fifo.push(0xDEADBEEF);

	// Now onto the rest of our init...

	pinMode(BUTTON_TOP_LEFT, INPUT_PULLUP);
	pinMode(BUTTON_TOP_RIGHT, INPUT_PULLUP);
	pinMode(BUTTON_BOTTOM_LEFT, INPUT_PULLUP);
	pinMode(BUTTON_BOTTOM_RIGHT, INPUT_PULLUP);

	attachInterrupt(digitalPinToInterrupt(BUTTON_TOP_LEFT), top_left_pushed, FALLING);
	attachInterrupt(digitalPinToInterrupt(BUTTON_TOP_RIGHT), top_right_pushed, FALLING);
	attachInterrupt(digitalPinToInterrupt(BUTTON_BOTTOM_LEFT), bottom_left_pushed, FALLING);
	attachInterrupt(digitalPinToInterrupt(BUTTON_BOTTOM_RIGHT), bottom_right_pushed, FALLING);

	pinMode(DISPLAY_BACKLIGHT_EN, OUTPUT);
	digitalWrite(DISPLAY_BACKLIGHT_EN, HIGH);

	pinMode(LED_RED, OUTPUT);
	pinMode(LED_GREEN, OUTPUT);
	pinMode(LED_BLUE, OUTPUT);
	digitalWrite(LED_RED, HIGH);
	digitalWrite(LED_GREEN, HIGH);
	digitalWrite(LED_BLUE, HIGH);

	bool r = LittleFS.begin();
	if (!r) {
		digitalWrite(LED_BLUE, LOW);
	} else {
		File f = LittleFS.open("CALIBRATION", "r");
		if (!f) {
			digitalWrite(LED_RED, LOW);
		} else {
			calibration_cool_lag_time = f.parseInt();
			calibration_heat_lag_time = f.parseInt();
			calibration_lag_degrees = f.parseInt();
			f.close();
			is_calibrated = true;
		}
		LittleFS.end();
	}

	change_state(MAIN_MENU);

	// Init the temperature sensor whilst we wait for the initial screen draw.
	for (int i = 0; i < 5; i++) {
		delay(100);
		update_temperature();
	}
  // Ensure the elements are off immediately, for safety.
  pinMode(TOP_ELEMENT, OUTPUT);
  pinMode(BOTTOM_ELEMENT, OUTPUT);
  digitalWrite(TOP_ELEMENT, LOW);
  digitalWrite(BOTTOM_ELEMENT, LOW);

  // Next set up our multicore comms, then signal to the other core that
  // it can proceed.
  queue_init(&drawing_queue, sizeof(DrawMessage), 16);
  rp2040.fifo.push(0xDEADBEEF);

  // Now onto the rest of our init...

  pinMode(BUTTON_TOP_LEFT, INPUT_PULLUP);
  pinMode(BUTTON_TOP_RIGHT, INPUT_PULLUP);
  pinMode(BUTTON_BOTTOM_LEFT, INPUT_PULLUP);
  pinMode(BUTTON_BOTTOM_RIGHT, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(BUTTON_TOP_LEFT), top_left_pushed, FALLING);
  attachInterrupt(digitalPinToInterrupt(BUTTON_TOP_RIGHT), top_right_pushed, FALLING);
  attachInterrupt(digitalPinToInterrupt(BUTTON_BOTTOM_LEFT), bottom_left_pushed, FALLING);
  attachInterrupt(digitalPinToInterrupt(BUTTON_BOTTOM_RIGHT), bottom_right_pushed, FALLING);

  pinMode(DISPLAY_BACKLIGHT_EN, OUTPUT);
  digitalWrite(DISPLAY_BACKLIGHT_EN, HIGH);

  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_BLUE, OUTPUT);
  digitalWrite(LED_RED, HIGH);
  digitalWrite(LED_GREEN, HIGH);
  digitalWrite(LED_BLUE, HIGH);

  bool r = LittleFS.begin();
  if (!r) {
    digitalWrite(LED_BLUE, LOW);
  } else {
    File f = LittleFS.open("CALIBRATION", "r");
    if (!f) {
      digitalWrite(LED_RED, LOW);
    } else {
      calibration_cool_lag_time = f.parseInt();
      calibration_heat_lag_time = f.parseInt();
      calibration_lag_degrees = f.parseInt();
      f.close();
      is_calibrated = true;
    }
    LittleFS.end();
  }

  change_state(MAIN_MENU);

  // Init the temperature sensor whilst we wait for the initial screen draw.
  for (int i = 0; i < 5; i++) {
    delay(100);
    update_temperature();
  }
}

void loop() {
	update_temperature();

	uint16_t temp_color = get_temperature_color();
	if (temp_color != current_temp_color) {
		current_temp_color = temp_color;
		draw_header();
		draw_footer();
	} else if (last_temp != current_temp){
		draw_temperature();
	}

	text_bg_color = 0x0000;

	int delay_ms = 100;
	switch (current_state) {
		case MAIN_MENU:
			main_menu_loop();
			break;
		case CALIBRATE_1:
			calibrate_1_loop();
			break;
		case CALIBRATE_2:
			calibrate_2_loop();
			break;
		case CALIBRATE_3:
			calibrate_3_loop();
			break;
		case PICK_PROFILE:
			break;
		case BAKE:
			break;
		case FINISHED_BAKE:
			break;
		case FINISHED_CALIBRATE:
			break;
	}

	if (next_state != current_state) {
		change_state(next_state);
	} else {
		delay(delay_ms);
	}
  update_temperature();

  uint16_t temp_color = get_temperature_color();
  if (temp_color != current_temp_color) {
    current_temp_color = temp_color;
    draw_header();
    draw_footer();
  } else if (last_temp != current_temp){
    draw_temperature();
  }

  text_bg_color = 0x0000;

  int delay_ms = 100;
  switch (current_state) {
    case MAIN_MENU:
      main_menu_loop();
      break;
    case CALIBRATE_1:
      calibrate_1_loop();
      break;
    case CALIBRATE_2:
      calibrate_2_loop();
      break;
    case CALIBRATE_3:
      calibrate_3_loop();
      break;
    case PICK_PROFILE:
      break;
    case BAKE:
      break;
    case FINISHED_BAKE:
      break;
    case FINISHED_CALIBRATE:
      break;
  }

  if (next_state != current_state) {
    change_state(next_state);
  } else {
    delay(delay_ms);
  }
}

/** SECOND CORE **/
Adafruit_ST7789 core1_display = Adafruit_ST7789(DISPLAY_CS, DISPLAY_DC, DISPLAY_MOSI, DISPLAY_SCLK);

void core1_draw_text(const TextType& text, const string& actual_text) {
	auto str = actual_text.c_str();
  auto str = actual_text.c_str();

	int16_t x, y;
	uint16_t w, h;
	core1_display.getTextBounds(str, 0, 0, &x, &y, &w, &h);
  int16_t x, y;
  uint16_t w, h;
  core1_display.getTextBounds(str, 0, 0, &x, &y, &w, &h);

	x = text.x;
	y = text.y;
  x = text.x;
  y = text.y;

	if (text.justify == CENTER) x -= w / 2;
	if (text.justify == RIGHT) x -= w;
  if (text.justify == CENTER) x -= w / 2;
  if (text.justify == RIGHT) x -= w;

	core1_display.setTextColor(text.fg_color);
	core1_display.fillRect(x, y, w, h, text.bg_color);
	core1_display.setCursor(x, y);
	core1_display.print(str);
  core1_display.setTextColor(text.fg_color);
  core1_display.fillRect(x, y, w, h, text.bg_color);
  core1_display.setCursor(x, y);
  core1_display.print(str);
}

void setup1() {
	core1_display.init(240, 320);
	core1_display.setSPISpeed(62'500'000);
	core1_display.setRotation(3);
  core1_display.init(240, 320);
  core1_display.setSPISpeed(62'500'000);
  core1_display.setRotation(3);

	core1_display.setFont();
	core1_display.setTextSize(2);
	core1_display.setTextColor(ST77XX_WHITE);
  core1_display.setFont();
  core1_display.setTextSize(2);
  core1_display.setTextColor(ST77XX_WHITE);

	// Wait for the other core to signal us before starting our loop.
	rp2040.fifo.pop();
  // Wait for the other core to signal us before starting our loop.
  rp2040.fifo.pop();
}

void loop1() {
	DrawMessage message{DrawMessage::CLEAR, { NoArgsType{} }};
	queue_remove_blocking(&drawing_queue, &message);

	string copied_text;
	if (message.str != nullptr) {
		copied_text = *message.str;
		delete message.str;
	}

	switch (message.type) {
		case DrawMessage::CLEAR:
			core1_display.fillScreen(0x0000);
			break;
		case DrawMessage::RECT:
			core1_display.fillRect(
					message.rect.x,
					message.rect.y,
					message.rect.w,
					message.rect.h,
					message.rect.color);
			break;
		case DrawMessage::CURSOR:
			core1_display.setCursor(
					message.cursor.x,
					message.cursor.y);
			break;
		case DrawMessage::PRINT:
			core1_display.print(copied_text.c_str());
			break;
		case DrawMessage::TEXT:
			core1_draw_text(message.text, copied_text);
			break;
		case DrawMessage::CONFIG:
			core1_display.setTextSize(message.config.textSize);
			core1_display.setTextColor(message.config.textColor);
			core1_display.setFont(message.config.font);
			break;
		case DrawMessage::LINE:
			core1_display.drawLine(
					message.line.x0,
					message.line.y0,
					message.line.x1,
					message.line.y1,
					message.line.color);
			break;
		case DrawMessage::PIXEL:
			core1_display.drawPixel(
					message.pixel.x,
					message.pixel.y,
					message.pixel.color);
			break;
		default:
			break;
	}
  DrawMessage message{DrawMessage::CLEAR, { NoArgsType{} }};
  queue_remove_blocking(&drawing_queue, &message);

  string copied_text;
  if (message.str != nullptr) {
    copied_text = *message.str;
    delete message.str;
  }

  switch (message.type) {
    case DrawMessage::CLEAR:
      core1_display.fillScreen(0x0000);
      break;
    case DrawMessage::RECT:
      core1_display.fillRect(
	  message.rect.x,
	  message.rect.y,
	  message.rect.w,
	  message.rect.h,
	  message.rect.color);
      break;
    case DrawMessage::CURSOR:
      core1_display.setCursor(
	  message.cursor.x,
	  message.cursor.y);
      break;
    case DrawMessage::PRINT:
      core1_display.print(copied_text.c_str());
      break;
    case DrawMessage::TEXT:
      core1_draw_text(message.text, copied_text);
      break;
    case DrawMessage::CONFIG:
      core1_display.setTextSize(message.config.textSize);
      core1_display.setTextColor(message.config.textColor);
      core1_display.setFont(message.config.font);
      break;
    case DrawMessage::LINE:
      core1_display.drawLine(
	  message.line.x0,
	  message.line.y0,
	  message.line.x1,
	  message.line.y1,
	  message.line.color);
      break;
    case DrawMessage::PIXEL:
      core1_display.drawPixel(
	  message.pixel.x,
	  message.pixel.y,
	  message.pixel.color);
      break;
    default:
      break;
  }
}