#include "dashboard.h"
#include "font.h"
#include "icons.h"
#include "usart.h"
#include "crc.h"
#include "float16.h"
#include "math.h"
//#include "monomaniacone14pt.h"
#include "monomaniacone27pt.h" // 2x antialiased version of 14pt
//#include "monomaniacone20pt.h"
#include "monomaniacone39pt.h" // 2x antialiased version of 20pt
#include "monomaniacone72pt.h"
#include <stdio.h>

const char *fault_code_strings[] = { "none", "over voltage", "under voltage",
    "driver fault", "abs over current", "over tmp FET", "over tmp motor",
    "gate drv over v", "gate drv under v", "MCU under voltage",
    "boot watchdog rst", "encoder SPI error", "encoder scos min",
    "encoder scos max", "flash corruption", "high ofst cur s1",
    "high ofst cur s2", "high ofst cur s3", "unbalanced curs", "brake fault",
    "resolver LOT", "resolver DOS", "resolver LOS", "app flash corupt",
    "mot flash corupt", "encoder no magnet", "encoder str magnet",
    "phase filter fault" };

void run_dashboard_loop_test() {
  LCD_Init();
  LCD_Fill_Screen(COLOR_OFF, 1);
  LCD_Fill_Screen(COLOR_BG, 0);
  draw_init();
  uint32_t i = 0;
  while (1) {

    draw_battery(654, i * 10 / 4, 13210000, 343000);
    draw_speed((((i/2) % 34)) * 1000, 0);
    int16_t duty = 1000 - (int16_t) i * 1000 / 200;
    draw_power_bars(duty);
    draw_power(100 * i / 20 * (duty < 0 ? -1 : 1), duty, 60 * 10);

    draw_adc(i % 34, (400 - i + 16) % 34, i % 34 >= 2.5,
        (400 - i + 16) % 34 >= 2.5);

    draw_temps((4 * i) % 900, ((10 * i) + 450) % 900);

    HAL_Delay(50);
    if (i == 400) {
      i = 0;
    } else {
      i++;
    }
  }

}

uint8_t initialized = 0;
void run_dashboard_loop() {
  //run_dashboard_loop_test();
  LCD_Init();
  LCD_Fill_Screen(COLOR_OFF, 1);
  LCD_Fill_Screen(COLOR_BG, 0);
  HAL_Delay(1000);

  uint8_t i = 0;
  while (1) {
    uint8_t refresh_slow = (i % 10) == 0;
    uint8_t refresh_mid = (i % 3) == 0;
    update_values(refresh_slow, refresh_mid);
    update_adc();

    HAL_Delay(50);
    i++;
    if (i == 120) {
      i = 0;
    }
  }

}

