#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"
#include "math.h"


#define TFT_DC 9
#define TFT_CS 10
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
#define SCREEN_WIDTH 320
#define SCREEN_HEIGHT 240

word RGB( byte R, byte G, byte B) {
  return ( ((R & 0xF8) << 8) | ((G & 0xFC) << 3) | (B >> 3) );
}


int* ScreenCenter() {
  static int center[2]; // Static array to hold the screen center
  center[0] = SCREEN_WIDTH / 2;   // Set the x coordinate
  center[1] = SCREEN_HEIGHT / 2;  // Set the y coordinate
   return center;  // Return a pointer to the array
}

float evaluateExpression(const char* expression, float x) {
  float numberToUse_Float = 1;
  float result = 0;
  char operation = '*';
  int len = strlen(expression);

  for (int i = 0; i <= len; i++) {
    operation = '*';
    numberToUse_Float = 1;
    char c = expression[i];
    if (i == 0 && (c == 'x' || isdigit(c))) {
      result = 1;
    };
    if (c == '+' || c == '-' || c == '*' || c == '/') {
      operation = c;
      i++;
      c = expression[i];
    };

    while (isdigit(c) || c == 'x' || c == '^') {

      String numberToUse_String = "";

      while (isdigit(c) || c == '.') {
        numberToUse_String += c;
        i++;
        c = expression[i];
      }
      if (numberToUse_String != "") {
        numberToUse_Float *= numberToUse_String.toFloat();
      };


      float MultiplicationX = 1;
      while (c == 'x') {
        MultiplicationX *= x;
        i++;
        c = expression[i];
        if (c != 'x') {
          i--;
        }
      }
      
      numberToUse_Float *= MultiplicationX;
      float PowerOf = 1;
      if (c == '^') {
        i++;
        c = expression[i];
        if (c == '-') {
          i++;
          c = expression[i];
          PowerOf *= -1;
        }
        while (isdigit(c) || c == 'x') {
          String Power_numberToUse_String = "";
          while (isdigit(c) || c == '.') {
            Power_numberToUse_String += c;
            i++;
            c = expression[i];
          }
          if (Power_numberToUse_String != "") {
            PowerOf *= Power_numberToUse_String.toFloat();
          };


          float Power_MultiplicationX = 1;
          while (c == 'x') {
            Power_MultiplicationX *= x;
            i++;
            c = expression[i];
          }
          PowerOf *= Power_MultiplicationX;
        }
      };

      numberToUse_Float = pow(numberToUse_Float, PowerOf);
    }
    if (operation == '+') {
      result = result+numberToUse_Float;
    } else if (operation == '-') {
      result = result-numberToUse_Float;
    } else if (operation == '*') {
      result = result*numberToUse_Float;
    } else if (operation == '/') {
        if (numberToUse_Float == 0) {
          return 0;
        } 
      result = result/numberToUse_Float;
    }
  }
  return result;
}

