#include "FT18_STW_DISPLAY.h"
#include "Arduino.h"
#include "EDIPTFT.h"
#include "FT18_STW_INIT.h"
#include "FT_2018_STW_CAN.h"

String pad_left(String orig, int len, char pad_char) {
  String result = {orig};
  for (int i = orig.length(); i < len; i++) {
    result = pad_char + result;
  }

  return result;
}

EDIPTFT tft(true, false);
String bezeichnungen[] = {"T_mot",    "T_oil",      "P_oil",    "% fa",
                          "U_batt",   "P_wat",      "T_air",    "P_b_front",
                          "P_b_rear", "Error Type", "Speed_fl", "Speed_fr",
                          "Speed"};
//"Drehzahl","P_fuel","Index"
int led_s[] = {led1, led2,  led3,  led4,  led5,  led6,  led7,  led8,
               led9, led10, led11, led12, led13, led14, led15, led16};

DataBox gear_box(121, 0, 199, 94, 160, 0, EA_SWISS30B, 4, 4, 'C', true);
DataBox left_box(0, 25, 119, 94, 110, 25, EA_FONT6X8, 3, 8, 'R', false);
DataBox right_box(201, 25, 320, 94, 310, 25, EA_FONT6X8, 3, 8, 'R', false);
DataBox autoshift_box(164+50, 184, 240+50, 230, 202+50, 178, EA_FONT7X12, 3, 5, 'C', false);

TireTempBox fl_box(80, 130, 156, 176, 118, 124, EA_FONT7X12, 3, 5, 'C');
TireTempBox fr_box(164, 130, 240, 176, 202, 124, EA_FONT7X12, 3, 5, 'C');
TireTempBox rl_box(80, 184, 156, 230, 118, 178, EA_FONT7X12, 3, 5, 'C');
TireTempBox rr_box(164, 184, 240, 230, 202, 178, EA_FONT7X12, 3, 5, 'C');

int testing_page = 0;
uint16_t clearcounter = 1;
void init_display() {
  pinMode(writeprotect, OUTPUT);
  digitalWrite(writeprotect, HIGH);
  pinMode(reset, OUTPUT);
  pinMode(disp_cs, OUTPUT);
  pinMode(MOSI, OUTPUT);
  pinMode(MISO, OUTPUT);
  digitalWrite(disp_cs, HIGH);
  digitalWrite(MOSI, HIGH);
  digitalWrite(MISO, HIGH);
  digitalWrite(reset, LOW);
  digitalWrite(reset, HIGH);
  tft.begin(115200); // start display communication
  tft.cursorOn(false);
  tft.terminalOn(false);
  tft.setDisplayColor(EA_WHITE, EA_BLACK);
  tft.setTextColor(EA_WHITE, EA_TRANSPARENT);
  tft.setTextSize(5, 8);
  tft.clear();

  gear_box.update_label(get_label(VAL_GEAR));
  left_box.update_label(get_label(VAL_FIRST_LEFT_BOX));
  right_box.update_label(get_label(VAL_RPM));
  autoshift_box.update_label("");
}