// refresh_slow = 0 if it should not refresh the low-refresh rate information.
// refresh_mid = 0  if it should not refresh the mid-refresh rate information.
void update_values(uint8_t refresh_slow, uint8_t refresh_mid) {
// UART send 0201 2F D58D 03
  uint8_t packet[1];
  packet[0] = 0x2F;// COMM_GET_VALUES_SETUP
  USART1_SendPacket(packet, 1);

// Read length (big-endian)
  uint8_t len = USART1_ReceiveUInt16() & 0xFF;
  if (len != 0x46) {
    HAL_Delay(500);
    USART1_Flush();
    return;// Timed out
  }
// Read packet
  USART1_ReceiveByte();// Packet id
  int16_t temp_fet = USART1_ReceiveInt16();
  int16_t temp_motor = USART1_ReceiveInt16();
  USART1_ReceiveInt32();// Current tot
  int32_t current_in_tot = USART1_ReceiveInt32();
  int16_t duty_cycle = USART1_ReceiveInt16();
  USART1_ReceiveInt32();// rpm
  int32_t speed = USART1_ReceiveInt32();
  int16_t input_voltage_filtered = USART1_ReceiveInt16();
  int16_t battery_level = USART1_ReceiveInt16();
  USART1_ReceiveInt32();// ah tot
  USART1_ReceiveInt32();// ah charge tot
  USART1_ReceiveInt32();// wh tot
  USART1_ReceiveInt32();// wh charge tot
  USART1_ReceiveInt32();// distance
  int32_t distance_abs = USART1_ReceiveInt32();
  USART1_ReceiveInt32();// pid pos now
  uint8_t fault_code = USART1_ReceiveByte();
  USART1_ReceiveByte();// Controller Id
  USART1_ReceiveByte();// Num VESCs
  USART1_ReceiveInt32();// wh battery left
  uint32_t distance_life = USART1_ReceiveUInt32();
  USART1_ReceiveUInt32();// System time

  USART1_ReceiveUInt16();// CRC
// End byte
  if (USART1_ReceiveByte() != 0x03) {
    return;// Invalid packet
  }
// CRC check
//  if (crc != crc16(data, len)) {
//    return;
//  }
  if (!initialized) {
    draw_init();
    initialized = 1;
    refresh_slow = 1;
    refresh_mid = 1;
  }

//speed = (int32_t) (((float) speed) / 1.609344);
//float wheel_diameter = 0.246; // in meter
//speed = ((float) rpm) * 3.14159265359 * wheel_diameter * 60 / 1000;

  draw_power_bars(duty_cycle);
  if (refresh_mid) {
    draw_speed((int32_t) (((float) speed) * 3.6), fault_code);
    draw_power(current_in_tot, duty_cycle, input_voltage_filtered);
  }
  if (refresh_slow) {
    draw_battery(input_voltage_filtered, battery_level, distance_abs,
        distance_life);
    draw_temps(temp_fet, temp_motor);
  }
}
void update_adc() {
// Replies to 0x24 0x65 for command id: 0, 1, 18, 19, 1B, 1D, C9, CA
//                                      0  1  24  25  27  29  201 202

  uint8_t packet[3];
  packet[0] = 0x24;// COMM_CUSTOM_APP_DATA
  packet[1] = 0x65;// reFloat package interface
  packet[2] = 0x01;// command id
  USART1_SendPacket(packet, 3);

  uint8_t len = USART1_ReceiveUInt16() & 0xFF;
  if (len == 0xFF) {// len is usually 0x3A = 58, but may be shorter.
    HAL_Delay(500);
    USART1_Flush();
    return;// Timed out
  }

// Read packet
  USART1_ReceiveByte();// packet id
  USART1_ReceiveByte();// reFloat package interface
  USART1_ReceiveByte();// command id
  USART1_ReceiveInt32();// Balance current
  USART1_ReceiveInt32();// IMU Pitch
  USART1_ReceiveInt32();// IMU Roll
  USART1_ReceiveByte();// State
  uint8_t beep_fs_state = USART1_ReceiveByte();// Beep reason + footpad sensors state
  uint32_t adc1_f32 = USART1_ReceiveUInt32();
  uint32_t adc2_f32 = USART1_ReceiveUInt32();

// Results if command id 31 (0x1F) was working:
//  uint8_t mask = USART1_ReceiveByte();// mask                                   // 0x00
//  uint8_t extra_flags = USART1_ReceiveByte();// extra flags                     // 0x02
//  uint32_t time = USART1_ReceiveInt32();// time                                 // 0x00 10 00 41
//  uint8_t state_and_time = USART1_ReceiveByte();// state_and_mode               // 0x9B
//  uint8_t flags_and_footpad = USART1_ReceiveByte();// flags_and_footpad         // 0x8A
//  uint8_t stop_cond_and_sat = USART1_ReceiveByte();// stop_cond_and_sat         // 0x29
//  uint8_t alert_reason = USART1_ReceiveByte();// alert_reason                   // 0x41
//
//  USART1_ReceiveInt16();//motor.speed
//  USART1_ReceiveInt16();//motor.erpm
//  USART1_ReceiveInt16();//motor.current
//  USART1_ReceiveInt16();//motor.dir_current
//  USART1_ReceiveInt16();//motor.filt_current
//  USART1_ReceiveInt16();//motor.duty_cycle
//  USART1_ReceiveInt16();//motor.batt_voltage
//  USART1_ReceiveInt16();//motor.batt_current
//  USART1_ReceiveInt16();//motor.mosfet_temp
//  USART1_ReceiveInt16();//motor.motor_temp
//  USART1_ReceiveInt16();//imu.pitch
//  USART1_ReceiveInt16();//imu.balance_pitch
//  USART1_ReceiveInt16();//imu.roll
//  uint16_t adc1_f16 = USART1_ReceiveUInt16();//footpad.adc1
//  uint16_t adc2_f16 = USART1_ReceiveUInt16();//footpad.adc2
//  USART1_ReceiveInt16();//remote.input
//// May have other ignored values.
//  int16_t adc1 = refloat_float16_to_int16_scaled(adc1_f16);
//  int16_t adc2 = refloat_float16_to_int16_scaled(adc2_f16);

  if (!initialized) {
    draw_init();
    initialized = 1;
  }
  float adc1 = *((float*) &adc1_f32);
  float adc2 = *((float*) &adc2_f32);

  uint8_t fs_state = beep_fs_state & 0b11;
  uint8_t adc1_en = 0;
  uint8_t adc2_en = 0;
  if (fs_state == 1) {
    if (adc1 > adc2) {
      adc1_en = 1;
    } else {
      adc2_en = 2;
    }
  } else if (fs_state == 2) {
    adc1_en = 1;
    adc2_en = 1;
  }

  draw_adc((int32_t) 10 * adc1, (int32_t) 10 * adc2, adc1_en, adc2_en);

// Make sure other values are discarded
  HAL_Delay(100);
  USART1_Flush();
}