void Function(const char* expression, word GraphLineColor, int Scale = 10) {
  Serial.print("Drawing Graph... ");


  int ScreenCenterPoint[2] = {SCREEN_WIDTH/2,SCREEN_HEIGHT/2};
  for (float x = -ScreenCenterPoint[0]/Scale; x <= ScreenCenterPoint[0]/Scale; x = x+0.01) {
    float y = evaluateExpression(expression, x);
    if (y*Scale+1 > -ScreenCenterPoint[1] && y*Scale-1 < ScreenCenterPoint[1]) {
      float GraphPosY = -y*Scale+ScreenCenterPoint[1];
      tft.drawPixel(x*Scale+ScreenCenterPoint[0],GraphPosY,GraphLineColor);
    }
  };
  Serial.println("Done.");
}
void Graph_Axis(unsigned int Axis_R = 0,unsigned int Axis_G = 0,unsigned int Axis_B = 0) {
  int* ScreenCenterPoint = ScreenCenter();
  //Draw Axis
  tft.drawLine(0,ScreenCenterPoint[1],SCREEN_WIDTH,ScreenCenterPoint[1],RGB(Axis_R,Axis_G,Axis_B));
  tft.drawLine(ScreenCenterPoint[0],0,ScreenCenterPoint[0],SCREEN_HEIGHT,RGB(Axis_R,Axis_G,Axis_B));
  tft.setTextColor(RGB(Axis_R,Axis_G,Axis_B));

  tft.setTextSize(1);
  tft.drawPixel(ScreenCenterPoint[0]-1,1,RGB(Axis_R,Axis_G,Axis_B));
  tft.drawPixel(ScreenCenterPoint[0]-2,2,RGB(Axis_R,Axis_G,Axis_B));
  tft.drawPixel(ScreenCenterPoint[0]-3,3,RGB(Axis_R,Axis_G,Axis_B));
  tft.drawPixel(ScreenCenterPoint[0]+1,1,RGB(Axis_R,Axis_G,Axis_B));
  tft.drawPixel(ScreenCenterPoint[0]+2,2,RGB(Axis_R,Axis_G,Axis_B));
  tft.drawPixel(ScreenCenterPoint[0]+3,3,RGB(Axis_R,Axis_G,Axis_B));
  tft.setCursor(ScreenCenterPoint[0]+2,3);
  tft.print("y");

  tft.drawPixel(SCREEN_WIDTH-1,ScreenCenterPoint[1]-1,RGB(Axis_R,Axis_G,Axis_B));
  tft.drawPixel(SCREEN_WIDTH-2,ScreenCenterPoint[1]-2,RGB(Axis_R,Axis_G,Axis_B));
  tft.drawPixel(SCREEN_WIDTH-3,ScreenCenterPoint[1]-3,RGB(Axis_R,Axis_G,Axis_B));
  tft.drawPixel(SCREEN_WIDTH-1,ScreenCenterPoint[1]+1,RGB(Axis_R,Axis_G,Axis_B));
  tft.drawPixel(SCREEN_WIDTH-2,ScreenCenterPoint[1]+2,RGB(Axis_R,Axis_G,Axis_B));
  tft.drawPixel(SCREEN_WIDTH-3,ScreenCenterPoint[1]+3,RGB(Axis_R,Axis_G,Axis_B));
  tft.setCursor(SCREEN_WIDTH-9,ScreenCenterPoint[1]);
  tft.print("x");
  return;
}
void Graph() {
  tft.fillRect(0,0,SCREEN_WIDTH,SCREEN_HEIGHT+1,RGB(255,255,255));
  int* ScreenCenterPoint = ScreenCenter();
  //Draw Grid
  for (int i = ScreenCenterPoint[0]; i <= SCREEN_WIDTH; i = i+10) {
    tft.drawLine(i,0,i,SCREEN_HEIGHT,RGB(225,225,225));
  }
  for (int i = ScreenCenterPoint[0]; i >= 0; i = i-10) {
    tft.drawLine(i,0,i,SCREEN_HEIGHT,RGB(225,225,225));
  }
  for (int i = ScreenCenterPoint[1]; i <= SCREEN_HEIGHT; i = i+10) {
    tft.drawLine(0,i,SCREEN_WIDTH,i,RGB(225,225,225));
  }
  for (int i = ScreenCenterPoint[1]; i >= 0; i = i-10) {
    tft.drawLine(0,i,SCREEN_WIDTH,i,RGB(225,225,225));
  }

  //Draw Axis
  Graph_Axis(0,0,0);

  Serial.println("Graph Done Drawing.");
  return;
}

void setup() {
  Serial.begin(115200);
  tft.begin();
  tft.setRotation(1);
  int* ScreenCenterPoint = ScreenCenter();
  Graph();
  /*
  //Make the Function
  Function("x",RGB(0,0,255));
  Function("2x",RGB(255,0,0));
  Function("3x",RGB(0,255,0));

  Function("2^x",RGB(0,255,255));
  Function("x^-2",RGB(90,50,170));
  Serial.println("Done.");
  */
  for (int i = 0; i< 3; i++) {
    Graph_Axis(0,200,0);
    delay(250);
    Graph_Axis(0,0,0);
    delay(250);
  }
}

void loop() {
  if (Serial.available() > 0) {
    String FunctionInputString = Serial.readStringUntil('\n');
    if (FunctionInputString == "CLEAR") {
      Graph();
    } else {
      Function(FunctionInputString.c_str(),RGB(random(0, 255),random(0, 255),random(0, 255)));
    }
  }
}