String get_value(Value val) {
  switch (val) {
  case VAL_GEAR:
    if (Vehicle_data.gear == 0) {
      return "N";
    }
    return String(Vehicle_data.gear);
  case VAL_RPM:
    return String(Vehicle_data.revol / 2);
  case VAL_TT_FL:
    return String(Vehicle_data.t_tfl * 0.423529 + 8, 0);
  case VAL_TT_FR:
    return String(Vehicle_data.t_tfr * 0.423529 + 0, 0);
  case VAL_TT_RL:
    return String(Vehicle_data.t_trl * 0.423529 + 11, 0);
  case VAL_TT_RR:
    return String(Vehicle_data.t_trr * 0.423529 + 4, 0);
  case VAL_LAPTIME: {
    double time =
        Vehicle_data.lap_time_sec + Vehicle_data.lap_time_msec / 1000.0;
    if (time < 100) {
      return String(time, 2);
    } else if (time < 1000) {
      return String(time, 1);
    } else if (time < 10000) {
      return String(time, 0);
    } else {
      return "2SLOW";
    }
  }
  case VAL_UBATT:
    return String(0.0706949 * Vehicle_data.u_batt, 2);
  case VAL_TMOT:
    return String(Vehicle_data.t_mot - 40);
  case VAL_TAIR:
    return String(Vehicle_data.t_air - 40);
  case VAL_TOIL:
    return String(Vehicle_data.t_oil - 40);
  case VAL_ERR_TYPE:
    return String(Stw_data.error_type);
  case VAL_PFUEL:
    return String(0.0514 * Vehicle_data.p_fuel, 2);
  case VAL_PWAT:
    return String(0.0514 * Vehicle_data.p_wat, 2);
  case VAL_POIL:
    return String(0.0514 * Vehicle_data.p_oil, 2);
  case VAL_PBF:
    return String(Vehicle_data.p_brake_front);
  case VAL_PBR:
    return String(Vehicle_data.p_brake_rear);
  case VAL_SPEED_FL:
    return String(Vehicle_data.speed_fl);
  case VAL_SPEED_FR:
    return String(Vehicle_data.speed_fr);
  case VAL_SPEED:
    return String(Vehicle_data.speed);
  case VAL_BBAL: {
    double p_total =
        Vehicle_data.p_brake_front + Vehicle_data.p_brake_rear / 2.398;
    double bbal = p_total == 0 ? 0 : 100 * Vehicle_data.p_brake_front / p_total;
    if (bbal >= 100) {
      return "100";
    }
    return String(bbal, 2);
  }
  default:
    return "???";
  }
}

String get_label(Value val) {
  switch (val) {
  case VAL_GEAR:
    return "GEAR";
  case VAL_RPM:
    return "RPM";
  case VAL_TT_FL:
    return "TEMP FL";
  case VAL_TT_FR:
    return "TEMP FR";
  case VAL_TT_RL:
    return "TEMP RL";
  case VAL_TT_RR:
    return "TEMP RR";
  case VAL_LAPTIME:
    return "LAPTIME";
  case VAL_UBATT:
    return "BATT VOLTAGE";
  case VAL_TMOT:
    return "TEMP ENG";
  case VAL_TAIR:
    return "TEMP AIR";
  case VAL_TOIL:
    return "TEMP OIL";
  case VAL_ERR_TYPE:
    return "ERROR TYPE";
  case VAL_PFUEL:
    return "PRESS FUEL";
  case VAL_PWAT:
    return "PRESS WAT";
  case VAL_POIL:
    return "PRESS OIL";
  case VAL_PBF:
    return "PRESS BRAKE F";
  case VAL_PBR:
    return "PRESS BRAKE R";
  case VAL_SPEED_FL:
    return "SPEED FL";
  case VAL_SPEED_FR:
    return "SPEED FR";
  case VAL_SPEED:
    return "SPEED";
  case VAL_BBAL:
    return "BBAL";
  default:
    return "???";
  }
}

bool check_alarms() {
  static uint32_t poil_last_valid, tmot_last_valid, toil_last_valid;
  uint32_t now = millis();
  if (Vehicle_data.p_oil >= POIL_ALARM_THRESH || Vehicle_data.speed == 0) {
    poil_last_valid = now;
  }
  if (Vehicle_data.t_mot <= TMOT_ALARM_THRESH ||
      Vehicle_data.t_mot == TMOT_SAFE_VALUE) {
    tmot_last_valid = now;
  }
  if (Vehicle_data.t_oil <= TOIL_ALARM_THRESH) {
    toil_last_valid = now;
  }
  // bool poil_alarm = now - poil_last_valid >= POIL_ALARM_TIME;
  bool poil_alarm = false;
  bool tmot_alarm = now - tmot_last_valid >= TMOT_ALARM_TIME;
  bool toil_alarm = now - toil_last_valid >= TOIL_ALARM_TIME;
  bool alarm_active = poil_alarm || tmot_alarm || toil_alarm;

  if (alarm_active) {
    String alarm_text = "";
    if (poil_alarm)
      alarm_text += "PO";
    if (tmot_alarm)
      alarm_text += "TM";
    if (toil_alarm)
      alarm_text += "TO";
    alarm(alarm_text);
  }

  return alarm_active;
}

