// Code für die ODMR - finales FabAcademy Projekt von Mic

// Basiert auf folgenden Codefragmenten: 
// MultiStepper.pde
// ReadAnalogVoltage
// ADS1115_Einzelmessung.ino (Edistechlab)
// SSD1306_OLED_Display.ino (Edistechlab)
// App für Arduino zum Display mit RP2040:

// Schrittmotoren: 
#include <AccelStepper.h>

//Fehlermeldung: 
//Alternatives for pgmspace.h: []C:\Users\User\Documents\Arduino\libraries\Adafruit_SSD1306\Adafruit_SSD1306.cpp:42:10: fatal error: pgmspace.h: No such file or directory
//ResolveLibrary(pgmspace.h) #include <pgmspace.h>
//#include <pgmspace.h>

//I2C und Display
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeSans12pt7b.h> 
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
// The pins for I2C are defined by the Wire-library. 
// On an arduino UNO:       A4(SDA), A5(SCL)
// On an arduino MEGA 2560: 20(SDA), 21(SCL)
// On an arduino LEONARDO:   2(SDA),  3(SCL), ...
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3c // i2c slave address
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//TwoWire myWire1(&sercom0, 26, 27);  // IC2 pins definieren; von CGPT vorgeschlagen - erst mal Lösung unten probieren
//TwoWire myWire2(&sercom0, 10, 11);  // IC2 pins definieren; von CGPT vorgeschlagen - erst mal Lösung unten probieren

// TwoWire I2C_1 = TwoWire(0); //https://wolles-elektronikkiste.de/i2c-schnittstellen-des-esp32-nutzen
// TwoWire I2C_2 = TwoWire(1); 

// Nutzung von bestimmten I2C Pins des RP2040
#define SDA_1 26
#define SCL_1 27
#define SDA_2 10
#define SCL_2 11
#define I2C_FREQ 400000  //ist optional - ausprobieren wenns nicht tut 

// I2C_1.begin(SDA_1, SCL_1, I2C_FREQ);
// I2C_2.begin(SDA_2, SCL_2, I2C_FREQ);

//ADC: 
#include<ADS1115_WE.h> 
// ADS1115_WE adc_1 = ADS1115_WE(&I2C_1, ADS1115_I2C_ADDRESS); //https://wolles-elektronikkiste.de/i2c-schnittstellen-des-esp32-nutzen
// ADS1115_WE adc_2 = ADS1115_WE(&I2C_2, ADS1115_I2C_ADDRESS);

#define ADS1115_I2C_ADDRESS  0x48

//  #include <Adafruit_ADS1X15.h>
//  Adafruit_ADS1115 ads;             // Benennung des ADC
//  #define ADS1115_I2C_addr0 0x48    // I2C Adresse
//  ads.setGain(GAIN_TWOTHIRDS);      // Standardeinstellung mit der bis 6V gemessen werden können
                                  // wenn man sicher ist, dass weniger Spannung anliegt, kann man den Gain hochsetzten
                                  // wenn dann mehr anliegt geht der ADC aber kaputt!!!

// Display: 
//  #include <Adafruit_SSD1306.h>
//  #include <Adafruit_GFX.h>
//
//  #define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
//  #define SCREEN_WIDTH 128 // OLED display Breite, in pixels
//  #define SCREEN_HEIGHT 64 // OLED display Höhe, in pixels
//  Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// define the sensor Pin: 
// pinMode(Axy, INPUT); //nicht nötig da über I2C

// PinModes LimitSwitches: 
// pinMode(29, OUTPUT);  //LimitSw1_Out
// pinMode(28, INPUT);  //LimitSw1_In
// pinMode(15, OUTPUT);  //LimitSw2_Out
// pinMode(14, INPUT);  //LimitSw2_In

// Define some steppers and the pins the will use
AccelStepper stepper2(AccelStepper::HALF4WIRE, 0, 2, 1, 3); // ggf.: 0, 2, 1, 3
AccelStepper stepper1(AccelStepper::HALF4WIRE, 4, 6, 5, 7); // ggf.: 4, 6, 5, 7

//Define an array
int16_t array[20][20];    // Erstellung eines Arrays mit 20x20 Einträgen - das Lesen sollte nicht viel mehr als etwa 5 min dauern

