#include <Wire.h>
#include <fp64lib.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27,20,4);  // set the LCD address to 0x27 for a 16 chars and 2 line display

#define NO_OUT 5
int outPort[NO_OUT] = {11, 10, 9, 8, 2};
#define NO_IN 6
int inPort[NO_IN] = {12, 7, 6, 5, 4, 3};

char keyMap[NO_OUT][NO_IN] = {
  {' ', '1', '7', '4', ' ', ' '},
  {'=', '2', '8', '5', '0', '/'},
  {' ', 'e', '+', 'z', ' ', ' '},
  {'-', '3', '9', '6', '.', '*'},
  {' ', ' ', ' ', ' ', ' ', 'd'}
};

char blankLine = "                    ";

#define MAX_DIGIT     16

#define STACK_DEPTH   8
float64_t stack[STACK_DEPTH];

void push(float64_t v) {
  for(int i = STACK_DEPTH - 1; i > 0; i--) {
    stack[i] = stack[i - 1];
  }
  stack[0] = v;
}

float64_t pop() {
  float64_t v;
  v = stack[0];
  for(int i = 0; i < STACK_DEPTH - 1; i++) {
    stack[i] = stack[i + 1];
  }
  stack[STACK_DEPTH - 1] = 0;
  return v;
}

char key_scan() {
  int i, j;
  char code = 0;
  
  for(i = 0; i < NO_OUT; i++) {
    digitalWrite(outPort[i], LOW);      
    for(j = 0; j < NO_IN; j++) {
      if(digitalRead(inPort[j]) == LOW) {
        code = keyMap[i][j];
      }
    }
    digitalWrite(outPort[i], HIGH);          
  }
  return code;
}

void setup() {
  lcd.init();
  lcd.init();
  lcd.backlight();

  int i;
  for(i = 0; i < NO_IN; i++) {
    pinMode(inPort[i], INPUT_PULLUP);
  }
  for(i = 0; i < NO_OUT; i++) {
    pinMode(outPort[i], OUTPUT);
    digitalWrite(outPort[i], HIGH);
  }
  update_display();
}

void messageBox(String str) {
  lcd.clear();
  lcd.setCursor(5,1);
  lcd.print(str);
  delay(1000);
}

int mode = -1; // function key mode : -1 : disabled, 0-6 : function key enabled
boolean degree = false; // radian / degree
String enteringStr = "";

void dispFunction(int pos, String str) {
  int x = pos * 6 + 2;
  lcd.setCursor(x, 3);
  lcd.print(str);  
}

String disp[4] = {"", "", "", ""};
boolean dispReady = false;
void update_display(void) {
  int entlen = enteringStr.length();

  if(entlen) {
    String dispEntStr;
    if(entlen > MAX_DIGIT) { // 文字列の前方を省略表示
      dispEntStr = ".." + enteringStr.substring(entlen - MAX_DIGIT + 2);
    }
    else {
      dispEntStr = enteringStr;
    }
    if(!dispReady) {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print(disp[2]);
      lcd.setCursor(0, 1);
      lcd.print(disp[1]);
      lcd.setCursor(0, 2);
      lcd.print(disp[0]);
      dispReady = true;
    }
    lcd.setCursor(0, 3);
    lcd.print(dispEntStr);
  }
  else {
    disp[0] = "x:  " + fp64_to_string_wrap(stack[0]);
    disp[1] = "y:  " + fp64_to_string_wrap(stack[1]);
    disp[2] = "    " + fp64_to_string_wrap(stack[2]);
    disp[3] = "    " + fp64_to_string_wrap(stack[3]);
    dispReady = false;
    lcd.clear();
    if(mode < 0) { // no function button
      lcd.setCursor(0, 0);
      lcd.print(disp[3]);
      lcd.setCursor(0, 1);
      lcd.print(disp[2]);
      lcd.setCursor(0, 2);
      lcd.print(disp[1]);
      lcd.setCursor(0, 3);
      lcd.print(disp[0]);
    }
    else {
      switch(mode) {
      case 0:
        dispFunction(0, "  x<>y");
        dispFunction(1, "  drop");
        dispFunction(2, " reset");
        break;
      case 1:
        dispFunction(0, "   sin");
        dispFunction(1, "   cos");
        dispFunction(2, "   tan");
        break;
      case 2:
        dispFunction(0, "  asin");
        dispFunction(1, "  acos");
        dispFunction(2, "  atan");
        break;
      case 3:
        if(degree) {
          dispFunction(0, "   rad");
        }
        else {
          dispFunction(0, "   deg");
        }
        dispFunction(1, "    pi");
        dispFunction(2, "    e");
        break;
      case 4:
        dispFunction(0, "   log");
        dispFunction(1, "    ln");
        dispFunction(2, " log_y");
        break;
      case 5:
        dispFunction(0, "  10^x");
        dispFunction(1, "   e^x");
        dispFunction(2, "   y^x");
        break;
      case 6:
        dispFunction(0, "    -x");
        dispFunction(1, "   1/x");
        dispFunction(2, "  sqrt");
        break;
      }
      lcd.setCursor(0, 0);
      lcd.print(disp[2]);
      lcd.setCursor(0, 1);
      lcd.print(disp[1]);
      lcd.setCursor(0, 2);
      lcd.print(disp[0]);
      char modeNo[] = " ";
      modeNo[0] = '0' + mode;
      lcd.setCursor(0, 3);
      lcd.print(modeNo);
    }
  }
}
    