bool check_enc_displays() {
  static uint8_t trc_old, mode_old;
  static bool display_trc, display_mode;
  static uint32_t display_trc_begin, display_mode_begin;

  return check_display(trc_old, Stw_data.trc, display_trc, display_trc_begin,
                       "ARB") ||
         check_display(mode_old, Stw_data.mode, display_mode,
                       display_mode_begin, "MODE");
}

bool check_display(uint8_t& val_old, uint8_t val_new, bool& active,
                   uint32_t& begin, const String& title) {
  if (val_old != val_new) {
    active = true;
    begin = millis();
    val_old = val_new;
    tft.clear();
    tft.fillDisplayColor(EA_RED);
    tft.setTextColor(EA_WHITE, EA_RED);
    tft.setTextSize(7, 8);
    String text = title + ":" + val_new;
    char text_arr[16];
    text.toCharArray(text_arr, 16);
    tft.drawText(15, 68, 'C', text_arr);
  } else if (active && millis() - begin > ENC_DISPLAY_TIME) {
    tft.setTextColor(EA_WHITE, EA_TRANSPARENT);
    tft.clear();
    active = false;
  }

  return active;
}

void update_display() {
  static DisplayView view = VIEW_DRIVER;
  static uint32_t last_cleared;
  static bool cleared = true;

  if (check_alarms()) {
    cleared = true;
    return;
  }
  if (tft.disconnected) {
    uint32_t now = millis();
    if (now - last_cleared < 1000) {
      return;
    }
    digitalWrite(reset, LOW);
    delay(100);
    digitalWrite(reset, HIGH);
    tft.disconnected = false;
    tft.clear();
    cleared = true;
    last_cleared = now;
  }

  if (check_enc_displays()) {
    cleared = true;
    return;
  }

  uint32_t now = millis();
  // Both buttons have to be pressed at the same time, but we also use the
  // debounced rises to ensure we don't keep toggling between the views
  if (Stw_data.buttonState1 && Stw_data.buttonState4 &&
      (Stw_data.button1_rises > 0 || Stw_data.button4_rises > 0)) {
    Stw_data.button1_rises = 0;
    Stw_data.button4_rises = 0;
    view = (DisplayView)((view + 1) % (VIEW_LAST + 1));
    tft.clear();
    cleared = true;
  }

  if (view == VIEW_DRIVER) {
    if (cleared) {
      redraw_view_driver();
      cleared = false;
    } else {
      update_view_driver();
    }
  } else {
    if (cleared) {
      redraw_view_testing();
      cleared = false;
    } else {
      update_view_testing();
    }
  }
  if (clearcounter>= 10000){
  pinMode(writeprotect, OUTPUT);
  digitalWrite(writeprotect, HIGH);
  pinMode(reset, OUTPUT);
  pinMode(disp_cs, OUTPUT);
  pinMode(MOSI, OUTPUT);
  pinMode(MISO, OUTPUT);
  digitalWrite(disp_cs, HIGH);
  digitalWrite(MOSI, HIGH);
  digitalWrite(MISO, HIGH);
  digitalWrite(reset, LOW);
  digitalWrite(reset, HIGH);
  //tft.begin(115200); // start display communication
  //tft.cursorOn(false);
  //tft.terminalOn(false);
  //tft.setDisplayColor(EA_WHITE, EA_BLACK);
  //tft.setTextColor(EA_WHITE, EA_TRANSPARENT);
  //tft.setTextSize(5, 8);
  //tft.clear();

 // gear_box.update_label(get_label(VAL_GEAR));
 // left_box.update_label(get_label(VAL_FIRST_LEFT_BOX));
 // right_box.update_label(get_label(VAL_RPM));
 
   clearcounter = 0; //clearen des display nach definierter Zeit
 cleared =true;
//
  }
 clearcounter+=1;
}

