#include <string.h>
#include <math.h>

//LCD Screen
#define D0 PA12
#define D1 PB11
#define D2 PF1
#define D3 PF0
#define D4 PC14
#define D5 PC13
#define D6 PC6
#define D7 PA13
#define E1 PB1
#define E2 PA0
#define R_S PA14
#define keyboard_check ((key < '0') || (key > '9')) && ((key != '\n') && (key != '\b')) && (key != '.')
#define keyboard_waveform ((key < '0') || (key > '2')) && ((key != '\n') && (key != '-')) || ((key == 'D') || (key == 'a') || (key == 'f') || (key == '.') || (key == 'K') || (key == 'M') || (key == '\b')) 
//

//Keyboard
#define row_1 PA10
#define row_2 PB5
#define row_3 PB10
#define row_4 PA8

#define col_1 PA7
#define col_2 PA6
#define col_3 PA5
#define col_4 PB9
#define col_5 PB7
// 

uint16_t amplitude_value = 819;
uint64_t frequency_value = 42950;
uint16_t dc_value = 819;
uint16_t symmetry_value = 16384;
uint16_t duty_cycle_value = 16384;
uint8_t waveform_type = 1;


uint8_t bitmask = 0;
uint8_t row_state[4] = {};
char key_array_amplitude[6] = "1.000";
char key_array_frequency[9] = "1.000000";
char key_array_dc[7] = "1.000";
char key_waveform = '1';
char key_array_DC[5] = "50.0";
char key_array_sym[5] = "50.0";

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, STM32!");

  //Data pins
  pinMode(D0, OUTPUT);
  pinMode(D1, OUTPUT); 
  pinMode(D2, OUTPUT);
  pinMode(D3, OUTPUT); 
  pinMode(D4, OUTPUT);
  pinMode(D5, OUTPUT); 
  pinMode(D6, OUTPUT);
  pinMode(D7, OUTPUT); 
  //

  //Enable pins
  pinMode(E1, OUTPUT);
  pinMode(E2, OUTPUT);
  //

  //Register Select pin
  pinMode(R_S, OUTPUT);
  //

  //Startup sequence
  digitalWrite(E1, LOW);
  digitalWrite(E2, LOW);
  digitalWrite(R_S, LOW);
  delay(45);

  //Wakeup calls
  SetDataPins(0b00110000);
  PulseEnables(5);
  PulseEnables(1);
  PulseEnables(1);
  //

  //Start in 8bt mode
  SetDataPins(0b00111000);
  PulseEnables(1);
  //

  //Display off
  SetDataPins(0b00001000);
  PulseEnables(1);
  //

  //Clear display
  SetDataPins(0b00000001);
  PulseEnables(2);
  //

  //Auto increment cursor to the right
  SetDataPins(0b0000011);
  PulseEnables(1);
  //

  //Start display
  SetDataPins(0b00001100);
  PulseEnables(1);
  //

  //Output pins for keyboard 4x4 (rows)
  pinMode(row_1, OUTPUT);
  pinMode(row_2, OUTPUT);
  pinMode(row_3, OUTPUT);
  pinMode(row_4, OUTPUT);

  //Input pins for keyboard 4x4 (columns)
  pinMode(col_1, INPUT_PULLDOWN);
  pinMode(col_2, INPUT_PULLDOWN);
  pinMode(col_3, INPUT_PULLDOWN);
  pinMode(col_4, INPUT_PULLDOWN);
  pinMode(col_5, INPUT_PULLDOWN);


  //Default start
  digitalWrite(row_1, LOW);
  digitalWrite(row_2, LOW);
  digitalWrite(row_3, LOW);
  digitalWrite(row_4, LOW);

  //Amplitude:
  SelectRowAndColumn(0, 0);
  SendArray("Amplitude: ", 1);

  //Frequency:
  SelectRowAndColumn(1, 0);
  SendArray("Frequency: ", 1);

  //DC offset:
  SelectRowAndColumn(0, 20);
  SendArray("DC offset: ", 2);

  //Waveform:
  SelectRowAndColumn(2, 0);
  SendArray("Waveform: ", 1);

  //Set default value
  amplitude_value = 819;
  frequency_value = 42950;
  dc_value = 819;
  waveform_type = 1;
  duty_cycle_value = 16384;
  symmetry_value = 16384;

  //Set default value LCD for amplitude
  SelectRowAndColumn(0, 11);
  SendArray("1.000 V", 1);
  
  //Set default value LCD for frequency
  SelectRowAndColumn(1, 11);
  for (uint8_t i = 0; i < 8; i++)
    SendCharacters(key_array_frequency[i], 1);
  SelectRowAndColumn(1, 20);
  SendArray("KHz", 2);

  //Set default value LCD for DC offset
  SelectRowAndColumn(0, 31);
  SendArray("1.000 V", 2);

  //Set default waveform shape LCD
  SelectRowAndColumn(2, 11);
  SendArray("Sine", 1);
}