void setup()
{  
    Serial.begin(115200); 

//  I2C_1.begin(SDA_1, SCL_1, I2C_FREQ);
//  I2C_2.begin(SDA_2, SCL_2, I2C_FREQ);
  
//  setupAdc_1();
//  setupAdc_2();
    
    Wire.setClock(400000); //400kHz
    delay(10000);

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!oled.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  oled.display();
  delay(2000); // Pause for 2 seconds

//    Wire1.begin();    //nicht hier - erst unter der Definition weiter unten

// ADC:
//    ads.begin();
//    if (!ads.begin(ADS1115_I2C_addr0)) {
//      Serial.println("Finde keinen ADS1115"); //Fehlermeldung, wenn kein ADC gefunden wurde
//       while (true);
//      }

// SDA and SCL Ports for I2C: (https://forum.arduino.cc/t/raspberry-pico-setting-sda-scl-using-arduino-ide/932218/4) 
// Wire.setSDA(26); 
// Wire.setSCL(27); 
// Wire1.setSDA(10); 
// Wire1.setSCL(11); 
// Wire1.begin();

// Schrittmotoren: 
     stepper1.setMaxSpeed(500);
     stepper1.setAcceleration(400);
//     stepper1.setCurrentPosition(0);
        // stepper1.setSpeed(200);  //nur interessant, wenn man keine Be- und Entschleunigung haben will

    stepper2.setMaxSpeed(500);
    stepper2.setAcceleration(400);
//    stepper2.setCurrentPosition(0);
        //  stepper2.setSpeed(200);  //nur interessant, wenn man keine Be- und Entschleunigung haben will
        
/*
    // Change direction at the limits
    if (stepper1.distanceToGo() == 0)
  stepper1.moveTo(-stepper1.currentPosition());
*/

/*
    // der Endschalter ist nicht dort montiert, wo die Messung starten soll, sondern deutlich weiter vorne: 
    // daher müssen beide Schrittmotoren erst mal so weit fahren bis sie die entsprechenden Wände des ReliMech berühren: 
    stepper1.moveTo(x); // x noch zu bestimmen
    stepper2.moveTo(y); // y noch zu bestimmen
*/

int row = 0; 
int col = 0;

    // die folgenden Scheifen sollen nur ein Mal ausgeführt werden - daher im Setup und nicht im Loop: 
      for(int row=0; row<9; row++)    // 19 mal folgende Schleife ausführen: 
      {
// /*
        if (col == 0) {   //bzw. col < 19
            for(int i=0; i<10; i++)    // 20 mal folgende Schleife ausführen
               {
            array[row][col] = analogRead(A2);   // Messwert in Array speichern ohne ADC
            // Messwert mit ADC aufnehmen: 
            // array[row][col] = ads.readADC_SingleEnded(0); // Eingang 0 vom ADC auslesen
  
//              float voltage = 0.0;              //https://wolles-elektronikkiste.de/i2c-schnittstellen-des-esp32-nutzen
//              voltage = adc_1.getResult_V();
//              Serial.print("Voltage [V], ADS1115 No 1: ");
//              Serial.println(voltage);
             
            // Bewegung Schrittmotor um xy Schritte: 
            // Bewegungsradius ReliMech = 5x5 mm
            // 5/20 = 0,25; Schrittmotor sollte nicht gegen die Wand fahren - also Abstand! also 0,2 mm = 200 µm
            // pro Schritt 0,2µm - also 100 Schritte für jeden Messwert 
            // es gibt auch 28byj mit 32 Schritten pro Umdrehungen - also erst mal mit 50 Schritten testen!!!
            // Berechung der Summe der Werte - ggf. später für jeden Eintrag im Array 10 Werte aufnehmen und davon den Mittelwert bestimmen
             
             stepper1.move(2000);      // Schittmotor1 50 Schritte im Uhrzeigersinn drehen
               while (stepper1.distanceToGo() != 0) {
               stepper1.run();
               }
               col == col++; 
               delay(200); 

//             Serial.print("INDEX:[row][col]"); 
//             Serial.println(array[row][col]);
                }
            }
         else {   // alternativ: else if    // alternativ: col != 0  // if (col == 3)
            for(int j=10; j>0; j--)    // 20 mal folgende Schleife ausführen // col == 19
                {
            array[col][row] = analogRead(A2);   // Messwert in Array speichern ohne ADC // hier wenn möglich von hinten anfangen - sonst Schrittmotor1 vorher wieder auf 0 fahren
            // Messwert mit ADC aufnehmen: 
            // array[row][col] = ads.readADC_SingleEnded(0); // Eingang 0 vom ADC auslesen
             
             stepper1.move(-2000);     // Schittmotor1 50 Schritte gegen den Uhrzeigersinn drehen
               while (stepper1.distanceToGo() != 0) {
               stepper1.run();
               }
             col == col--;
             delay(200); 
             
             // Messwert mit ADC aufnehmen: 
             // hinzufügen

//             Serial.print("INDEX:[row][col]"); 
//             Serial.println(array[row][col]);
                }
             }

/*

        stepper1.move(500);             // Schrittmotor2 50 Schritte im Uhrzeigersinn drehen
               while (stepper1.distanceToGo() != 0) {
               stepper1.run();
               }
           delay(200);
        
*/        
        stepper2.move(2000);             // Schrittmotor2 50 Schritte im Uhrzeigersinn drehen
               while (stepper2.distanceToGo() != 0) {
               stepper2.run();
               }
           delay(200);

}

// Maximalwert im Array bestimmen und dessen Index rausfinden: 

//  int maxVal = dataArray[0][0]; // Initialize maxVal with the first element
//  int maxRow = 0;
//  int maxCol = 0;
//
//  // Traverse the 2D array to find the maximum value and its indices
//  for (int row = 0; row < 20; row++) {
//    for (int col = 0; col < 20; col++) {
//      if (dataArray[row][col] > maxVal) {
//        maxVal = dataArray[row][col];
//        maxRow = row;
//        maxCol = col;
//      }
//    }
//  }
//
//  // Output the maximum value and its indices
//  Serial.print("Maximum Value: ");
//  Serial.println(maxVal);
//  Serial.print("Index (Row, Column): (");
//  Serial.print(maxRow);
//  Serial.print(", ");
//  Serial.print(maxCol);
//  Serial.println(")");
//
//// die Schrittmotoren bis an den Anschlag fahren
//LimitSw1 = digital.Read(Axy); 
//while (LimitSw1 = LOW), 
//stepper1.move
//stepper1.run
//
//LimitSw2 = digital.Read(Axy); 
//stepper1.move
//stepper1.run


//// die Schrittmotoren an die Position 0,0 fahren
//
//    // der Endschalter ist nicht dort montiert, wo die Messung starten soll, sondern deutlich weiter vorne: 
//    // daher müssen beide Schrittmotoren erst mal so weit fahren bis sie die entsprechenden Wände des ReliMech berühren: 
//    stepper1.moveTo(x); // x noch zu bestimmen
//    stepper2.moveTo(y); // y noch zu bestimmen
//
//
//// die Schrittmotoren an den Platz mit der höchsten Intensität fahren: 
//stepper1.move(maxRow * 50);
//stepper1.run(); 
//stepper2.move(maxCol * 50);
//stepper2.run();
//
//// Serial.print(ganzen Array) // noch rauskriegen wie das geht. 
//
//  for (int row = 20; row < 0; row--) {
//    for (int col = 0; col < 20; col++) {
//      Serial.print(array[row][col]);
//      Serial.print(" "); // Add a space for formatting
//    }
//    Serial.println(); // Move to the next row in the Serial Monitor
//  }
//
//*/
//
}

