#include <algorithm>
#include <ESP32Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

typedef struct Eye{
  int x0 ; //pivot x0
  int x1 ; //pivot x1
  int currentX0 ;
  int currentX1 ;
};
Eye left = { 79, 107 }; //79 107
Eye right = { 20, 48 }; //20 48

int eyeY0 = 17 ; //17
int eyeY1 = 40 ; //44
int eyeWidth = 28 ;
bool isOpen = false ;

Adafruit_SSD1306 oled(128, 64, &Wire, -1);

Servo leftArm ;
Servo rightArm ;

// func def
//background
void setInitialPosition() ;
void resetRigth() ;
void resetLeft() ;
void renderEye(int x0, int y0, int x1, int y1, uint16_t color) ;
//spesial act
void bukaMata() ; //setelah kedip 100%
void kedip() ; //mungkin setelah buka mata 80%
//tidak spesial
void happyExpress() ;
void lookRight() ;
void lookLeft() ;
//polinomial sampling with random

void setup() {
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  oled.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  oled.clearDisplay() ;

  rightArm.attach(15, 500, 2400) ;
  leftArm.attach(13, 500, 2400) ;
  leftArm.write(90) ;
  rightArm.write(90) ;

  setInitialPosition() ;
}

void loop() {
  //chooseAction(); 
  bukaMata() ;
  lookRight() ;
  // lookRight() ;
  // delay(1000) ;
  // stopScroll() ;
  // delay(500) ;
  // lookLeft() ;
  // delay(1000) ;
  // stopScroll() ;
  rightArm.write(180) ;
  delay(500) ;
  rightArm.write(90) ;
  delay(2000) ;
  kedip() ;
  delay(100) ;
}

void setInitialPosition() {
  resetRight() ;
  resetLeft() ;
}

void resetRight() {
  right.currentX0 = right.x0 ;
  right.currentX1 = right.x1 ;
}

void resetLeft() {
  left.currentX0 = left.x0 ;
  left.currentX1 = left.x1 ;
}

void chooseAction() {
  int act = 1 ;
  switch (act) {
    case 0: lookRight() ;
    case 1: lookLeft() ;
    default: break;
  }
}

void renderRightEye(int x0, int y0, int width, int y1, int rad, uint16_t color) {
  oled.drawRoundRect(x0, y0, width, y1, rad, color);
  oled.fillRoundRect(x0, y0, width, y1, rad, color);
}

void renderLeftEye(int x0, int y0, int width, int y1, int rad, uint16_t color) {
  oled.drawRoundRect(x0, y0, width, y1, rad, color) ;
  oled.fillRoundRect(x0, y0, width, y1, rad, color);
}

void bukaMata() { 
  if (isOpen) return ;
  int rad = 13 ;
  for (int y = eyeY0; y <= eyeY1; y++) {
    renderRightEye(right.currentX0, eyeY0, eyeWidth, y, rad, WHITE) ;
    renderLeftEye(left.currentX0, eyeY0, eyeWidth, y, rad, WHITE) ;
    if (y % 18 == 0) oled.display();
  }
  oled.display();
  isOpen = true ;
}

void kedip() {
  if (!isOpen) return ;
  int rad = 13 ;
  renderRightEye(right.currentX0, eyeY0, eyeWidth, eyeY1, rad, NULL) ;
  renderLeftEye(left.currentX0, eyeY0, eyeWidth, eyeY1, rad, NULL) ;
  oled.display();
  isOpen = false ;
}

void lookRight() {
  oled.startscrollright(0, 111) ;
}

void lookLeft() {
  oled.startscrollleft(0, 111) ;
}

void stopScroll() {
  oled.stopscroll() ;
}

void rightToNormal() { 
  //for (int x = )
  setInitialPosition() ;
}

void leftToNormal() { 
  //for (int x = )
  setInitialPosition() ;
}