/*
 * Board: ESP 32 38 Pin
 * 
 * Ydlidar         <-> ESP32 38 Pin
 *  1 (+5V, red)   <->  5V
 *  2 (TX, green)  <->  GPIO 16 (RX verbunden mit TX des Lidar)
 *  3 (RX, yellow) <->  GPIO 17 (TX verbunden mit RX des Lidar)
 *  4 (GND, black) <->  GND
 *  5 (M_SCTR, nc) <->  IO ? Motor speed control (noch nicht verbunden)
 *  6 (M_DEV, nc)
 *  7 (M_EN, blue) <->  GPIO 22 Motor enable
 *  8 (NC, nc)
 * 
 * Author: Wilfried
 * History:
 * started: 02.04.2024    finished:
 * This sketch was created with Arduino IDE 1.8.16.
 */

 #include <HardwareSerial.h>
 #include "LidarFunctions.h"

 #define RXD2 16
 #define TXD2 17
 
const int ydlidar_en_pin = 4 ;       // Motor enable
const int start_pin = 23 ;           // Starte den Scan über den Schalter

int start_pin_state ;

int look = 0 ;
int nudge = 0 ;
int turn = 0 ;
char start ;

void ydlidar_start()
{
  look = 0 ;
  digitalWrite(ydlidar_en_pin,HIGH);
  Serial.println("Start Scan") ;
  Serial2.write(0xA5);
  Serial.print(0xA5, HEX) ;
  Serial.print("   ") ;
  Serial2.write(0x60);                 // Schaltet in den Scan-Modus
  Serial.println(0x60, HEX) ; 
}

void ydlidar_stop()
{
  Serial2.write(0xA5);
  Serial.print(0xA5, HEX) ;
  Serial.print("   ") ;
  Serial2.write(0x65);
  Serial.println(0x65, HEX) ;
  Serial.println("gestoppt ! ") ;
}
     
void setup()
{  
    pinMode(motorPin1, OUTPUT);
    pinMode(motorPin2, OUTPUT);
    pinMode(motorPin3, OUTPUT);
    pinMode(motorPin4, OUTPUT);
    digitalWrite(motorPin1, LOW);  // Motor 1 steht
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, LOW);  // Motor 2 steht
    digitalWrite(motorPin4, LOW); 

    pinMode(ydlidar_en_pin,OUTPUT);
    digitalWrite(ydlidar_en_pin,HIGH);
         
    pinMode(start_pin,INPUT);

    Serial.begin(500000);
    Serial2.begin(128000, SERIAL_8N1, RXD2, TXD2);
    
    Serial.println("Esp32 38 PIN listening on Serial 2, sending on Serial 0");
}

void loop()
  {
     // char start = Serial.read() ;          // können wir über die Eingabe am seriellen Monitor starten
     // if (start == 's')
    
    int new_sign = 0 ;
    
    start_pin_state = digitalRead(start_pin) ;        
    
    // hier bauen wir ein: --> if LOW "nomove() sonst --> den Rest ...."
    
    if (start_pin_state == HIGH)
   {   
    forward() ;      
      {               
        ydlidar_start() ;
        int look = 0 ;
        int sect = 0 ;

       while(look <= 4 )
       {
         
       if (Serial2.available())                     
           {                                          
           pre_sign_scan = sign_scan ;
           sign_scan = (Serial2.read()) ;  
           new_sign = sign_scan ;                                    
                                                                                
        if (pre_sign_scan == 0xAA && sign_scan == 0x55)
              {                              
                read_header() ;

                  if (header[2] == 1)
                    {
//                    Serial.println("Anfang gefunden   ");                 
                    raw_data_einlesen() ;
                    extract_winkel();
                    arrays_umsortieren() ;
                    sectors_einlesen(sect);
                    look++ ;
                    sect++;
//                    Serial.print("look ist:  ") ; 
//                    Serial.println(look) ;                                                                                                          
                      }                     
                     }                                                  
                    }                                     
                   }                
                ydlidar_stop();                
                summieren() ;                              //ermittelt den Mittelwert der Zeilen und legt ihn in [i][5] ab               
                sektoren_ermitteln() ;
                   if (sektor_d <= 200)
                    {
//                      Serial.print("sektor_d ist kleiner als 20 cm ") ;     
                      nomove() ;
                      delay (30) ;
                      backward() ;
                      delay (50) ;                                                                 
                      turn = highest_distance() ; 
//                      Serial.print("turn ist:  ") ; 
//                      Serial.println(turn) ;    

                     switch (turn)
                      {
                       case 11 :  //Serial.println("case 11  ") ;
                                  nomove() ;
                                  delay (30) ;
                                  turnright() ;
                                  delay (200) ;
                                  nomove() ;
                                  delay (30) ;
                                  forward() ;
                                  break ;
                       case 12 :  //Serial.println("case 12  ") ;
                                  nomove() ;
                                  delay (30) ;
                                  turnright() ;
                                  delay (400) ;
                                  nomove() ;
                                  delay (30) ;
                                  forward() ;
                                  break ;     
                       case 13 :  //Serial.println("case 13  ") ;
                                  nomove() ;
                                  delay (30) ;
                                  turnright() ;
                                  delay (600) ;
                                  nomove() ;
                                  delay (30) ;
                                  forward() ;
                                  break ;
                       case 14 :  //Serial.println("case 14  ") ;
                                  nomove() ;
                                  delay (30) ;
                                  turnleft() ;
                                  delay (200) ;
                                  nomove() ;
                                  delay (30) ;
                                  forward() ;     
                                  break ;
                       case 15 :  //Serial.println("case 15  ") ;
                                  nomove() ;
                                  delay (30) ;
                                  turnleft() ;
                                  delay (400) ;
                                  nomove() ;
                                  delay (30) ;
                                  forward() ;
                                  break ;
                       case 16 :  //Serial.println("case 16  ") ;
                                  nomove() ;
                                  delay (30) ;
                                  turnleft() ;
                                  delay (600) ;
                                  nomove() ;
                                  delay (30) ;
                                  forward() ;
                                  break ;
                 }
                }                                                                               
               }                                 
              }
              else 
              { nomove(); }
             }

             
int highest_distance()
  {
 int highest_distance = sektor_1 ;
 int nudge = 11 ; 

    if (sektor_2 > highest_distance)
        {
          highest_distance = sektor_2 ;
          nudge = 12 ;         
        }
    if (sektor_3 > highest_distance)
        {
          highest_distance = sektor_3 ; 
          nudge = 13 ;        
        }
    if (sektor_4 > highest_distance)
        {
          highest_distance = sektor_4 ;
          nudge = 14 ;
        }
    if (sektor_5 > highest_distance)
        {
          highest_distance = sektor_5 ;
          nudge = 15 ;
        }
    if (sektor_6 > highest_distance)
        {
          highest_distance = sektor_6 ;
          nudge = 16 ;
        }           
      return nudge ;              
    }