String fp64_to_string_wrap(float64_t n) {
  String s;
    if (fp64_signbit(n)) {  // minus
        s = fp64_to_string(n, MAX_DIGIT, MAX_DIGIT - 3);
    }
    else {
        s = fp64_to_string(n, MAX_DIGIT, MAX_DIGIT - 2);
    }
    while(s.length() < MAX_DIGIT) {
        s = " " + s; // align right
    }
    return s;
}

float64_t toRad(float64_t x) {
  if(degree) {
    return fp64_div(fp64_mul(x, float64_NUMBER_PI), fp64_atof("180"));
  }
  else return x;
}

float64_t fromRad(float64_t x) {
  if(degree) {
    return fp64_div(fp64_mul(x, fp64_atof("180")), float64_NUMBER_PI);
  }
  else return x;
}

void enter(void) {
  if(enteringStr.length()) {
    push(fp64_atof((char*)enteringStr.c_str()));
    enteringStr = "";
  }
}

void loop() {
  float64_t x, y;

  char key = key_scan();
  if (key == 0) {
    return;
  }

  // detect long push
  boolean long_push = false;
  unsigned long pushed_time = millis();
  while (key_scan() == key) {
    if (millis() - pushed_time > 500) {
      long_push = true;
      break;
    }
  }

  switch(key) {
  case '+':
    enter();
    x = pop();
    y = pop();
    push(fp64_add(y, x));
    break;
  case '*':
    enter();
    if(mode >= 0) { // function key mode
      switch(mode) {
      case 0: // drop
        pop();
        break;
      case 1:
        push(fp64_cos(toRad(pop())));
        break;
      case 2:
        push(fromRad(fp64_acos(pop())));
        break;
      case 3: // pi
        push(float64_NUMBER_PI);
        break;
      case 4:
        push(fp64_log(pop()));
        break;
      case 5:
        push(fp64_exp(pop()));
        break;
      case 6: // 1/x
        push(fp64_inverse(pop()));
        break;
      }
      mode = -1;
    }
    else if (long_push) { // square root
      push(fp64_sqrt(pop()));
    }
    else { // - substruct
      x = pop();
      y = pop();
      push(fp64_mul(y, x));
    }
    break;
  case '-':
    enter();
    if (long_push) { // +/- CHS
      push(fp64_neg(pop()));
    }
    else {
      x = pop();
      y = pop();
      push(fp64_sub(y, x));
    }
    break;
  case '=':
    if(mode >= 0) { // function key mode
      mode = (mode + 1) % 7; // next page
    }
    else {
      enter();
      mode = 0;
    }
    break;
  case '/':
    enter();
    if(mode >= 0) { // function key mode
      switch(mode) {
      case 0: // x<>y (swap)
        x = pop();
        y = pop();
        push(x);
        push(y);
        break;
      case 1:
        push(fp64_sin(toRad(pop())));
        break;
      case 2:
        push(fromRad(fp64_asin(pop())));
        break;
      case 3: // switch degree / radian
        degree = !degree;
        if(degree) {
          messageBox("degree mode");
        }
        else {
          messageBox("radian mode");
        }
        break;
      case 4:
        push(fp64_log10(pop()));
        break;
      case 5:
        push(fp64_exp10(pop()));
        break;
      case 6: // +/- (CHS)
        push(fp64_neg(pop()));
        break;
      }
      mode = -1;
    }
    else if (long_push) {            // SWAP x and y  
      x = pop();
      y = pop();
      push(x);
      push(y);
    }
     else {
      x = pop();
      y = pop();
      push(fp64_div(y, x));
    }
    break;
  case 'e':
    if(enteringStr.length()) { // enter
      enter();
    }
    else { // DUP
      x = pop();
      push(x);
      push(x);    
    }
    break;
  case 'd':
    enter();
    if(mode >= 0) { // function key mode
      switch(mode) {
      case 0: // reset
        messageBox("reset");
        asm volatile ("  jmp 0");  
        break;
      case 1:
        push(fp64_tan(toRad(pop())));
        break;
      case 2:
        push(fromRad(fp64_atan(pop())));
        break;
      case 3: // e
        push(float64_EULER_E);
        break;
      case 4:
        x = pop();
        y = pop();
        push(fp64_div(fp64_log(x), fp64_log(y)));
        break;
      case 5:
        x = pop();
        y = pop();
        push(fp64_pow(y, x));
        break;
      case 6:
        push(fp64_sqrt(pop()));
        break;
      }
      mode = -1;
    }
    else {
      pop();
    }
    break;
  case 'z':
    enteringStr.concat("000");
    break;
  default:
    if(mode >= 0) { // function key mode
      int select = key - '0';
      if(select == mode) { //cancel function mode
        mode = -1;
      }
      else if(0 <= select && select <= 6) {
        mode = select;
      }
      else { // '.'
        mode = -1;
      }
    }
    else if(long_push) {
      if('0' <= key && key <= '6') {
        enter();
        mode = key - '0';
      }
    }
    else { // entering number
      if (enteringStr.indexOf(".") == -1 || key != '.') {
        enteringStr.concat(key);
      }
    }
  }
  update_display();
  while(key_scan())
    ;
}