#include "tinyexpr.h"
#include <U8x8lib.h>

#define BUFF_LENGTH 100

int pins[5] = {D0, D1, D2, D3, D9};

char buttons[5][4] = {
    {'(', ')', '^', 'C'},
    {'7', '8', '9', '/'},
    {'4', '5', '6', '*'},
    {'1', '2', '3', '-'},
    {'0', '.', '=', '+'}};

// buffers for keeping track of current input and past expressions
char buff[100];
char prev_buff[3][100];
int buff_l = 0;

// OLED display constructor
U8X8_SSD1306_128X64_NONAME_HW_I2C u8x8(U8X8_PIN_NONE);

void setup()
{
  // put your setup code here, to run once:
  Serial.begin(115200);
  u8x8.begin();
  u8x8.setFlipMode(true);
  u8x8.setFont(u8x8_font_chroma48medium8_r);
  u8x8.setCursor(0, 6);
  memset(buff, '\0', BUFF_LENGTH);
}

// sets input and output pins correctly to determine charlieplexed button
bool probe(int in_pin, int out_pin)
{
  for (int i = 0; i < 5; i++)
  {
    if (i != in_pin && i != out_pin)
    {
      pinMode(pins[i], INPUT);
    }
  }
  pinMode(pins[out_pin], OUTPUT);
  pinMode(pins[in_pin], INPUT_PULLDOWN);
  digitalWrite(pins[out_pin], HIGH);
  delay(5);
  int value = digitalRead(pins[in_pin]);
  return (value == HIGH);
}

// turns charlieplexed pins into a 2d index
char pins_to_char(int in_pin, int out_pin)
{
  char butto;
  if (in_pin > out_pin)
  {
    butto = buttons[out_pin][in_pin - 1];
  }
  else
  {
    butto = buttons[out_pin][in_pin];
  }
  return butto;
}

// calculates the answer to the math problem
void calc()
{
  // do math
  double answer = te_interp(buff, 0);

  // add = back into buffer
  buff[buff_l - 1] = '=';

  // copy answer into buffer after =
  snprintf(&buff[buff_l], BUFF_LENGTH - buff_l - 1, "%.2f", answer);
}

void clear_math()
{
  // clear the buffer
  memset(buff, '\0', BUFF_LENGTH);
  buff_l = 0;

  // clear the display
  u8x8.clearDisplay();

  // write past 3 expressions
  for (int i = 0; i < 3; i++)
  {
    u8x8.drawString(0, i * 2, prev_buff[i]);
  }

  // set the cursor to the start of the line
  u8x8.setCursor(0, 6);
}

void loop()
{

  // loop over input and output pins to charliplex
  for (int out = 0; out < 5; out++)
  {
    for (int in = 0; in < 5; in++)
    {
      if (out != in)
      {
        bool ispressed = probe(in, out);
        if (ispressed)
        {
          char butto = pins_to_char(in, out);
          buff[buff_l] = butto;
          buff_l++;

          if (butto == 'C')
          {
            clear_math();
          }
          else
          {
            u8x8.print(butto);
          }

          delay(100);

          if (butto == '=')
          {
            buff[buff_l - 1] = '\0';
            calc();

            // push expression into history
            memcpy(prev_buff[0], prev_buff[1], BUFF_LENGTH);
            memcpy(prev_buff[1], prev_buff[2], BUFF_LENGTH);
            memcpy(prev_buff[2], buff, BUFF_LENGTH);

            clear_math();
          }
        }
      }
    }
  }

  delay(10);
}