#define LEFT_CENTER_COL1 38
#define LEFT_CENTER_COL2 116
#define RIGHT_CENTER 400

void draw_init() {

// Draw footpad sensors rect
  uint16_t sensor_width = 126;
  LCD_DrawHollowRoundRect(RIGHT_CENTER - sensor_width / 2, 70, sensor_width, 73,
      15, 0, 6, COLOR_PRIMARY, 0, 0);
  LCD_Draw_Rectangle(RIGHT_CENTER - 3, 76, 6, 61, COLOR_PRIMARY);

// Draw temp icons
  LCD_Draw_Icon_3(motort_icon, LEFT_CENTER_COL1 - MOTORT_ICON_WIDTH / 2, 73,
  MOTORT_ICON_WIDTH, MOTORT_ICON_HEIGHT, COLOR_SECONDARY, COLOR_ERROR,
  COLOR_BG);
  LCD_Draw_Icon_3(chipt_icon, LEFT_CENTER_COL2 - CHIPT_ICON_WIDTH / 2, 73,
  CHIPT_ICON_WIDTH, CHIPT_ICON_HEIGHT, COLOR_PRIMARY, COLOR_ERROR,
  COLOR_BG);

//  GFX_DrawChar(40, 40, '!', &monomaniacone20pt, COLOR_PRIMARY, COLOR_BG);
//  GFX_DrawChar(60, 40, '2', &monomaniacone20pt, COLOR_PRIMARY, COLOR_BG);
//  GFX_DrawChar(80, 40, '3', &monomaniacone20pt, COLOR_SECONDARY, COLOR_BG);
//  GFX_DrawChar(100, 40, '4', &monomaniacone20pt, COLOR_SECONDARY, COLOR_BG);
//  GFX_DrawChar(120, 40, '5', &monomaniacone20pt, COLOR_ERROR, COLOR_BG);

//  GFX_DrawText(LCD_WIDTH / 2, 50, "Bonjour Monsieur !", &monomaniacone12pt,
//  COLOR_SECONDARY, COLOR_BG, 1, -2);
//  GFX_DrawText(LCD_WIDTH / 2, 70, "Bonjour Monsieur !", &monomaniacone14pt,
//  COLOR_SECONDARY, COLOR_BG, 1, -2);
//  GFX_DrawText(LCD_WIDTH / 2, 100, "Bonjour MONSieur !", &monomaniacone20pt,
//  COLOR_SUCCESS, COLOR_BG, 1, -2);

// Draw a rectangle with different top/bottom radii
//  LCD_DrawHollowRoundRect(10, 10, 100, 50, 10, 0, 4, BLACK, COLOR_PRIMARY, 1);
//// Draw a pill-shaped rectangle (same radius for all corners)
//  LCD_DrawHollowRoundRect(20, 70, 80, 30, 15, 15, 2, COLOR_SUCCESS, 0, 0);
//// Draw a square with rounded corners
//  LCD_DrawHollowRoundRect(50, 100, 60, 60, 10, 10, 6, COLOR_ERROR, 0, 0);
//  LCD_DrawHollowRoundRect(130, 100, 60, 60, 10, 10, 0, 0, COLOR_ERROR, 1);
//  LCD_DrawHollowRoundRect(136, 106, 48, 48, 4, 4, 0, 0, COLOR_PRIMARY, 1);
//
//  LCD_DrawHollowRoundRect(200, 100, 4, 20, 6, 6, 0, 0, COLOR_PRIMARY, 1);
//  LCD_DrawHollowRoundRect(200, 125, 6, 20, 6, 6, 0, 0, COLOR_PRIMARY, 1);
//  LCD_DrawHollowRoundRect(200, 150, 8, 20, 6, 6, 0, 0, COLOR_PRIMARY, 1);
}

