//save and play 3 servo motor arduino uno
#include <Servo.h> //add servo library. this library is standard library
#include "U8glib.h"

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST); // Fast I2C / TWI

// 'icon_c', 16x16px
const unsigned char epd_bitmap_icon_c [] PROGMEM = {
	0x00, 0x00, 0x07, 0xe0, 0x0f, 0xf0, 0x1c, 0x38, 0x18, 0x18, 0x18, 0x00, 0x18, 0x00, 0x18, 0x00, 
	0x18, 0x00, 0x18, 0x00, 0x18, 0x00, 0x18, 0x18, 0x1c, 0x38, 0x0f, 0xf0, 0x07, 0xe0, 0x00, 0x00
};
// 'icon_m', 16x16px
const unsigned char epd_bitmap_icon_m [] PROGMEM = {
	0x00, 0x00, 0x38, 0x1c, 0x38, 0x1c, 0x3c, 0x3c, 0x3c, 0x3c, 0x36, 0x6c, 0x36, 0x6c, 0x33, 0xcc, 
	0x33, 0xcc, 0x31, 0x8c, 0x31, 0x8c, 0x30, 0x0c, 0x30, 0x0c, 0x30, 0x0c, 0x30, 0x0c, 0x00, 0x00
};
// 'icon_o', 16x16px
const unsigned char epd_bitmap_icon_o [] PROGMEM = {
	0x00, 0x00, 0x07, 0xe0, 0x0f, 0xf0, 0x0c, 0x30, 0x1c, 0x38, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 
	0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x1c, 0x38, 0x0c, 0x30, 0x0f, 0xf0, 0x07, 0xe0, 0x00, 0x00
};
// 'icon_w', 16x16px
const unsigned char epd_bitmap_icon_w [] PROGMEM = {
	0x00, 0x00, 0x30, 0x0c, 0x30, 0x0c, 0x30, 0x0c, 0x30, 0x0c, 0x31, 0x8c, 0x31, 0x8c, 0x33, 0xcc, 
	0x33, 0xcc, 0x36, 0x6c, 0x36, 0x6c, 0x3c, 0x3c, 0x3c, 0x3c, 0x38, 0x1c, 0x38, 0x1c, 0x00, 0x00
};

// 'icon_arrow_select', 16x16px
const unsigned char epd_bitmap_icon_arrow_select [] PROGMEM = {
	0x00, 0x00, 0x00, 0x30, 0x00, 0x60, 0x00, 0xc0, 0x01, 0x80, 0x03, 0x00, 0x06, 0x00, 0x0c, 0x00, 
	0x0c, 0x00, 0x06, 0x00, 0x03, 0x00, 0x01, 0x80, 0x00, 0xc0, 0x00, 0x60, 0x00, 0x30, 0x00, 0x00
};

// Array of all bitmaps for convenience. (Total bytes used to store images in PROGMEM = 192)
const unsigned char* bitmap_icon[4] = {
	epd_bitmap_icon_c,
	epd_bitmap_icon_m,
	epd_bitmap_icon_o,
	epd_bitmap_icon_w
};

const int NUM_ITEMS = 4;
char menu_item[NUM_ITEMS] [20] ={
 {"Controller"},
 {"Manual"},
 {"Option"},
 {"Windows"}
};

int item_select = 0;
int item_sel_previous;
int item_sel_next;

const int NUM_ITEMS1 = 5;
char menu_item1[NUM_ITEMS1] [20] ={
 {"Position 1 :"},
 {"Position 2 :"},
 {"Position 3 :"},
 {"Position 4 :"},
 {"Position 5 :"}
};

int item_select1 = 0;
int item_sel_previous1;
int item_sel_next1;

const int NUM_ITEMS2 = 6;
char menu_item2[NUM_ITEMS2] [20] ={
 {"Light 1 :"},
 {"Light 2 :"},
 {"Light 3 :"},
 {"Light 4 :"},
 {"Light 5 :"},
 {"Light 6 :"}
};