void SetDataPins(uint8_t data)
{
  const uint8_t data_pins[] = {D0, D1, D2, D3, D4, D5, D6, D7};

  for (int i = 0; i <= 7; i++)
  {
    digitalWrite(data_pins[i], (data >> i) & 1);
  }
}

void PulseEnable_1(int time_ms)
{
  digitalWrite(E1, HIGH);
  delay(1);
  digitalWrite(E1, LOW);
  delay(time_ms);
}

void PulseEnable_2(int time_ms)
{
  digitalWrite(E2, HIGH);
  delay(1);
  digitalWrite(E2, LOW);
  delay(time_ms);
}

void PulseEnables(int time_ms)
{
  digitalWrite(E1, HIGH);
  digitalWrite(E2, HIGH);
  delay(1);
  digitalWrite(E1, LOW);
  digitalWrite(E2, LOW);
  delay(time_ms);
}

void SendCharacters(uint8_t data, uint8_t EN)
{
  digitalWrite(R_S, HIGH);
  SetDataPins(data);
  switch (EN)
  {
    case (1):
      PulseEnable_1(1);
      break;
    case (2): 
      PulseEnable_2(1);
      break;
  }
}

void SendArray(char data_array[], uint8_t EN)
{
  for (uint8_t i = 0; (data_array[i] != 0) && (i < 20); i++)
    SendCharacters(data_array[i], EN);
}

void SelectRowAndColumn(uint8_t row, uint8_t column)
{
  const uint8_t row_data[] = {0, 64, 20, 84};
  if (column < 20)
    SendCommands(128 | (row_data[row] + column), 1);
  else
    SendCommands(128 | (row_data[row] + column - 20), 2);
}

void SendCommands(uint8_t data, uint8_t EN_12)
{
  digitalWrite(R_S, LOW);
  SetDataPins(data);
  switch (EN_12)
  {
    case (1): 
      PulseEnable_1(2);
      break;
    case (2): 
      PulseEnable_2(2);
      break;
    case (3):
      PulseEnables(2);
      break;
  }
}

uint8_t ReadKeyboard()
{ 
  char pressed_key = 0;
  //Read first row
  digitalWrite(row_4, LOW);
  digitalWrite(row_1, HIGH);
  bitmask = digitalRead(col_1) + (digitalRead(col_2) << 1) + (digitalRead(col_3) << 2) + (digitalRead(col_4) << 3);
  if (bitmask != row_state[0])
  {
    delay(40);
    switch (bitmask)
    {
      case (0):
        break;
      case (1):
        Serial.println("1");
        pressed_key = '1';
        break;
      case (2):
        Serial.println("2");
        pressed_key = '2';
        break;
      case (4):
        Serial.println("3");
        pressed_key = '3';
        break;
      case (8):
        Serial.println("a");
        pressed_key = 'a';
        break;
    }

    if (row_state[0] != bitmask)
    {
      row_state[0] = bitmask;
      digitalWrite(row_1, LOW);
      return pressed_key;
    }
  }
  //

  //Read second row
  digitalWrite(row_1, LOW);
  digitalWrite(row_2, HIGH);
  bitmask = digitalRead(col_1) + (digitalRead(col_2) << 1) + (digitalRead(col_3) << 2) + (digitalRead(col_4) << 3) + (digitalRead(col_5) << 4);
  if (bitmask != row_state[1])
  {
    delay(40);
    switch (bitmask)
    {
      case (0):
        break;
      case (1):
        Serial.println("4");
        pressed_key = '4';
        break;
      case (2):
        Serial.println("5");
        pressed_key = '5';
        break;
      case (4):
        Serial.println("6");
        pressed_key = '6';
        break;
      case (8):
        Serial.println("f");
        pressed_key = 'f';
        break;
      case (16):
        Serial.println("M");
        pressed_key = 'M';
        break;
    }

    if (row_state[1] != bitmask)
    {
      row_state[1] = bitmask;
      digitalWrite(row_2, LOW);
      return pressed_key;
    }
  }
  //

  //Read third row
  digitalWrite(row_2, LOW);
  digitalWrite(row_3, HIGH);
  bitmask = digitalRead(col_1) + (digitalRead(col_2) << 1) + (digitalRead(col_3) << 2) + (digitalRead(col_4) << 3) + (digitalRead(col_5) << 4);
  if (bitmask != row_state[2])
  {
    delay(40);
    switch (bitmask)
    {
      case (0):
        break;
      case (1):
        Serial.println("7");
        pressed_key = '7';
        break;
      case (2):
        Serial.println("8");
        pressed_key = '8';
        break;
      case (4):
        Serial.println("9");
        pressed_key = '9';
        break;
      case (8):
        Serial.println("0");
        pressed_key = '0';
        break;
      case (16):
        Serial.println("K");
        pressed_key = 'K';
        break;
    }

    if (row_state[2] != bitmask)
    {
      row_state[2] = bitmask;
      digitalWrite(row_3, LOW);
      return pressed_key;
    }
  }

  //Read third row
  digitalWrite(row_3, LOW);
  digitalWrite(row_4, HIGH);
  bitmask = digitalRead(col_1) + (digitalRead(col_2) << 1) + (digitalRead(col_3) << 2) + (digitalRead(col_4) << 3) + (digitalRead(col_5) << 4);
  if (bitmask != row_state[3])
  {
    delay(40);
    switch (bitmask)
    {
      case (0):
        break;
      case (1):
        Serial.println("\n");
        pressed_key = '\n';
        break;
      case (2):
        Serial.println("\b");
        pressed_key = '\b';
        break;
      case (4):
        Serial.println(".");
        pressed_key = '.';
        break;
      case (8):
        Serial.println("-");
        pressed_key = '-';
        break;
      case (16):
        Serial.println("D");
        pressed_key = 'D';
        break;
    }

    if (row_state[3] != bitmask)
    {
      row_state[3] = bitmask;
      digitalWrite(row_4, LOW);
      return pressed_key;
    }
  }

  return pressed_key;
}