// Displays the battery voltage and percent, with the trip and life distances
// Input voltage from COMM_GET_VALUES_SETUP, scale 10
// battery percent (level) from COMM_GET_VALUES_SETUP, scale 10
// trip distance from COMM_GET_VALUES_SETUP, scale 1000, in m
// life distance (odometer) from COMM_GET_VALUES_SETUP, in m
int16_t last_voltage = 0;
int32_t last_percent = 0;
int32_t last_trip_dist = 0;
void draw_battery(
    int16_t voltage,
    int32_t percent,
    int32_t trip_dist,
    uint32_t life_dist) {

  if (voltage == last_voltage && percent / 10 == last_percent
      && trip_dist / 100000 == last_trip_dist) {
    return;
  }
  last_voltage = voltage;
  last_percent = percent / 10;
  last_trip_dist = trip_dist / 100000;

  uint16_t bar_width = LCD_WIDTH - 12;
  uint16_t bar_height = 22;
  uint16_t filled_bar_width = (((float) (percent / 10)) / 100.0) * bar_width;
  if (filled_bar_width < 6) {
    filled_bar_width = 6;// Must be at least the size of the border radius.
  }
  uint16_t filled_bar_end_x = 4 + 2 + filled_bar_width;
//uint16_t text_y_12 = LCD_HEIGHT - 6 - (22 - 16) / 2;
  uint16_t text_y_14 = LCD_HEIGHT - 6 - (22 - 18) / 2;

  char voltage_text[8];
  sprintf(voltage_text, "%.1fV", ((float) voltage) / 10.0);
  char percent_text[6];
  sprintf(percent_text, "%lu%%", percent / 10);
  char distances_text[20];
  sprintf(distances_text, "%.1fKm / %luKm",
      ((float) (trip_dist / 100000)) / 10.0, life_dist / 1000);

// Drawing the bars
  LCD_DrawHollowRoundRect(4, LCD_HEIGHT - (bar_height + 4) - 4, bar_width + 4,
      bar_height + 4, 8, 8, 2, COLOR_FG, COLOR_BG, 1);
  if (filled_bar_width > 0) {
    LCD_DrawHollowRoundRect(4 + 2, LCD_HEIGHT - (bar_height + 2) - 4,
        filled_bar_width, bar_height, 6, 6, 0, 0, COLOR_SUCCESS, 1);
  }

// Drawing the values
  uint16_t left_x = 10;
  uint16_t right_x = LCD_WIDTH - 10;

  if (percent > 20 * 10) {
// Drawing the voltage to the left
    left_x = 10
        + GFX_DrawTextScaled(left_x, text_y_14, voltage_text,
            &monomaniacone27pt,
            COLOR_BG, COLOR_SUCCESS, 0, -2, 2);
  } else {
// Drawing the voltage to the right
    right_x = -10
        + GFX_DrawTextScaled(right_x, text_y_14, voltage_text,
            &monomaniacone27pt,
            COLOR_FG, COLOR_BG, 2, -2, 2);
  }

  if (percent > 50 * 10) {
// Drawing the distances on the left
    GFX_DrawTextScaled(left_x, text_y_14, distances_text, &monomaniacone27pt,
    COLOR_BG, COLOR_SUCCESS, 0, -2, 2);
  } else {
// Drawing the distances on the right
    GFX_DrawTextScaled(right_x, text_y_14, distances_text, &monomaniacone27pt,
    COLOR_FG, COLOR_BG, 2, -2, 2);
  }

  if (percent > 80 * 10) {
// Drawing the distances on the left
    GFX_DrawTextScaled(filled_bar_end_x - 4, text_y_14, percent_text,
        &monomaniacone27pt, COLOR_BG, COLOR_SUCCESS, 2, 0, 2);
  } else {
// Drawing the distances on the right
    GFX_DrawTextScaled(filled_bar_end_x + 4, text_y_14, percent_text,
        &monomaniacone27pt, COLOR_FG, COLOR_BG, 0, 0, 2);
  }

}