void alarm(String textstr) {
  uint8_t x = 1;
  ;
  char text[7];
  textstr.toCharArray(text, 7);
  tft.setTextSize(8, 8);
  while (x == 1) {
    if (!tft.disconnected) {
      tft.setTextColor(EA_BLACK, EA_RED);
      tft.fillDisplayColor(EA_RED);
      tft.drawText(5, 68, 'L', text);
    }
    for (int j = 0; j < 16; j++) {
      digitalWrite(led_s[j], HIGH);
    }
    delay(100);
    if (!tft.disconnected) {
      tft.setTextColor(EA_BLACK, EA_WHITE);
      tft.fillDisplayColor(EA_WHITE);
      tft.drawText(5, 68, 'L', text);
    }
    for (int j = 0; j < 16; j++) {
      digitalWrite(led_s[j], LOW);
    }
    delay(100);
    if (Stw_data.buttonState1 & Stw_data.buttonState4) {
      x = 0;
      tft.setTextColor(EA_WHITE, EA_TRANSPARENT);
    }
  }
}

void redraw_view_driver() {
  tft.setTextColor(EA_WHITE, EA_TRANSPARENT);

  // Boxes
  tft.drawLine(0, 110, 320, 110);
  tft.drawLine(120, 0, 120, 110);
  tft.drawLine(200, 0, 200, 110);

  // Tire temperature cross
  tft.drawLine(80, 180, 240, 180);
  tft.drawLine(160, 130, 160, 230);

  // Boxes
  gear_box.redraw();
  left_box.redraw();
  right_box.redraw();
  autoshift_box.redraw();
  fl_box.redraw();
  fr_box.redraw();
  rl_box.redraw();
  rr_box.redraw();
}

void update_view_driver() {
  uint8_t prev_autoshift = Vehicle_data.autoshift;
  static Value left_box_value = VAL_FIRST_LEFT_BOX;
  if (Stw_data.button4_rises > 0) {
    Stw_data.button4_rises--;
    if (left_box_value == VAL_LAST) {
      left_box_value = VAL_FIRST_LEFT_BOX;
    } else {
      left_box_value = (Value)(left_box_value + 1);
    }
    left_box.update_label(get_label(left_box_value));
  }
  if (Stw_data.button1_rises > 0) {
    Stw_data.button1_rises--;
    if (left_box_value == VAL_FIRST_LEFT_BOX) {
      left_box_value = VAL_LAST;
    } else {
      left_box_value = (Value)(left_box_value - 1);
    }
    left_box.update_label(get_label(left_box_value));
  }

  // These can change rapidly, which would lead to a lot of flickering if
  // rendered in the clear-redraw method. So instead, they're simply overwritten
  // with a black background.
  tft.setTextColor(EA_WHITE, EA_BLACK);
  left_box.update_value(pad_left(get_value(left_box_value), 5));
  right_box.update_value(pad_left(get_value(VAL_RPM), 5));
  // Vehicle_data.autoshift = true;
  if(Vehicle_data.autoshift == true){
    tft.drawRectf(164+100, 184, 240+100, 230, TT_COL3);
  }
  else{
    tft.drawRectf(164+100, 184, 240+100, 230, EA_BLACK);
  }
  autoshift_box.update_value(pad_left(String(Vehicle_data.autoshift), 5));
  
  // These don't change as rapidly, and would overwrite adjacent elements
  // (lines/labels) if rendered with a background because of the empty pixels
  // above/below the characters. So they're rendered using the clear-redraw
  // method.0
  tft.setTextColor(EA_WHITE, EA_TRANSPARENT);
  //gear_box.update_value(get_value(VAL_GEAR));
  fl_box.update_value(get_value(VAL_TT_FL).toInt());
  fr_box.update_value(get_value(VAL_TT_FR).toInt());
  rl_box.update_value(get_value(VAL_TT_RL).toInt());
  rr_box.update_value(get_value(VAL_TT_RR).toInt());
  //Vehicle_data.autoshift = true; //For testing only!!
  if(Vehicle_data.autoshift == true){
    tft.setTextColor(EA_ORANGE, EA_TRANSPARENT);
  }
  gear_box.update_value(get_value(VAL_GEAR));
}

void redraw_view_testing() {
  tft.clear();
  tft.setTextFont(EA_FONT7X12);
  tft.setTextSize(2, 2);
  int start = 10 * testing_page;

  tft.setTextColor(EA_WHITE, EA_BLACK);
  for (int i = start; i <= min(VAL_LAST, start + 9); i += 2) {
    redraw_label_testing(i, EA_BLACK);
  }
  tft.setTextColor(EA_WHITE, EA_DARKGREY);
  for (int i = start + 1; i <= min(VAL_LAST, start + 9); i += 2) {
    redraw_label_testing(i, EA_DARKGREY);
  }

  update_view_testing();
}

