#include <Encoder.h>
#include <Timer.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MCP4725.h>

#define DAC_RESOLUTION  (9);

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Adafruit_MCP4725 dac; // create DAC
Timer timer; // create timer
Encoder knob(2, 3); // create encoder

int sw = 4; // encoder switch pin
int rly = 8; // relay output pin
long position  = 0; //encoder position (redundant)
volatile int dir; // encoder direction
volatile int step = 1; // encoder increment step
volatile long time; // time between encoder steps (speed)
volatile int count = 0; // encoder count (absolute position)
int countMax = 4095; 
int countMin = 0;
volatile float dacV; // voltage expected from DAC output
volatile float biasV; // voltage expected at opamp output ( assuming gain=2 )




void setup() {
  Serial.begin(9600);
  delay(500);
  dac.begin(0x60); //initialize DAC on i2c adress 0x60
  delay(500);
  timer.start(); 
  if(!display.begin(!SSD1306_SWITCHCAPVCC, 0X3C)) {
    Serial.println(F("SSD 1306 allocation failed"));
  }
  pinMode(sw, INPUT_PULLUP);
  pinMode(rly, OUTPUT);  
  delay(500);
  display.clearDisplay(); // set display settings
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 10);
  display.println("Encoder Test:");
  display.display();
  Serial.println("Encoder Test:");

}





void loop() {
    time = timer.read();
    long newPosition; //  to be compared with position
  newPosition -= knob.read()/2; // flip direction and divide output to match real steps (1 step normally outputs in increments of 4)
  
  if (newPosition != position ) {  //if encoder position changes stop the timer ,update and print values
    timer.stop();
    
 if (position > newPosition) { // get direction from difference between position and newposition
    dir = -1;
    }
    else {
      dir = 1;
    }
 if (time < 100) { // get speed from timer and decide step size
   step = 100;
  }
  else if (time < 250) {
   step = 10;
  }
   else {
    step = 1;
  }
    count = count + step * dir;
 
 if(count > countMax) { //limit count output (0-4095)
  count = countMax;
 }
 if(count < countMin) {
  count = countMin;
 }

  //Serial.print("Time ms=");
    //Serial.print(time);
    //Serial.print("   ENCODER=");
    //Serial.print(newPosition); 
  //Serial.print("\t");  
    Serial.print("  Count=");
    Serial.print(count);
    Serial.print("\t"); 
    //Serial.print("  Step=");
    //Serial.print(step);
    //Serial.print("\t"); 
    dacV = count * 0.00122; // calculate expected voltage from count
    biasV = dacV * 2;
    Serial.print("  dacV=" );
    Serial.print(dacV);
    Serial.print("\t"); 
    Serial.print("  BiasV=");
    Serial.print(biasV);
    Serial.println("\t"); 

    display.clearDisplay();
    display.setCursor(0, 10);
    display.println("Encoder Test:");
    display.setCursor(0, 25);
    display.println("dacV =" );
    display.setCursor(40, 25);
    display.println(dacV);
    display.display();
    


    position = newPosition; // update position
    dac.setVoltage(count, false); // set DAC output
    timer.start(); // restart timer
      }
  

   static byte toggle_sw_memmory=0; // switch

   
   if ( !digitalRead(sw) ) {          // Check for keypress (pulled up so zero = pushed)

      delay(10); // debounce

      if ( !digitalRead(sw) ) {       // if it is still pushed after a delay.
         toggle_sw_memmory = !toggle_sw_memmory;

         if (toggle_sw_memmory) {
            digitalWrite(rly,HIGH);
            Serial.println("Relay ON");
            }
         else {
            digitalWrite(rly,LOW);
            Serial.println("Relay OFF");
            }
      }
      while(!digitalRead(sw)); // wait for low
   }
  
    
  
}