void loop() {

    test();
//    delay(30);   // alt:   delay(3000); 
    oled.clearDisplay();
    oled.display();
//    delay(30);  // alt:   delay(3000); 


/*
// Display
  // Value = analogRead(A2);   // Messwert in Array speichern ohne ADC
  // Messwert mit ADC aufnehmen: 
  ValueADC = ads.readADC_SingleEnded(0); // Eingang 0 vom ADC auslesen

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // I2C address = 0x3C
  delay(1000);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);  //ggf. (SSD1306_WHITE); 
  display.setCursor(0, 0);
  display.print("Value: "); //ggf. (F("Value: "));
  display.setCursor(0,20);
  display.print(ValueADC);  
  display.display(); 
*/
}

/*
void setupAdc_1(){        //https://wolles-elektronikkiste.de/i2c-schnittstellen-des-esp32-nutzen
  if(!adc_1.init()){
    Serial.println("ADS1115 No 1 not connected!");
  }
  adc_1.setVoltageRange_mV(ADS1115_RANGE_6144);
  adc_1.setCompareChannels(ADS1115_COMP_0_GND);
  adc_1.setMeasureMode(ADS1115_CONTINUOUS); 
}
void setupAdc_2(){
  if(!adc_2.init()){
    Serial.println("ADS1115 No 2 not connected!");
  }
  adc_2.setVoltageRange_mV(ADS1115_RANGE_6144);
  adc_2.setCompareChannels(ADS1115_COMP_0_GND);
  adc_2.setMeasureMode(ADS1115_CONTINUOUS); 
}
*/

void test(void) 
{
//   float v = 0.0;
   float v = analogRead(26);
   Serial.print("Value: ");
   Serial.println(v);

//String myStr;         // TUT NICHT
//myStr = String(v); 
   
//   delay(50); // alt:    delay(500); 
    
    oled.clearDisplay();
    oled.setTextSize(1);      // Normal 1:1 pixel scale
    oled.setTextColor(SSD1306_WHITE); // Draw white text
    oled.cp437(true);         // Use full 256 char 'Code Page 437' font    
    oled.setFont(&FreeSans12pt7b);

    oled.drawRect(0, 0, SCREEN_WIDTH-1,SCREEN_HEIGHT-1 , SSD1306_WHITE);
    oled.setCursor(10, 25);   
    oled.write("Value:");
    oled.setCursor(10, 50);
//    oled.write(String(v));  TUT NICHT
    oled.print(String(v));
    
    oled.display();
//    delay(20);    // alt:     delay(2000);
}