void update_view_testing() {
  if (Stw_data.button4_rises > 0) {
    Stw_data.button4_rises--;
    testing_page++;
    if (testing_page * 10 > VAL_LAST) {
      testing_page = 0;
    }
    redraw_view_testing();
  }
  if (Stw_data.button1_rises > 0) {
    Stw_data.button1_rises--;
    testing_page--;
    if (testing_page < 0) {
      testing_page = VAL_LAST / 10;
    }
    redraw_view_testing();
  }

  tft.setTextFont(EA_FONT7X12);
  tft.setTextSize(2, 2);

  int start = 10 * testing_page;

  tft.setTextColor(EA_WHITE, EA_BLACK);
  for (int i = start; i <= min(VAL_LAST, start + 9); i += 2) {
    update_value_testing(i);
  }
  tft.setTextColor(EA_WHITE, EA_DARKGREY);
  for (int i = start + 1; i <= min(VAL_LAST, start + 9); i += 2) {
    update_value_testing(i);
  }
}

void redraw_label_testing(int i, uint8_t color) {
  String text = get_label((Value)i) + ":";
  int y = (i % 10) * 24;
  tft.drawRectf(0, y, 320, y + 23, color);
  tft.drawText(10, y, 'L', text.c_str());
}

void update_value_testing(int i) {
  String text = pad_left(get_value((Value)i), 5);
  int y = (i % 10) * 24;
  tft.drawText(310, y, 'R', text.c_str());
}

DataBox::DataBox(int x1, int y1, int x2, int y2, int text_x, int text_y,
                 int font, int size_x, int size_y, uint8_t justification,
                 bool do_clear)
    : x1{x1}, y1{y1}, x2{x2}, y2{y2}, text_x{text_x}, text_y{text_y},
      font{font}, size_x{size_x}, size_y{size_y},
      justification{justification}, do_clear{do_clear}, value{""}, label{""} {}

void DataBox::update_value(String val_new) {
  if (!val_new.equals(value)) {
    value = val_new;
    redraw_value();
  }
}

void DataBox::update_label(String label_new) {
  if (!label_new.equals(label)) {
    label = label_new;
    redraw_label();
  }
}

void DataBox::redraw() {
  redraw_value();
  redraw_label();
}

void DataBox::redraw_value() {
  tft.setTextFont(font);
  tft.setTextSize(size_x, size_y);
  Serial.println("Redrawing value:");
  if (do_clear) {
    tft.clearRect(x1, y1, x2, y2);
  }
  tft.drawText(text_x, text_y, justification, value.c_str());
}

void DataBox::redraw_label() {
  tft.setTextFont(EA_FONT7X12);
  tft.setTextSize(1, 1);
  Serial.println("Redrawing label:");
  tft.clearRect(x1, y2 + 1, x2, y2 + 13);
  tft.drawText((x1 + x2) / 2, y2 + 1, 'C', label.c_str());
}

TireTempBox::TireTempBox(int x1, int y1, int x2, int y2, int text_x, int text_y,
                         int font, int size_x, int size_y,
                         uint8_t justification)
    : DataBox{x1,     y1,   x2,     y2,     text_x,
              text_y, font, size_x, size_y, justification,
              false},
      num_value{-1} {}

void TireTempBox::update_value(int val_new) {
  if (val_new != num_value) {
    num_value = val_new;
    if (val_new < TT_THRESH1) {
      color = TT_COL0;
    } else if (val_new < TT_THRESH2) {
      color = TT_COL1;
    } else if (val_new < TT_THRESH3) {
      color = TT_COL2;
    } else {
      color = TT_COL3;
    }
    String val_str = pad_left(String(val_new), 3);
    DataBox::update_value(val_str);
  }
}

void TireTempBox::redraw_value() {
  tft.setTextFont(font);
  tft.setTextSize(size_x, size_y);
  tft.drawRectf(x1, y1, x2, y2, color);
  tft.drawText(text_x, text_y, justification, value.c_str());
}

void TireTempBox::redraw_label() {}