#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // pixel ความกว้าง
#define SCREEN_HEIGHT 64 // pixel ความสูง 

#define OLED_RESET     -1 //ขา reset เป็น -1 ถ้าใช้ร่วมกับขา Arduino reset
Adafruit_SSD1306 OLED(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


int var = 0; // ตัวแปรสำหรับทดสอบแสดงผล




int dirPin = 2;                  // pin 2 on Direction pin
int stepPin = 3;                 // pin 3 on Step pin
  
int analogPinAmpitude = A0;       //VR1
int val_Ampitude;
int analogPinSpeed = A1;          //VR2
int val_Speed;
//int analogPinInterval = A2;      //VR3
//int val_Interval;
int analogPinWave = A2;           //VR3
int val_Wave;
int analogPinTimedelay = A3;      //VR4
int val_Timedelay;

int limitswitch = 5;              //Connect to Ground

int EnablePin = 4;                //Enable pin Low 

int StarterStep = 6400;           // 3200 = 1 Rev (micro step = 1/16)
int setpos1Step = 600;
int setpos2Step = 360;
int setpos3Step = 180;



void setup() {

 Serial.begin(9600);




  if (!OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // สั่งให้จอ OLED เริ่มทำงานที่ Address 0x3C
    Serial.println("SSD1306 allocation failed");
  } else {
    Serial.println("ArdinoAll OLED Start Work !!!");
  }





 pinMode(analogPinAmpitude, INPUT);    //VR1
 pinMode(analogPinSpeed, INPUT);       //VR2  
 //pinMode(analogPinInterval, INPUT);    //VR3
 pinMode(analogPinWave, INPUT);        //VR4
 pinMode(analogPinTimedelay, INPUT);   //VR5

 pinMode(limitswitch, INPUT);          //Limitswitch

 pinMode(dirPin, OUTPUT);               
 pinMode(stepPin, OUTPUT);

 pinMode(8, OUTPUT);                   //5volt OP
 pinMode(13, OUTPUT);                  //5volt OP
 pinMode(12, OUTPUT);                  //5volt OP

 pinMode(EnablePin, OUTPUT);           //Enable nomal low

 

 digitalWrite(8, HIGH);               //5volt OP
 digitalWrite(13, HIGH);              //5volt OP
 digitalWrite(12, HIGH);              //5volt OP
 digitalWrite(EnablePin, LOW);        //Enable nomal low

 // digitalWrite(limitswitch, LOW);


 /////// Starting Function Setup /////////

startOLEDdisplay();

 delay(1000);

 starter();

 delay(1000);

 setpos();
 
 delay(500);

}


void loop() {

 

 
 
 
 Serial.println("Start \t");


  val_Ampitude = analogRead(analogPinAmpitude);
  int vr_amp = map(val_Ampitude,0,1023,800,100);


  val_Speed = analogRead(analogPinSpeed);
  int vr_spd = map(val_Speed,0,1023,460,1200);


  //val_Interval = analogRead(analogPinInterval);
  //int vr_itv = map(val_Interval,0,1023,1000,100);
  

  val_Wave = analogRead(analogPinWave);
  int vr_wave = map(val_Wave,0,1023,10,2);


  val_Timedelay = analogRead(analogPinTimedelay);
  int vr_tdelay = map(val_Timedelay,0,1023,10000,1000);


  int vr_itv = 100;


   
  OLEDdisplay(vr_amp, vr_spd, vr_itv, vr_wave, vr_tdelay);


  function(vr_amp, vr_spd, vr_itv, vr_wave, vr_tdelay);             //Argument




delay(vr_tdelay);

}





void starter(){

 digitalWrite(dirPin, LOW);

 int i;
 for ( i = 0; i < StarterStep; i++) {
   digitalWrite(stepPin, HIGH);
   delayMicroseconds(500);
   digitalWrite(stepPin, LOW);
   delayMicroseconds(500);

   int val_limmit = digitalRead(limitswitch);
   if (val_limmit == 1){
     i = 6401;
   }

 }


}

void setpos(){

  digitalWrite(dirPin, HIGH);
  for (int i = 0; i < setpos1Step; i++) {
   digitalWrite(stepPin, HIGH);
   delayMicroseconds(500);
   digitalWrite(stepPin, LOW);
   delayMicroseconds(500);
   }

 delay(750);

 digitalWrite(dirPin, HIGH);
 for (int i = 0; i < setpos2Step; i++) {
   digitalWrite(stepPin, HIGH);
   delayMicroseconds(500);
   digitalWrite(stepPin, LOW);
   delayMicroseconds(500);
 }

delay(250);

 digitalWrite(dirPin, HIGH);
 for (int i = 0; i < setpos3Step; i++) {
   digitalWrite(stepPin, HIGH);
   delayMicroseconds(500);
   digitalWrite(stepPin, LOW);
   delayMicroseconds(500);
 }


}

void function(int _amp, int _spd, int _itv, int _wave, int _tdel) {

for (int i = 0; i < _wave; i++) { 

 digitalWrite(dirPin, LOW);

 for (int i = 0; i < _amp; i++) {
   digitalWrite(stepPin, HIGH);
   delayMicroseconds(_spd);
   digitalWrite(stepPin, LOW);
   delayMicroseconds(_spd);   
 }

 digitalWrite(dirPin, HIGH);

 for (int i = 0; i < _amp; i++) {
   digitalWrite(stepPin, HIGH);
   delayMicroseconds(_spd);
   digitalWrite(stepPin, LOW);
   delayMicroseconds(_spd);   
 }
 
 delay(_itv);

Serial.println("AMP");
Serial.println(_amp);

Serial.println("SPEED");
Serial.println(_spd);

Serial.println("INTERVAL");
Serial.println(_itv);

Serial.println("WAVE");
Serial.println(_wave);

Serial.println("Time");
Serial.println(_tdel);

 }
}


void OLEDdisplay(int _amp, int _spd, int _itv, int _wave, int _tdel) {
  
  OLED.clearDisplay(); // ลบภาพในหน้าจอทั้งหมด
  OLED.setTextColor(WHITE, BLACK);  //กำหนดข้อความสีขาว ฉากหลังสีดำ
  OLED.setCursor(0, 0); // กำหนดตำแหน่ง x,y ที่จะแสดงผล
  OLED.setTextSize(1); // กำหนดขนาดตัวอักษร
  OLED.print("AMPITUDE");   // แสดงผลข้อความ ALL
  OLED.setCursor(70, 0);
  OLED.println(_amp);       // แสดงผลข้อความ ALL
  OLED.print("SPEED");   // แสดงผลข้อความ ALL
  OLED.setCursor(70, 9);
  OLED.println(_spd);       // แสดงผลข้อความ ALL

  OLED.print("WAVE LOOP");   // แสดงผลข้อความ ALL
  OLED.setCursor(70, 18);
  OLED.println(_wave);       // แสดงผลข้อความ ALL
  OLED.print("TIME DELAY");   // แสดงผลข้อความ ALL
  OLED.setCursor(70, 27);
  OLED.println(_tdel);       // แสดงผลข้อความ ALL

  OLED.display(); // สั่งให้จอแสดงผล
  
  delay(100);

}

void startOLEDdisplay() {

  OLED.clearDisplay(); // ลบภาพในหน้าจอทั้งหมด
  OLED.setTextColor(WHITE, BLACK);  //กำหนดข้อความสีขาว ฉากหลังสีดำ
  OLED.setCursor(0, 0); // กำหนดตำแหน่ง x,y ที่จะแสดงผล
  OLED.setTextSize(2); // กำหนดขนาดตัวอักษร
  OLED.println("STARTING"); // แสดงผลข้อความ ALL
  OLED.println("SETTING"); // แสดงผลข้อความ ALL
  OLED.print("POSITION"); // แสดงผลข้อความ ALL  

  OLED.display(); // สั่งให้จอแสดงผล
}