int item_select2 = 0;
int item_sel_previous2;
int item_sel_next2;

#define BOTTON_UP_PIN 2
#define BOTTON_SELECT_PIN 4
#define BOTTON_DOWN_PIN 7
#define BOTTON_CHANG_PIN 8

void pageMian(){

  u8g.firstPage();
  do {
       u8g.setFont(u8g_font_7x14);
       u8g.drawStr(40, 15, menu_item[item_sel_previous]);
       u8g.drawBitmapP( 7, 2, 16/8, 16, bitmap_icon[item_sel_previous]);

       u8g.setFont(u8g_font_7x14);
       u8g.drawStr(40, 37, menu_item[item_select]);
       u8g.drawBitmapP( 7, 24, 16/8, 16, bitmap_icon[item_select]);

       u8g.setFont(u8g_font_7x14);
       u8g.drawStr(40, 59, menu_item[item_sel_next]);
       u8g.drawBitmapP( 7, 46, 16/8, 16, bitmap_icon[item_sel_next]);

       u8g.drawBitmapP( 110, 24, 16/8, 16, epd_bitmap_icon_arrow_select);


  } while ( u8g.nextPage() );

}

void pageController(){

  u8g.firstPage();
  do {
       u8g.setFont(u8g_font_7x14);
       u8g.drawStr(0, 15, menu_item1[item_sel_previous1]);

       u8g.setFont(u8g_font_7x14);
       u8g.drawStr(0, 37, menu_item1[item_select1]);

       u8g.setFont(u8g_font_7x14);
       u8g.drawStr(0, 59, menu_item1[item_sel_next1]);

       u8g.drawBitmapP( 110, 24, 16/8, 16, epd_bitmap_icon_arrow_select);


  } while ( u8g.nextPage() );

}

void pageManual(){

  u8g.firstPage();
  do {
       u8g.setFont(u8g_font_7x14);
       u8g.drawStr(0, 15, menu_item2[item_sel_previous2]);

       u8g.setFont(u8g_font_7x14);
       u8g.drawStr(0, 37, menu_item2[item_select2]);

       u8g.setFont(u8g_font_7x14);
       u8g.drawStr(0, 59, menu_item2[item_sel_next2]);

       u8g.drawBitmapP( 110, 24, 16/8, 16, epd_bitmap_icon_arrow_select);


  } while ( u8g.nextPage() );

}

const long Taskoled = 0;
const long Taskoservo = 0;

//define the servos
Servo servo1;
Servo servo2;

//define the buttons
const int button1 = 12;
const int button2 = 13;

//define variable for values of the button
int button1Pressed = 0;
boolean button2Pressed = false;

//define potentiometers
const int pot1 = A0;

//define variable for values of the potentiometers
int pot1Val;
int pot2Val;
int pot3Val;
int pot4Val;
int pot5Val;
int pot6Val;

//define variable for angles of the potentiometer
int pot1Angle;
int pot2Angle;
int pot3Angle;
int pot4Angle;
int pot5Angle;
int pot6Angle;

