byte seg = 0;
byte segA = 11;
byte segB = 7;
byte segC = 4;
byte segD = 2;
byte segE = 1;
byte segF = 10;
byte segG = 5;
byte segDP = 3;

byte digit1 = 12;
byte digit2 = 9;
byte digit3 = 8;
byte digit4 = 6;
byte digits_available = 4; // Amount of available digits on display

//                    1,    2,    3,    4,    5,    6,    7,    8,    9,    10,   11,   12
byte pin_map[] = {-1, P2_4, P2_5, P1_6, P1_7, P1_4, P1_3, P1_5, P2_0, P2_1, P2_2, 19, P2_3};
byte char_map[128]; // Fragmented set of first 128 characters of ASCII table.
byte seg_map[] = {segA, segB, segC, segD, segE, segF, segG, segDP};
byte digits_map[4] = {digit1, digit2, digit3, digit4};


void setup() {
  P2SEL &= ~(BIT6|BIT7);
  P2DIR |= (BIT6|BIT7);
  P2OUT |= BIT7;
  P2OUT |= BIT6;

  char_map['0'] = B11111100;
  char_map['1'] = B01100000;
  char_map['2'] = B11011010;
  char_map['3'] = B11110010;
  char_map['4'] = B01100110;
  char_map['5'] = B10110110;
  char_map['6'] = B10111110;
  char_map['7'] = B11100000;
  char_map['8'] = B11111110;
  char_map['9'] = B11110110;

  char_map['H'] = B01101110;
  char_map['L'] = B00011100;
  char_map['E'] = B10011110;
  char_map['R'] = B11101110;


  Serial.begin(9600);
  int i;

  for (i = 1; i < sizeof(pin_map); i++) {
    pinMode(pin_map[i], OUTPUT);
  }
}


void turnSegOff(int seg) {
  digitalWrite(pin_map[seg], HIGH);
}

void turnDisplayOff() {
  int i;


  for (i = 1; i < sizeof(pin_map); i++) {
    digitalWrite(pin_map[i], LOW);
  }
}

void turnSegOn(int seg) {
  digitalWrite(pin_map[seg], LOW);
}

void turnAllOff() {
  byte i;

  for (i = 0; i < 8; i++) {
      turnSegOff(seg_map[i]);
  }
}

//String floatToString(float f) {
//  char temp[10];
//
//  dtostrf(f, 1, 2, temp);
//  return String(temp);
//}

void displayChar(char c, byte digit, boolean with_dot = false) {
  byte i;
  char mask = char_map[c];

  turnDisplayOff();
  digitalWrite(pin_map[digits_map[digit]], LOW);

  if (mask) {
    if (with_dot) {
      mask = mask | 1;
    }

    for (i = 0; i < 8; i++) {
      if ((mask & (1 << i)) == (1 << i)) {
        turnSegOn(seg_map[7 - i]);
      }
      else {
        turnSegOff(seg_map[7 - i]);
      }
    }
    digitalWrite(pin_map[digits_map[digit]], HIGH);
    delay(3);
  } else {
    turnAllOff();
  }
}

void displayString(String s) {
  String stripped = s;

  stripped.replace(".", "");

  int i,
    len = s.length(),
    slen = stripped.length(),
    correction = 0;

  len = min(len, digits_available + len - slen);
  slen = min(slen, digits_available);

  for (i = 0; i < len; i++) {
    if (s[i] == '.') {
      correction++;
    } else {
      displayChar(s[i],  // Character to display
                  digits_available - (slen - i) - correction, // Position
                  i + 1 < len && s[i + 1] == '.'); // Should we show the dot?
    }
  }
}

char buffer[5];
byte pos = 0;
String message = " L0H";

void loop() {
  displayString(message);

  if (Serial.available()) {
    buffer[pos] = Serial.read();
    pos++;

    if (pos == 4) {
      buffer[4] = 0;
      Serial.println(buffer);
      message = buffer;
      pos = 0;
    }
  }
}