/*
Header Datei mit den meisten Funktionen für den Lidar

*/

int health[722] ;
int header[16]  ;
int raw_data_array[1700] ;
int angle[150] ;
int sectors [180][7] ;

int sign_scan = 0 ;
int pre_sign_scan = 0 ;
int ch = 0 ;
int distance = 0 ;
int werte_start = 0 ;
int werte_ende = 0 ;
int average_angle(int anfang, int ende) ;

int sektor_1 = 0 ;
int sektor_2 = 0 ;
int sektor_3 = 0 ;
int sektor_d = 0 ;
int sektor_4 = 0 ;
int sektor_5 = 0 ;
int sektor_6 = 0 ; 

float LSA ;
float FSA ;

//Motor 1
const int motorPin1 = 25;
const int motorPin2 = 26;

//Motor 2
const int motorPin3 = 12;
const int motorPin4 = 14;
void read_header()                                        // liest die Header-Informationen ein, wenn "AA 55" erkannt wurde
{                                                         // dann werden aus den Header-Daten FSA und LSA errechnet
  // Serial.println("Lese Header");
   header[0] = pre_sign_scan ;
   header[1] = sign_scan ;
   
   for (int i = 2 ; i <= 11 ; i++)                        // der Header hat nur 8 relevante Bytes, dann kommen Prüfcode und Testdaten, also statt 16 besser 11 - erledigt
   {
    if (Serial2.available())
      {
      ch = (Serial2.read()) ;                            // muss wahrscheinlich noch in "if Serial2.available ....." - erledigt
      header[i] = ch ;
      }
   }
     header[12] = (header[5] << 8 | header[4]);
     header[13] = ((header[12] >> 1)/64.0) ;
     LSA =  header[13] ;
     header[14] = (header[7] << 8 | header[6]);
     header[15] = ((header[14] >> 1)/64.0) ;
     FSA =  header[15] ;
     if (FSA > 360)
      {
         Serial.print("Headerfehler   ") ;
         Serial.println(FSA);
         FSA = 360 ;
      }    
}

void raw_data_einlesen()            // liest 1700 bytes von der Schnittstelle in das raw_data_array ein
        {
        int i = 0;
                
        while (i <= 1699)
          {                        
            if (Serial2.available())
             {
              sign_scan = (Serial2.read()) ;                        
              raw_data_array[i] = sign_scan ;                                                     
              i++ ;                                                     
             }                                           
            }  
           }

void extract_winkel()                                       // im raw_data_array stehen 1700 aufeinander folgende Daten - was mindestens den Daten eines Vollkreises entspricht
{                                                           // das array wird der Reihe nach durchsucht und   
   Serial.println("Suche Winkel");
   int k = 0 ;
  for (int i = 1 ; i < 1689 ; i++)
   {
    pre_sign_scan = raw_data_array[i-1];
    sign_scan =raw_data_array[i];
   
    if (pre_sign_scan == 0xAA && sign_scan == 0x55)        // wenn der Anfang einen Datenrunde "AA55" im array gefunden wurde ...
     {      
     ch = raw_data_array[i+2] ;                            // werden die Anzahl der Werte (meist "28") ... 
     werte_start = (i+9) ;                                // sowie die Position (index des raw_data_array) an der die Entfernungsdatenfolge beginnt ...
     werte_ende =((werte_start) + (2*(ch-1))) ;           // und die Position an der sie endet, ermittelt     
     angle[k] = (ch) ;    
     angle[k+1] = (raw_data_array[i+4] << 8 | raw_data_array[i+3]);   // hier wird der Winkel "LSA" ermittelt und zugewiesen
     angle[k+2] = ((angle[k+1] >> 1)/64.0) ;         
     LSA =  angle[k+2] ;
     angle[k+3] = (raw_data_array[i+6] << 8 | raw_data_array[i+5]);  // hier wird der Winkel "FSA" ermittelt und zugewiesen
     angle[k+4] = ((angle[k+3] >> 1)/64.0) ;
     FSA =  angle[k+4] ;     
     angle[k+5] = werte_start ;
     angle[k+6] = werte_ende ;    
     k = k+7 ;                                                      // der index im array "angle" wird erhöht und der nächste "Satz Daten" angelegt                       
     } 
   }
}
  
void arrays_umsortieren()                             // diese Funktion sortiert nach Maßgabe der Daten im Array "angle" aus dem raw_data_array die Werte in das array "health" 
  {
    int z = 0 ;                                       // Zähler für den Index angle_array - pro Runde um 7 erhöhen
    int health_ind = 0 ;                              // Index des Ergebnis-Array und entspricht immer dem Doppelten des Winkels
    int rounds = 0 ;                                  // Anzahl der Messwerte
    int angle_low = 0 ;                               // Index vom raw_data_array in dem das erste LOW-Byte steht
    int ta_low = 0 ;                                  // low_Byte des Entfernungswertes
    int ta_high = 0 ;                                 // high_Byte des Entfernungswertes
 
    while(z <= 149)    
      {
         health_ind = (angle[z+2] * 2) ;                                             
         rounds = angle[z] ;                           
         angle_low = angle[z+5] ; 
         
         for (int i = 1 ; i <= rounds ; i++)
          {                    
             ta_low = raw_data_array[angle_low]  ;             
             ta_high = raw_data_array[angle_low +1]  ;                                  
             distance = (((ta_high << 8 | ta_low) / 4)) ;
             health[health_ind] = distance ;
             angle_low++ ;
             angle_low++ ;
             health_ind++ ;                      
           }         
             z = (z + 7) ;                             
      }
  }