//define variable for saved position of the servos
int servo1PosSave[]={1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
int servo2PosSave[]={1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};

void setup() {
  //define attached pins of the servos
  servo1.attach(3);
  servo2.attach(5);

  pinMode(BOTTON_UP_PIN, INPUT_PULLUP);
  pinMode(BOTTON_SELECT_PIN, INPUT_PULLUP);
  pinMode(BOTTON_DOWN_PIN, INPUT_PULLUP);
  pinMode(BOTTON_CHANG_PIN, INPUT_PULLUP);

  u8g.setFont(u8g_font_tpssb);
  u8g.setColorIndex(1);


  //define buttons as input units
  pinMode(button1, INPUT);
  pinMode(button2, INPUT);

  //initialize serial communication for saved position
  Serial.begin(9600);
}

void loop() {
  runTaskoled();
  runTaskservo();
}

void runTaskoled(){
  if(digitalRead(BOTTON_CHANG_PIN) == HIGH ){
    pageMian();
  }

  if(digitalRead(BOTTON_CHANG_PIN) == LOW && item_select == 0){
    pageController();
  }

  if(digitalRead(BOTTON_CHANG_PIN) == LOW && item_select == 1){
    pageManual();
  }

////////////////////////////////////////////////////////////////////////////////////////
    if (digitalRead(BOTTON_UP_PIN) == LOW && digitalRead(BOTTON_CHANG_PIN) == HIGH ) {
    item_select = item_select - 1 ;
    if (item_select < 0){
    item_select = NUM_ITEMS - 1;
    }
  }
    if (digitalRead(BOTTON_DOWN_PIN) == LOW && digitalRead(BOTTON_CHANG_PIN) == HIGH ) {
    item_select = item_select + 1 ;
    if (item_select >= NUM_ITEMS){
      item_select = 0;
    }
  }
  
  item_sel_previous = item_select - 1;
  if (item_sel_previous < 0) {item_sel_previous = NUM_ITEMS-1; }
  item_sel_next = item_select + 1;
  if (item_sel_next >=NUM_ITEMS) {item_sel_next = 0; }
//////////////////////////////////////////////////////////////////////////////////////// 
    if (digitalRead(BOTTON_UP_PIN) == LOW && item_select == 0 && digitalRead(BOTTON_CHANG_PIN) == LOW ) {
    item_select1 = item_select1 - 1 ;
    if (item_select1 < 0){
    item_select1 = NUM_ITEMS1 - 1;
    }
  }
    if (digitalRead(BOTTON_DOWN_PIN) == LOW && item_select == 0 && digitalRead(BOTTON_CHANG_PIN) == LOW ) {
    item_select1 = item_select1 + 1 ;
    if (item_select1 >= NUM_ITEMS1){
      item_select1 = 0;
    }
  }
  
  item_sel_previous1 = item_select1 - 1;
  if (item_sel_previous1 < 0) {item_sel_previous1 = NUM_ITEMS1-1; }
  item_sel_next1 = item_select1 + 1;
  if (item_sel_next1 >=NUM_ITEMS1) {item_sel_next1 = 0; }
//////////////////////////////////////////////////////////////////////////////////////// 
    if (digitalRead(BOTTON_UP_PIN) == LOW && item_select == 1 && digitalRead(BOTTON_CHANG_PIN) == LOW ) {
    item_select2 = item_select2 - 1 ;
    if (item_select2 < 0){
    item_select2 = NUM_ITEMS2 - 1;
   }
  }
    if (digitalRead(BOTTON_DOWN_PIN) == LOW && item_select == 1 && digitalRead(BOTTON_CHANG_PIN) == LOW ) {
    item_select2 = item_select2 + 1 ;
    if (item_select2 >= NUM_ITEMS2){
      item_select2 = 0;
    }
  }
  
  item_sel_previous2 = item_select2 - 1;
  if (item_sel_previous2 < 0) {item_sel_previous2 = NUM_ITEMS2-1; }
  item_sel_next2 = item_select2 + 1;
  if (item_sel_next2 >=NUM_ITEMS2) {item_sel_next2 = 0; }
//////////////////////////////////////////////////////////////////////////////////////// 
}

void runTaskservo(){
  //read the potentiometer values and define the servo angle to
  //the potentiometer value with the map function
  if ((item_select) == 0) {
      pot1Val = analogRead(pot1);
      pot1Angle = map (pot1Val, 5, 1023, 0, 170);
    }
  if ((item_select) == 1) {
  pot2Val = analogRead(pot1);
  pot2Angle = map (pot2Val, 5, 1023, 0, 170);
  }

  //servos move to mapped angles
  servo1.write(pot1Angle);
  servo2.write(pot2Angle);

  //if button1 is pressed (HIGH), save the potentiometers position
  //as long as button1 is pressed
  if(digitalRead(button1) == HIGH){
    button1Pressed++;
    switch(button1Pressed){
      case 1:
        servo1PosSave[0] = pot1Angle;
        servo2PosSave[0] = pot2Angle;
        Serial.println("Position #1 Saved");
        
       break;
       case 2:
        servo1PosSave[1] = pot1Angle;
        servo2PosSave[1] = pot2Angle;
        Serial.println("Position #2 Saved");
        
       break;
       case 3:
        servo1PosSave[2] = pot1Angle;
        servo2PosSave[2] = pot2Angle;
        Serial.println("Position #3 Saved");
        
       break;
       case 4:
        servo1PosSave[3] = pot1Angle;
        servo2PosSave[3] = pot2Angle;
        Serial.println("Position #4 Saved");
        
       break;
       case 5:
        servo1PosSave[4] = pot1Angle;
        servo2PosSave[4] = pot2Angle;
        Serial.println("Position #5 Saved");
       
       break;
      case 6:
        servo1PosSave[5] = pot1Angle;
        servo2PosSave[5] = pot2Angle;
        Serial.println("Position #6 Saved");
        
      break;
      case 7:
        servo1PosSave[6] = pot1Angle;
        servo2PosSave[6] = pot2Angle;
        Serial.println("Position #7 Saved");
        
      break;
      case 8:
        servo1PosSave[7] = pot1Angle;
        servo2PosSave[7] = pot2Angle;
        Serial.println("Position #8 Saved");
        
         break;
      case 9:
        servo1PosSave[8] = pot1Angle;
        servo2PosSave[8] = pot2Angle;
        Serial.println("Position #9 Saved");
        
         break;
      case 10:
        servo1PosSave[9] = pot1Angle;
        servo2PosSave[9] = pot2Angle;
        Serial.println("Position #10 Saved");
      
      break;
      case 11:
        servo1PosSave[10] = pot1Angle;
        servo2PosSave[10] = pot2Angle;
        Serial.println("Position #11 Saved");
      
      break;
      case 12:
        servo1PosSave[11] = pot1Angle;
        servo2PosSave[11] = pot2Angle;
        Serial.println("Position #12 Saved");
      
      break;
      case 13:
        servo1PosSave[12] = pot1Angle;
        servo2PosSave[12] = pot2Angle;
        Serial.println("Position #13 Saved");
      
      break;
      case 14:
        servo1PosSave[13] = pot1Angle;
        servo2PosSave[13] = pot2Angle;
        Serial.println("Position #14 Saved");
      
      break;
      case 15:
        servo1PosSave[14] = pot1Angle;
        servo2PosSave[14] = pot2Angle;
        Serial.println("Position #15 Saved");
      
      break;
      case 16:
        servo1PosSave[15] = pot1Angle;
        servo2PosSave[15] = pot2Angle;
        Serial.println("Position #16 Saved");
      
      break;
      case 17:
        servo1PosSave[16] = pot1Angle;
        servo2PosSave[16] = pot2Angle;
        Serial.println("Position #17 Saved");
      
      break;
      case 18:
        servo1PosSave[17] = pot1Angle;
        servo2PosSave[17] = pot2Angle;
        Serial.println("Position #18 Saved");
      
      break;
      case 19:
        servo1PosSave[18] = pot1Angle;
        servo2PosSave[18] = pot2Angle;
        Serial.println("Position #19 Saved");
      
       break;
      case 20:
        servo1PosSave[19] = pot1Angle;
        servo2PosSave[19] = pot2Angle;
        Serial.println("Position #20 Saved");
    }
  }
  //if button2 pressed (HIGH), the servos move saved position
  if(digitalRead(button2) == HIGH){
    button2Pressed = true;
  }
  if(button2Pressed){
    for(int i=0; i<20; i++){
      servo1.write(servo1PosSave[i]);
      delay(300);
      servo2.write(servo2PosSave[i]);
      delay(300);
    }
  }
  delay(10);
}