// Displays the power bars at the top
// Duty from COMM_GET_VALUES, scale 1000
uint16_t last_width = 0;
uint8_t last_regen = 0;
void draw_power_bars(int16_t dutyy) {
  uint16_t offset = LCD_WIDTH / 2;
  uint16_t max_width = LCD_WIDTH / 2;
  uint8_t regen = dutyy < 0;
  uint16_t duty = regen ? -dutyy : dutyy;
  duty *= 1 / 0.8;// 80% duty means the bar is at 100%
  uint16_t width = ((float) duty / 1000.0) * max_width;

  uint16_t last_origin = last_regen ? offset - last_width : offset;
  uint16_t origin = regen ? offset - width : offset;
  uint16_t color = regen ? COLOR_ERROR : COLOR_SUCCESS;

  if (regen != last_regen) {
    LCD_Draw_Rectangle(last_origin, 0, last_width, 20, COLOR_BG);
    LCD_Draw_Rectangle(origin, 0, width, 20, color);
    last_regen = regen;
    last_width = width;

  } else if (width != last_width) {
    if (regen) {
      if (last_origin < origin) {
        LCD_Draw_Rectangle(last_origin, 0, origin - last_origin, 20, COLOR_BG);
      } else {
        LCD_Draw_Rectangle(origin, 0, last_origin - origin, 20, color);
      }
    } else {
      if (last_width < width) {
        LCD_Draw_Rectangle(origin + last_width, 0, width - last_width, 20,
            color);
      } else {
        LCD_Draw_Rectangle(origin + width, 0, last_width - width, 20, COLOR_BG);
      }
    }

    last_width = width;
  }
}

// Displays the huge speed counter with avg and max values.
// Speed from COMM_GET_VALUES_SETUP, scale 1000
uint32_t last_speed = 30;
uint32_t max_speed = 0;
uint32_t last_avg_speed = 30;
uint32_t avg_speed_tot = 0;// You need to ride super fast for a super long time for it to overflow ;)
uint32_t avg_speed_count = 0;
uint8_t last_fault_code = 0;
uint16_t last_speed_cursor_x = 0;
char speed_text[4];
void draw_speed(int32_t speedd, uint8_t fault_code) {

  uint32_t speed = speedd < 0 ? -speedd / 1000 : speedd / 1000;
  if (speed >= 100) {
    speed = 99;
  }

// update max speed
  uint8_t update_stats = 0;
  if (speed > max_speed) {
    max_speed = speed;
    update_stats = 1;
  }
// update avg speed
  avg_speed_tot += speed;
  avg_speed_count += 1;
  uint32_t avg_speed = avg_speed_tot / avg_speed_count;
  if (last_avg_speed != avg_speed) {
    last_avg_speed = avg_speed;
    update_stats = 1;
  }

// Draw
  if (last_speed != speed) {
    //uint16_t erase_width = 85;// width to erase from center

    if (last_speed >= 10 && speed >= 10 && last_speed / 10 == speed / 10) {
      uint16_t old_glyphIndex = speed_text[0] - monomaniacone72pt.firstChar;
      const GFXglyph *old_glyph = &monomaniacone72pt.glyphs[old_glyphIndex];
      GFX_ClearChar(last_speed_cursor_x - old_glyph->advance, 32 + 95, speed_text[1], &monomaniacone72pt, COLOR_BG);
      sprintf(speed_text, "%lu", speed);
      last_speed_cursor_x = GFX_DrawChar(last_speed_cursor_x - old_glyph->advance, 32 + 95, speed_text[1],
          &monomaniacone72pt, COLOR_FG, COLOR_BG);
    } else {
      GFX_ClearText(LCD_WIDTH / 2, 32 + 95, speed_text,
          &monomaniacone72pt, COLOR_BG, 1, -5);
      sprintf(speed_text, "%lu", speed);
//      LCD_Draw_Rectangle(LCD_WIDTH / 2 - erase_width, 22, 2 * erase_width, 110,
//      COLOR_BG);
      last_speed_cursor_x = 5 + GFX_DrawText(LCD_WIDTH / 2, 32 + 95, speed_text,
          &monomaniacone72pt,
          COLOR_FG, COLOR_BG, 1, -5);
    }
    last_speed = speed;
  }

  uint16_t erase_width = 90;// width to erase from center
  if (last_fault_code != fault_code) {
    last_fault_code = fault_code;
    if (fault_code == 0) {
      update_stats = 1;
      erase_width = 100;
    } else {
      update_stats = 0;
      char fault_text[50];// Increased size to accommodate longer strings

      if (fault_code <= FAULT_CODE_MAX) {
        sprintf(fault_text, "%u %s", fault_code,
            fault_code_strings[fault_code]);
      } else {
        sprintf(fault_text, "%u unknown fault", fault_code);
      }

      LCD_Draw_Rectangle(LCD_WIDTH / 2 - erase_width, 140, 2 * erase_width, 25,
      COLOR_BG);
      GFX_DrawTextScaled(LCD_WIDTH / 2, 159, fault_text, &monomaniacone27pt,
      COLOR_ERROR, COLOR_BG, 1, -3, 2);
    }
  }
  if (update_stats) {
    char stats_text[20];
    sprintf(stats_text, "avg: %lu  max: %lu", avg_speed, max_speed);
    LCD_Draw_Rectangle(LCD_WIDTH / 2 - erase_width, 140, 2 * erase_width, 25,
    COLOR_BG);
    GFX_DrawTextScaled(LCD_WIDTH / 2, 159, stats_text, &monomaniacone27pt,
    COLOR_SECONDARY, COLOR_BG, 1, -1, 2);
  }
}