void sectors_einlesen(int sect)
  {
     int health_position = 270 ;
     int chs = 0 ;
     for (int i = 0 ; i <= 179 ; i++)
    {
      chs = health[health_position] ;            
      sectors [i][sect]=  chs ;
      health_position++ ;          
    }     
  }

void summieren()                          // soll zeilenweise die Summe der Spalten bilden und in Spalte 6 den Mittelwert ablegen
  {                                       // wenn ein Wert Null ist, soll dieser nicht mit einfließen. Division durch Null wird verhindert    
     int line_wert = 0;
     int row_sum = 0 ;
     int counter = 0 ;
     
     for (int i = 0 ; i <= 179 ; i++)
       {       
         for (int sum = 0 ; sum <= 4 ; sum++)
            {
              line_wert = sectors[i][sum];
              
              if (line_wert > 0)
                {
                row_sum = row_sum + line_wert ;                                
                counter++ ;               
                }              
            }              
               if (counter == 0)
                {
                  counter = 1 ;         // Division durch Null verhindern
                }
               sectors[i][5] = row_sum/counter ;                                                        
               row_sum = 0 ;
               counter = 0 ;    
        }
       Serial.println("Summieren beendet");                 
   }

void sektoren_ermitteln()
{  
   sektor_1 = average_angle(0, 29) ;
   sektor_2 = average_angle(30, 59) ;
   sektor_3 = average_angle(60, 89) ;
   sektor_d = average_angle(88, 92) ;
   sektor_4 = average_angle(90, 119) ;
   sektor_5 = average_angle(120, 149) ;
   sektor_6 = average_angle(150, 179) ;
/*
   Serial.print("Sektor 1 ist  ") ;
   Serial.println(sektor_1) ;
   Serial.print("Sektor 2 ist  ") ;
   Serial.println(sektor_2) ;
   Serial.print("Sektor 3 ist  ") ;
   Serial.println(sektor_3) ;
   Serial.print("Sektor 4 ist  ") ;
   Serial.println(sektor_4) ;
   Serial.print("Sektor 5 ist  ") ;
   Serial.println(sektor_5) ;
   Serial.print("Sektor 6 ist  ") ;
   Serial.println(sektor_6) ;
   Serial.print("Sektor d ist  ") ;
   Serial.println(sektor_d) ;
*/
}


int average_angle(int anfang, int ende) 
{ 
   int winkel = 0 ;
   int count = 1 ;
   int winkel_sum = 0 ;
   int ergebnis = 0 ;
   
   for (int i = anfang ; i <= ende ; i++)
        {
        winkel = sectors[i][5] ;
          
          if (winkel > 0)
              {
                winkel_sum = winkel_sum + winkel ;
                count++ ;               
              } 
        }
 
        if (count == 0)
        {
          count = 1 ;
        }
        
     ergebnis = winkel_sum/(count) ;
     // Serial.print("average_angle ermittelt  ") ;
     // Serial.println(ergebnis) ;
     return ergebnis ;
}

void forward() {
   digitalWrite(motorPin1, HIGH);  
   digitalWrite(motorPin2, LOW);  
   digitalWrite(motorPin3, LOW);  
   digitalWrite(motorPin4, HIGH);
   //delay(1000) ;
} 

void turnleft() {
   digitalWrite(motorPin1, HIGH);  // Motor 1 beginnt zu rotieren
   digitalWrite(motorPin2, LOW);
   digitalWrite(motorPin3, HIGH);  // Motor 2 beginnt zu rotieren
   digitalWrite(motorPin4, LOW);
   //delay(1000);
} 
  
void turnright() {
   digitalWrite(motorPin1, LOW);  
   digitalWrite(motorPin2, HIGH);
   digitalWrite(motorPin3, LOW);  
   digitalWrite(motorPin4, HIGH);
   //delay(1000);
} 

void backward() {
   digitalWrite(motorPin1, LOW);  
   digitalWrite(motorPin2, HIGH);    
   digitalWrite(motorPin3, HIGH);  
   digitalWrite(motorPin4, LOW);
   //delay(1000);
} 

void nomove() {
   digitalWrite(motorPin1, LOW);  // Motor 1 steht
   digitalWrite(motorPin2, LOW);
   digitalWrite(motorPin3, LOW);  // Motor 2 steht
   digitalWrite(motorPin4, LOW);
   delay(50);
}