void SelectMode()
{
  char key;
  char key_array[9] = {0};
  double input_value = 0;
  int8_t dot_index = 0;
  double multiplier = 10.0;

  key = ReadKeyboard();
  int8_t count_press = 0;
  if (key == 'a')
  {
    SelectRowAndColumn(0, 11);

    //Clear portion of the screen
    for(int i = 0; i < 8; i++) 
      SendCharacters(0b00100000, 1); 
    
    SelectRowAndColumn(0, 11);

    
    while (1)
    {
      key = 0;
      //Check what key was pressed
      while (keyboard_check && (key != 'a') || ((key == 'f') || (key == 'D') || (key == 'K') || (key == '-'))) 
        key = ReadKeyboard();

      if (key == 'a')
      {
        SelectRowAndColumn(0, 11);

        //Clear portion of the screen
        for (int i = 0; i < 8; i++) 
        SendCharacters(0b00100000, 1); 
    
        SelectRowAndColumn(0, 11);
        //Rewrite the old value
        SendArray(key_array_amplitude, 1);

        SelectRowAndColumn(0, 16);
        SendCharacters(' ', 1);
        if (amplitude_value > 737)
          SendCharacters('V', 1);
        else
          SendArray("mv", 1);

        return;
      }
      else if ((key == '.') && (count_press == 1) && (dot_index == 0) || ((count_press == 2) && (key_array[0] == '-') && (dot_index == 0) && (key == '.')))
        dot_index = count_press;
      else if (key == '.') 
        continue;
      //Interpret the backspace in an array
      else if ((key == '\b') && (count_press > 0))
      {
        count_press--;
        if ((dot_index != 0) && (dot_index == count_press))
          dot_index = 0;
        SelectRowAndColumn(0, 11 + count_press);
        SendCharacters(' ', 1);
        SelectRowAndColumn(0, 11 + count_press);
        key_array[count_press] = 0;
      }
      //Check if the value should be enterd
      else if(key == '\n')
      {
        if ((count_press == 1) || (count_press == 2) && (dot_index == 0))
        {
          SendArray(".000", 1);
          for (uint8_t i = count_press; i < 6; i++)
            if (i == (count_press))
              key_array[i] = '.';
            else
              key_array[i] = '0';
        }
        else if (dot_index != 0)
          switch (count_press - dot_index)
          {
            case (1):
              SendArray("000", 1);
              for (uint8_t i = count_press; i < count_press + 3; i++)
                key_array[i] = '0';
              break;
            case (2):
              SendArray("00", 1);
              for (uint8_t i = count_press; i < count_press + 2; i++)
                key_array[i] = '0';
              break;
            case (3):
              SendCharacters('0', 1);
                key_array[count_press] = '0';
              break;
          }
        break;
      }
      
      if ((count_press < 5) && (key != '\b'))
      {
        key_array[count_press] = key;

        //Send to LCD screen
        SendCharacters(key, 1);
        count_press++;
      }
    }
    
    //The non fractionary part
    if (key_array[1] != '.')
    {
      strcpy(key_array, "5.000");
      input_value = 5.0;
    }
    else
      input_value = key_array[0] - '0';

    //The fractionary part
    for (uint8_t i = 2; i < 5; i++)
    {
      input_value = input_value + (key_array[i] - '0')/multiplier;
      multiplier = multiplier * 10;
    }
    
    //Amplitude 
    if (input_value > 5.0)
    {
      input_value = 5.0;
      strcpy(key_array, "5.000");
      SelectRowAndColumn(0, 11);

      //Clear portion of the screen
      for(int i = 0; i < 7; i++) 
        SendCharacters(0b00100000, 1); 
    
      SelectRowAndColumn(0, 11);
      
      SendArray(key_array, 1);
    }

    amplitude_value = round((input_value) * 4096.0 / 5.0);
    if (amplitude_value > 4095)
      amplitude_value = 4095;
    input_value = amplitude_value / 4096.0 * 5.0;

    key_array[0] = (uint8_t)(input_value) + '0';
    key_array[1] = '.';
    multiplier = 10;
    for (uint8_t i = 2; i < 5; i++)
    {
      key_array[i] = (uint16_t)(input_value * multiplier) % 10 + '0';
      multiplier = multiplier * 10;
    }
    key_array[5] = 0;


    if (amplitude_value > 737)
    {
      SelectRowAndColumn(0, 11);

      //Clear portion of the screen
      for (uint8_t i = 0; i < 8; i++) 
        SendCharacters(0b00100000, 1); 
    
      SelectRowAndColumn(0, 11);

      strncpy(key_array_amplitude, key_array, sizeof(key_array_amplitude) - 1);
      key_array_amplitude[sizeof(key_array_amplitude) - 1] = 0;

      SendArray(key_array_amplitude, 1);
      
      SelectRowAndColumn(0, 16);
      SendArray(" V ", 1);
    }
    else
    {
      SelectRowAndColumn(0, 11);

      //Clear portion of the screen
      for (uint8_t i = 0; i < 6; i++) 
        SendCharacters(0b00100000, 1); 
    
      SelectRowAndColumn(0, 11);

      strncpy(key_array_amplitude, key_array, sizeof(key_array_amplitude) - 1);
      key_array_amplitude[sizeof(key_array_amplitude) - 1] = 0;

      for (uint8_t i = 0; i < 5; i++)
        key_array_amplitude[i] = key_array_amplitude[i + 1];

      for (uint8_t i = 0; i < 4; i++)
        key_array_amplitude[i] = key_array_amplitude[i + 1];

      if (key_array_amplitude[0] == '0')
        for (uint8_t i = 0; i < 3; i++)
          key_array_amplitude[i] = key_array_amplitude[i + 1];

      if (key_array_amplitude[0] == '0')
        for (uint8_t i = 0; i < 2; i++)
          key_array_amplitude[i] = key_array_amplitude[i + 1];

      for (uint8_t i = 0; i < 7; i++)
        SendCharacters(key_array_amplitude[i], 1);
      
      SelectRowAndColumn(0, 16);
      SendArray(" mV", 1);
    }
  }
  else if (key == 'D')
  {
    SelectRowAndColumn(0, 31);

    //Clear portion of the screen
    for(int i = 0; i < 8; i++) 
      SendCharacters(0b00100000, 2); 
    
    SelectRowAndColumn(0, 31);

    
    while (1)
    {
      key = 0;
      //Check what key was pressed
      while (keyboard_check && (key != '-') && (key != 'D') || ((key == 'f') || (key == 'a') || (key == 'D') || (key == 'K'))) 
        key = ReadKeyboard();

      if (key == 'D')
      {
        SelectRowAndColumn(0, 31);

        //Clear portion of the screen
        for (int i = 0; i < 6; i++) 
        SendCharacters(0b00100000, 2); 
    
        SelectRowAndColumn(0, 31);
        //Rewrite the old value
        for (uint8_t i = 0; i < 6; i++)
        {
          if (key_array_dc[i] == 0)
            break;
          SendCharacters(key_array_dc[i], 1);
        }

        SelectRowAndColumn(0, 37);
        SendCharacters(' ', 2);
        if ((dc_value > 2417) || (dc_value < 1679))
          SendCharacters('V', 2);
        else
          SendArray("mV", 2);

        return;
      }
      else if ((key == '-') && (key_array[0] != '-'))
      {
        for (int8_t i = count_press - 1; i >= 0; i--)
          key_array[i + 1] = key_array[i];
        key_array[0] = key;
        
        SelectRowAndColumn(0, 31);

        //Clear portion of the screen
        for (int i = 0; i < 7; i++) 
          SendCharacters(0b00100000, 2); 
    
        SelectRowAndColumn(0, 31);

        if (dot_index != 0)
          dot_index++;
        count_press++;

        for (uint8_t i = 0; i < count_press; i++)
          SendCharacters(key_array[i], 2);
        continue;
      }
      else if ((key == '-') && (key_array[0] == '-'))
      {
        for (int8_t i = 0; i < count_press; i++)
          key_array[i] = key_array[i + 1];

        SelectRowAndColumn(0, 31);

        //Clear portion of the screen
        for (int i = 0; i < 7; i++) 
          SendCharacters(0b00100000, 2); 
    
        SelectRowAndColumn(0, 31);

        if (dot_index != 0)
          dot_index--;

        count_press--;

        for (uint8_t i = 0; i < count_press; i++)
          SendCharacters(key_array[i], 2);

        SelectRowAndColumn(0, 31 + count_press);
        SendCharacters(' ', 2);
        SelectRowAndColumn(0, 31 + count_press);
        key_array[count_press] = 0;

        continue;
      }
      else if ((key == '.') && (count_press == 1) && (dot_index == 0) || ((count_press == 2) && (key_array[0] == '-') && (dot_index == 0) && (key == '.')))
        dot_index = count_press;
      else if (key == '.') 
        continue;
      //Interpret the backspace in an array
      else if ((key == '\b') && (count_press > 0))
      {
        count_press--;
        if ((dot_index != 0) && (dot_index == count_press))
          dot_index = 0;
        SelectRowAndColumn(0, 31 + count_press);
        SendCharacters(' ', 2);
        SelectRowAndColumn(0, 31 + count_press);
        key_array[count_press] = 0;
      }
      //Check if the value should be enterd
      else if(key == '\n')
      {
        if ((count_press == 1) || (count_press == 2) && (dot_index == 0))
        {
          SendArray(".000", 2);
          for (uint8_t i = count_press; i < 6; i++)
            if (i == (count_press))
              key_array[i] = '.';
            else
              key_array[i] = '0';
        }
        else if (dot_index != 0)
          switch (count_press - dot_index)
          {
            case (1):
              SendArray("000", 2);
              for (uint8_t i = count_press; i < count_press + 3; i++)
                key_array[i] = '0';
              break;
            case (2):
              SendArray("00", 2);
              for (uint8_t i = count_press; i < count_press + 2; i++)
                key_array[i] = '0';
              break;
            case (3):
              SendCharacters('0', 2);
                key_array[count_press] = '0';
              break;
          }
        break;
      }
      
      if (((count_press < 5) || ((count_press < 6) && (key_array[0] == '-'))) && (key != '\b'))
      {
        key_array[count_press] = key;

        //Send to LCD screen
        SendCharacters(key, 2);
        count_press++;
      }
    }
    
    multiplier = 10.0;

    //The non fractionary part
    if ((key_array[0] == '-') && (key_array[2] != '.'))
    {
      strcpy(key_array, "-5.000");
      input_value = -5.0;
    }
    else if (key_array[0] == '-')
    {
      input_value = key_array[1] - '0';
      for (uint8_t  i = 3; i < 6; i++)
      {
        input_value = input_value + (key_array[i] - '0')/multiplier;
        multiplier = multiplier * 10;
      }
      key_array[6] = 0;
      input_value = input_value * (-1);
    }
    else if ((key_array[0] != '-') && (key_array[1] != '.'))
    {
      strcpy(key_array, "5.000");
      input_value = 5.0;
    }
    else if (key_array[0] != '-')
    {
      input_value = key_array[0] - '0';
      for (uint8_t  i = 2; i < 5; i++)
      {
        input_value = input_value + (key_array[i] - '0')/multiplier;
        multiplier = multiplier * 10;
      }
      key_array[5] = 0;
    }

    //DC


    dc_value = round((input_value + 5.0) * 4096.0 / 10.0);
    if (dc_value > 4095)
      dc_value = 4095;
    input_value = dc_value / 4096.0 * 10.0 - 5.0;

    if (input_value < 0.0)
    {
      key_array[0] = '-';
      key_array[1] = (uint8_t)((-1.0) * input_value) + '0';
      key_array[2] = '.';
      multiplier = 10;
      for (uint8_t i = 3; i < 6; i++)
      {
        key_array[i] = (uint16_t)((-1.0) * input_value * multiplier) % 10 + '0';
        multiplier = multiplier * 10;
      }
      key_array[6] = 0;
    }
    else
    {
      key_array[0] = (uint8_t)(input_value) + '0';
      key_array[1] = '.';
      multiplier = 10;
      for (uint8_t i = 2; i < 5; i++)
      {
        key_array[i] = (uint16_t)(input_value * multiplier) % 10 + '0';
        multiplier = multiplier * 10;
      }
      key_array[5] = 0;
    }

    if ((input_value >= 0.9) || (input_value < -0.9))
    {
      SelectRowAndColumn(0, 31);

      //Clear portion of the screen
      for (uint8_t i = 0; i < 8; i++) 
        SendCharacters(0b00100000, 2); 
    
      SelectRowAndColumn(0, 31);

      strncpy(key_array_dc, key_array, sizeof(key_array_dc) - 1);
      key_array_dc[sizeof(key_array_dc) - 1] = 0;

      for (uint8_t i = 0; i < 7; i++)
      {
        SendCharacters(key_array_dc[i], 2);
      }
      
      SelectRowAndColumn(0, 37);
      SendArray(" V ", 2);
    }
    else
    {
      SelectRowAndColumn(0, 31);

      //Clear portion of the screen
      for (uint8_t i = 0; i < 6; i++) 
        SendCharacters(0b00100000, 2); 
    
      SelectRowAndColumn(0, 31);

      strncpy(key_array_dc, key_array, sizeof(key_array_dc) - 1);
      key_array_dc[sizeof(key_array_dc) - 1] = 0;

      if (input_value < 0.0)
      {
        for (uint8_t i = 1; i < 6; i++)
          key_array_dc[i] = key_array_dc[i + 1];

        for (uint8_t i = 1; i < 5; i++)
          key_array_dc[i] = key_array_dc[i + 1];

        if (key_array_dc[1] == '0')
          for (uint8_t i = 1; i < 4; i++)
            key_array_dc[i] = key_array_dc[i + 1];

        if (key_array_dc[1] == '0')
          for (uint8_t i = 1; i < 3; i++)
            key_array_dc[i] = key_array_dc[i + 1];
      }

      else
      {
        for (uint8_t i = 0; i < 5; i++)
          key_array_dc[i] = key_array_dc[i + 1];

        for (uint8_t i = 0; i < 4; i++)
          key_array_dc[i] = key_array_dc[i + 1];

        if (key_array_dc[0] == '0')
          for (uint8_t i = 0; i < 3; i++)
            key_array_dc[i] = key_array_dc[i + 1];

        if (key_array_dc[0] == '0')
          for (uint8_t i = 0; i < 2; i++)
            key_array_dc[i] = key_array_dc[i + 1];
      }
      SendArray(key_array_dc, 2);
      
      SelectRowAndColumn(0, 37);
      SendArray(" mV", 2);
    }
  }
  else if (key == 'f')
  {
    SelectRowAndColumn(1, 11);

    //Clear portion of the screen
    for(int i = 0; i <= 7; i++) 
      SendCharacters(0b00100000, 1); 
    
    SelectRowAndColumn(1, 11);

    while (1)
    {
      key = 0;

      //Check what key was pressed
      while (keyboard_check && (key != 'f') || ((key == 'D') || (key == 'a') || (key == 'D') || (key == 'K') || (key == '-')))
        key = ReadKeyboard();

      //Check if the value should be enterd
      if ((key == 'a') || (key == 'f'))
      {
        SelectRowAndColumn(1, 11);

        //Clear portion of the screen
        for(int i = 0; i < 8; i++) 
          SendCharacters(0b00100000, 1); 
    
        SelectRowAndColumn(1, 11);
        //Rewrite the old value
        for (uint8_t i = 0; i < 8; i++)
        {
          if (key_array_frequency[i] == 0)
            break;
          SendCharacters(key_array_frequency[i], 1);
        }
        return;
      }
      else if ((key == '.') && ((count_press >= 1) && (count_press <= 7)) && (dot_index == 0))
        dot_index = count_press;
      else if ((key == '.')) 
        continue;
      //Interpret the backspace in an array
      else if ((key == '\b') && (count_press > 0))
      {
        count_press--;
        if ((dot_index != 0) && (dot_index == count_press))
          dot_index = 0;
        SelectRowAndColumn(1, 11 + count_press);
        SendCharacters(' ', 1);
        SelectRowAndColumn(1, 11 + count_press);
        key_array[count_press] = 0;
      }
      //Check if the value should be enterd
      else if(key == '\n')
      {
        if ((dot_index == 0) && (count_press < 8))
        {
          key_array[count_press] = '.';
          SendCharacters('.', 1);
          for (uint8_t i = count_press + 1; i < 8; i++)
          {
            key_array[i] = '0';
            SendCharacters('0', 1);
          }
          break;
        }
        else
        {
          for (uint8_t i = count_press; i < 8; i++)
          {
            key_array[i] = '0';
            SendCharacters('0', 1);
          }
        }
        break;
      }

      //Record the press in an array
      if ((count_press < 8) && (key != '\b'))
      {
        key_array[count_press] = key;

        //Send to LCD screen
        SendCharacters(key, 1);
        count_press++;
      }
    }

    for (uint8_t i = 0; i < 8; i++)
    {
      if (key_array[i] == '.')
        break;
      input_value = input_value * 10 + (key_array[i] - '0');
    }

    if (dot_index > 0)
      for (uint8_t i = dot_index + 1; i < 8; i++)
      {
        if (key_array[i] == 0)
          break;
        input_value = input_value + (key_array[i] - '0') * multiplier;
        multiplier = multiplier / 10;
      }

    //Frequency
    if (input_value > 10000000)
    {
      input_value = 10000000;
      strcpy(key_array, "10.000");
      SelectRowAndColumn(1, 11);

      //Clear portion of the screen
      for(int i = 0; i < 8; i++) 
        SendCharacters(0b00100000, 1); 
    
      SelectRowAndColumn(1, 11);
      
      for (uint8_t i = 0; i < 6; i++)
        SendCharacters(key_array[i], 1);
    }

    frequency_value = round(input_value * (double)(1ULL << 32) / (10 * 1e6));
    if (frequency_value > ((1ULL << 32) - 1))
      frequency_value = (1ULL << 32) - 1;
    input_value = frequency_value / (double)(1ULL << 32) * (10 * 1e6);
    multiplier = 10;

    double temp = input_value;
    uint8_t count_whole = 0;
    uint8_t count_frac = 0;
    while (temp >= 1.0)
    {
      temp = temp / multiplier;
      count_whole++;
    }

    if (count_whole > 0)
    {
      if (count_whole <= 7)
      {
        for (uint8_t i = 0; i < count_whole; i++)
        {
          key_array[i] = (uint32_t)(temp * multiplier) % 10 + '0';
          multiplier = multiplier * 10.0;
        }
        key_array[count_whole] = '.';
        for (uint8_t i = count_whole + 1; i < 8; i++)
        {
          key_array[i] = (uint32_t)(temp * multiplier) % 10 + '0';
          multiplier = multiplier * 10.0;
        }
      }
      else
      {
        for (uint8_t i = 0; i < 8; i++)
        {
          key_array[i] = (uint32_t)(temp * multiplier) % 10 + '0';
          multiplier = multiplier * 10.0;
        }
      }
    }
    else
    {
      key_array[0] = '0';
      key_array[1] = '.';
      for (uint8_t i = 2; i < 8; i++)
      {
        key_array[i] = (uint32_t)(temp * multiplier) % 10 + '0';
        multiplier = multiplier * 10.0;
      }

      while ((temp < 1.0) && (temp != 0))
      {
        temp = temp * multiplier;
        count_frac++;
      }
    }

    strcpy(key_array_frequency, key_array);

    if (input_value < 0.9)
    {
      SelectRowAndColumn(1, 11);

      //Clear portion of the screen
      for(int i = 0; i < 8; i++) 
        SendCharacters(0b00100000, 1); 
    
      SelectRowAndColumn(1, 11);

      for (uint8_t i = 0; i < count_frac; i++)
        key_array_frequency[i] = key_array_frequency[i + 1];
      
      for (uint8_t i = 0; i < 7; i++)
        if (key_array_frequency[i] == 0)
          break;
        else 
          SendCharacters(key_array_frequency[i], 1);

      if (input_value == 0)
        SendCharacters('0', 1);

      SelectRowAndColumn(1, 20);
      SendArray("mHz", 2);
    }
    else if (input_value < 900.0)
    {
      double temp = input_value;
      SelectRowAndColumn(1, 11);

      //Clear portion of the screen
      for (int i = 0; i < 8; i++) 
        SendCharacters(0b00100000, 1); 
      
      SelectRowAndColumn(1, 11);

      for (uint8_t i = 0; i < 8; i++)
      {
        key_array_frequency[i] = key_array[i];
        SendCharacters(key_array_frequency[i], 1);
      }

      SelectRowAndColumn(1, 20);
      SendArray("Hz ", 2);
    }
    else if (input_value < 900000.0)
    {
      char temp;
      SelectRowAndColumn(1, 11);

      //Clear portion of the screen
      for (int i = 0; i < 8; i++) 
        SendCharacters(0b00100000, 1); 
      
      SelectRowAndColumn(1, 11);
      

      for (uint8_t i = 0; i < 3; i++)
      {
        temp = key_array_frequency[count_whole - i - 1];
        key_array_frequency[count_whole - i - 1] =  key_array_frequency[count_whole - i];
        key_array_frequency[count_whole - i] = temp;
      }

      for (uint8_t i = 0; i < 8; i++)
        SendCharacters(key_array_frequency[i], 1);

      SelectRowAndColumn(1, 20);
      SendArray("KHz", 2);
    }
    else
    {
      char temp;
      SelectRowAndColumn(1, 11);

      //Clear portion of the screen
      for (int i = 0; i < 8; i++) 
        SendCharacters(0b00100000, 1); 
      
      SelectRowAndColumn(1, 11);
      
      if (count_whole < 8)
        for (uint8_t i = 0; i < 6; i++)
        {
          temp = key_array_frequency[count_whole - i - 1];
          key_array_frequency[count_whole - i - 1] =  key_array_frequency[count_whole - i];
          key_array_frequency[count_whole - i] = temp;
        }
      else
        strcpy(key_array_frequency, "10.00000");
    

      for (uint8_t i = 0; i < 8; i++)
        SendCharacters(key_array_frequency[i], 1);

      SelectRowAndColumn(1, 20);
      SendArray("MHz", 2);
    }
 }
 else if (key == '-')
 {
    //Clear Row
    SelectRowAndColumn(2, 11);
    for (uint8_t i = 0; i < 7; i++)
      SendCharacters(' ', 1);
    SelectRowAndColumn(2, 11);

    while (1)
    {
      char key_array[9] = {};
      key = 0;
      while (keyboard_waveform)
        key = ReadKeyboard();

      if (key == '-')
      {
        //Clear Row
        SelectRowAndColumn(2, 11);
        for (uint8_t i = 0; i < 7; i++)
          SendCharacters(' ', 1);
        SelectRowAndColumn(2, 11);

        for (uint8_t i = 0; i < 8; i++)
          if (key_array_dc_sym[i] != 0)
            SendCharacters(key_array_dc_sym[i], 1);
          else
            break;
      }
      else if (key == '\n')
      {
        switch (key_array[0])
        {
          case ('1'):
            SendArray("Sine", 1);

            SelectRowAndColumn(2, 20);
            for (uint8_t i = 0; i < 20; i++)
              SendCharacters(' ', 1);

            duty_cycle_value = 16384;
            symmetry_value = 16384;
            strcpy(key_array_DC, "50.0");
            strcpy(key_array_sym, "50.0");
            key_waveform = '1';

            break;
          case ('2'):
            SendArray("Triangle", 1);

            SelectRowAndColumn(2, 20);
            for (uint8_t i = 0; i < 20; i++)
              SendCharacters(' ', 1);
            SelectRowAndColumn(2, 20);

            duty_cycle_value = 16384;
            strcpy(key_array_DC, "50.0");

            SendArray("Symmetry: ", 2);
            SendArray(key_array_sym, 2);
            key_waveform = '2';

            break;
          case ('3'):
            SendArray("Square", 1);

            SelectRowAndColumn(2, 20);
            for (uint8_t i = 0; i < 20; i++)
              SendCharacters(' ', 1);
            SelectRowAndColumn(2, 20);

            symmetry_value = 16384;
            strcpy(key_array_sym, "50.0");

            SendArray("Duty cycle: ", 2);
            SendArray(key_array_DC, 2);
            key_waveform = '3';

            break;
        }
      }
      
      switch (key)
      {
        case ('1'):
          key_array[0] = '1';
          break;
        case ('2'):
          key_array[0] = '2';
          break;
        case ('3'):
          key_array[0] = '3';
          break;
      }
    }
  }
  else if ((key == '.') && (waveform_type != 1))
  {
    if (waveform_type == 2)
    {
      SelectRowAndColumn(2, 31);
      for (uint8_t i = 0; i < 4; i++)
        SendCharacters(' ', 2);
      SelectRowAndColumn(2, 31);

      while (1)
      {
        key = 0;
        while (keyboard_check || (key == 'a') || (key == 'f') || (key == '-') || (key == 'D'))
          key = ReadKeyboard();
        
        if (key == '.')
        {
          SelectRowAndColumn(2, 31);
          for (uint8_t i = 0; i < 4; i++)
            SendCharacters(' ', 2);
          SelectRowAndColumn(2, 31);

          SendArray(key_array_sym, 2);
        }
      }
    }
  }
}

void loop() 
{
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
  
  //Scan inputs
  SelectMode();
  Serial.print("Amplitude = ");
  Serial.println(amplitude_value);
  Serial.print("Frequency = ");
  Serial.println(frequency_value);
  Serial.print("DC = ");
  Serial.println(dc_value);
  Serial.println();
  //
}