int16_t last_current = 99;
uint16_t last_duty = 99;
int32_t last_power = 1000;
// Displays Current, Duty, Watts
// Current from COMM_GET_VALUES, scale 100
// Duty from COMM_GET_VALUES, scale 1000
// Input voltage from COMM_GET_VALUES, scale 10
void draw_power(int32_t current_i, int16_t duty_i, int16_t voltage_i) {
  if (duty_i < 0 && current_i > 0) {
    current_i = -current_i;
  }
  int16_t current = current_i / 100;
  uint16_t duty = duty_i < 0 ? -duty_i / 10 : duty_i / 10;
  int32_t power = ((int32_t) voltage_i) * ((int32_t) current_i) / 10000 * 10;

  if (power >= 1000) {
    power = power / 100 * 100;
  } else if (power <= -1000) {
    power = power / 100 * 100;
  }

  if (current != last_current) {
    last_current = current;
    if (current >= 100) {
      current = 99;
    } else if (current <= -100) {
      current = -99;
    }

    char text[8];
    sprintf(text, "%d.", current);
    LCD_Draw_Rectangle(LEFT_CENTER_COL1 - 38, 30, 2 * 39, 35, COLOR_BG);
    GFX_DrawTextScaled(LEFT_CENTER_COL1, 60, text, &monomaniacone39pt,
    COLOR_PRIMARY, COLOR_BG, 1, -2, 2);
  }
  if (duty != last_duty) {
    last_duty = duty;
    if (duty >= 100) {
      duty = 99;
    }

    char text[8];
    sprintf(text, "%u,", duty);
    LCD_Draw_Rectangle(LEFT_CENTER_COL2 - 39, 30, 2 * 39, 35, COLOR_BG);
    GFX_DrawTextScaled(LEFT_CENTER_COL2, 60, text, &monomaniacone39pt,
    COLOR_PRIMARY, COLOR_BG, 1, -2, 2);
  }
  if (power != last_power) {
    last_power = power;
    if (power >= 10000) {
      power = 9999;
    } else if (power <= -10000) {
      power = -9999;
    }

    char text[10];
    sprintf(text, "%ld/", power);
    LCD_Draw_Rectangle(RIGHT_CENTER - 70, 30, 2 * 70, 35, COLOR_BG);
    GFX_DrawTextScaled(RIGHT_CENTER, 60, text, &monomaniacone39pt,
    COLOR_PRIMARY, COLOR_BG, 1, -2, 2);
  }

}

uint16_t last_adc1 = 1000;
uint16_t last_adc2 = 1000;
uint8_t last_adc1_en = 0;
uint8_t last_adc2_en = 0;
// Displays the two ADC voltages
// 2 voltages from reFloat - COMM_CUSTOM_APP_DATA, scale 10, adcx_en are booleans.
void draw_adc(
    int32_t adc1_scaled,
    int32_t adc2_scaled,
    uint8_t adc1_en,
    uint8_t adc2_en) {
  uint16_t adc1 = adc1_scaled < 0 ? -adc1_scaled : adc1_scaled;
  uint16_t adc2 = adc2_scaled < 0 ? -adc2_scaled : adc2_scaled;
  if (adc1 > 33) {
    adc1 = 33;
  }
  if (adc2 > 33) {
    adc2 = 33;
  }
// Max filled height = 39
  if (adc1 != last_adc1 || adc1_en != last_adc1_en) {
    uint16_t filled_height = adc1 * 39 / 33;
    uint16_t free_height = 61 - filled_height;

// Reset the top part
    LCD_Draw_Rectangle(RIGHT_CENTER - 3 - 54 + 2, 76 + 2, 54 - 4, 2, COLOR_BG);
    LCD_Draw_Rectangle(RIGHT_CENTER - 3 - 54 + 0, 76 + 4, 54, free_height - 4,
    COLOR_BG);

// Draw the colored zone
    uint16_t color = adc1_en ? COLOR_SUCCESS : COLOR_ERROR;
    LCD_Draw_Rectangle(RIGHT_CENTER - 3 - 54, 76 + free_height, 54,
        filled_height, color);

// Draw the text
    char text[8];
    sprintf(text, "%.1fV", ((float) adc1) / 10);
    GFX_DrawTextScaled(RIGHT_CENTER - 3 - 27, 76 + free_height - 2, text,
        &monomaniacone27pt, color, COLOR_BG, 1, -1, 2);

    last_adc1 = adc1;
    last_adc1_en = adc1_en;
  }
// Max filled height = 39
  if (adc2 != last_adc2 || adc2_en != last_adc2_en) {
    uint16_t filled_height = adc2 * 39 / 33;
    uint16_t free_height = 61 - filled_height;

// Reset the top part
    LCD_Draw_Rectangle(RIGHT_CENTER + 3 + 2, 76 + 2, 54 - 4, 2, COLOR_BG);
    LCD_Draw_Rectangle(RIGHT_CENTER + 3 + 0, 76 + 4, 54, free_height - 4,
    COLOR_BG);

// Draw the colored zone
    uint16_t color = adc2_en ? COLOR_SUCCESS : COLOR_ERROR;
    LCD_Draw_Rectangle(RIGHT_CENTER + 3, 76 + free_height, 54, filled_height,
        color);

// Draw the text
    char text[8];
    sprintf(text, "%.1fV", ((float) adc2) / 10);
    GFX_DrawTextScaled(RIGHT_CENTER + 3 + 27, 76 + free_height - 2, text,
        &monomaniacone27pt, color, COLOR_BG, 1, -1, 2);

    last_adc2 = adc2;
    last_adc2_en = adc2_en;
  }

}

uint16_t last_temp_fet = 1000;
uint16_t last_temp_mot = 1000;
// Displays the controller and motor temperatures
// 2 temperatures from COMM_GET_VALUES_SETUP, scale 10
void draw_temps(int16_t temp_fet_scaled, int16_t temp_motor_scaled) {
  uint16_t temp_fet = temp_fet_scaled < 0 ? 0 : temp_fet_scaled / 10;
  uint16_t temp_mot = temp_motor_scaled < 0 ? 0 : temp_motor_scaled / 10;
  if (temp_fet > 99) {
    temp_fet = 99;
  }
  if (temp_mot > 99) {
    temp_mot = 99;
  }

  if (temp_mot != last_temp_mot) {
    last_temp_mot = temp_mot;

    char text[8];
    sprintf(text, "%u>C", temp_mot);
    LCD_Draw_Rectangle(LEFT_CENTER_COL1 - 38, 138, 2 * 39, 25, COLOR_BG);
    GFX_DrawTextScaled(LEFT_CENTER_COL1, 159, text, &monomaniacone27pt,
    COLOR_SECONDARY, COLOR_BG, 1, -1, 2);
  }
  if (temp_fet != last_temp_fet) {
    last_temp_fet = temp_fet;

    char text[8];
    sprintf(text, "%u>C", temp_fet);
    LCD_Draw_Rectangle(LEFT_CENTER_COL2 - 39, 138, 2 * 39, 25, COLOR_BG);
    GFX_DrawTextScaled(LEFT_CENTER_COL2, 159, text, &monomaniacone27pt,
    COLOR_PRIMARY,
    COLOR_BG, 1, -1, 2);
  }
}