#include <UTFT.h>

// Declare which fonts we will be using
extern uint8_t SmallFont[];

UTFT myGLCD(ILI9486,38,39,40,41);

extern unsigned short M51[0xE350];

float vx_vorher[102], vy_vorher[102];        // Geschwindigkeiten vor dem Zeitschritt in x- und y-Richtung
float vx_nachher[102], vy_nachher[102];      // Geschwindigkeiten nach dem Zeitschritt in x- und y-Richtung
float sx_vorher[102], sy_vorher[102];        // Orte vor dem Zeitschritt in x- und y-Richtung
float sx_nachher[102], sy_nachher[102];      // Orte nach dem Zeitschritt in x- und y-Richtung
float Kraft_x[102] [102];                    // Kraftmatrix für alle Wechselwirkungen zwischen allen Massen
float Kraft_y[102] [102];                    // Kraftmatrix für alle Wechselwirkungen zwischen allen Massen
float m[102];                                // Masse der Objekte

// Hinweis: Zentralkörper 1 = Index 0, Sterne Galaxie 1 = Index 1 bis n
//          Zentralkörper 2 = Index n+1, Sterne Galaxie 2 = Index n+2 bis 2n+1
// ===========================================================================

float Force_x,Force_y;            // die auf das Objekt wirkende Kraft in x- und y-Richtung
float delta_t, t, time_step;      // Zeitschrittweite und aktuelle Zeit bzw. Zeitdauer für eine komplette Berechnung
float Vergroesserung;             // Vergrößerungsfaktor bei der grafischen Darstellung


int n;                // Anzahl der Objekte ohne Zentralkörper und ohne schwarzes Loch. n muss durch 4 teilbar sein!
int zaehler, i, j;    // Zählvariable
    
const float Pi = 3.141592653589;
const float rad = Pi / 180.0;

const int Pin_continue = 6;
const int Pin_fast = 5;
const int Pin_slow = 4;
const int Pin_zoom_in = 3;
const int Pin_zoom_out = 2;

int Pin_value = A0;
int sensorValue;



// =================
// ==    SETUP    ==
// =================

void setup()
   {
    Serial.begin(9600);           // Init serial interface 

    pinMode(Pin_continue, INPUT);
    pinMode(Pin_fast, INPUT);
    pinMode(Pin_slow, INPUT);
    pinMode(Pin_zoom_in, INPUT);
    pinMode(Pin_zoom_out, INPUT);

    // Setup the LCD
    myGLCD.InitLCD();
    myGLCD.setFont(SmallFont);     
   }
    

// ================
// ==    LOOP    ==
// ================

void loop()
   {
    myGLCD.clrScr();

    myGLCD.setColor(255, 255, 0);
    myGLCD.fillRect(0, 0, 479, 13);
    myGLCD.setColor(0, 0, 0);
    myGLCD.setBackColor(255, 255, 0);
    myGLCD.print("Galaxy vs. Galaxy - stoppi", CENTER, 1);

    myGLCD.drawBitmap (10, 30, 223, 261, M51);

    myGLCD.setColor(255, 255, 255);
    myGLCD.setBackColor(0, 0, 0);
    myGLCD.print("Anzahl der Sterne", 250, 30);
    myGLCD.print("Masse Zentralkoerper 1", 250, 70);
    myGLCD.print("Masse Zentralkoerper 2", 250, 110);
    myGLCD.print("x-Position Galaxie 2", 250, 150);
    myGLCD.print("y-Position Galaxie 2", 250, 190);
    myGLCD.print("v_x Galaxie 2", 250, 230);
    myGLCD.print("v_y Galaxie 2", 250, 270);
    myGLCD.print("<Weiter>", 310, 310);

    delay(1000);

    // ======================
    // ===  WERTEEINGABE  ===
    // ======================

    while(digitalRead(Pin_continue))
       {
        sensorValue = analogRead(Pin_value);
        n = map(sensorValue,0,1023,1,12);
        n = 4 * n;
        
        myGLCD.setColor(0,0,0);
        myGLCD.fillRect(260,50,350,60);
        myGLCD.setColor(255, 255, 0);
        myGLCD.printNumI(n,260,50);
        //myGLCD.printNumF(n, 3, 260,50);
        
        delay(10);
       }
    
    delay(300);

    while(digitalRead(Pin_continue))
       {
        sensorValue = analogRead(Pin_value);
        m[0] = map(sensorValue,0,1023,1,100);
        
        myGLCD.setColor(0,0,0);
        myGLCD.fillRect(260,90,350,100);
        myGLCD.setColor(255, 255, 0);
        myGLCD.printNumI(m[0],260,90);
        //myGLCD.printNumF(m[0], 3, 260,50);
        
        delay(10);
       }
    
    delay(300);

    while(digitalRead(Pin_continue))
       {
        sensorValue = analogRead(Pin_value);
        m[n+1] = map(sensorValue,0,1023,1,100);
        
        myGLCD.setColor(0,0,0);
        myGLCD.fillRect(260,130,350,140);
        myGLCD.setColor(255, 255, 0);
        myGLCD.printNumI(m[n+1],260,130);
        //myGLCD.printNumF(m[n+1], 3, 260,130);
        
        delay(10);
       }
    
    delay(300);

    while(digitalRead(Pin_continue))
       {
        sensorValue = analogRead(Pin_value);
        sx_vorher[n+1] = map(sensorValue,0,1023,50,500);
        
        myGLCD.setColor(0,0,0);
        myGLCD.fillRect(260,170,350,180);
        myGLCD.setColor(255, 255, 0);
        myGLCD.printNumI(sx_vorher[n+1],260,170);
        //myGLCD.printNumF(sx_vorher[n+1], 3, 260,170);
        
        delay(10);
       }
    
    delay(300);

    while(digitalRead(Pin_continue))
       {
        sensorValue = analogRead(Pin_value);
        sy_vorher[n+1] = map(sensorValue,0,1023,-200,200);
        
        myGLCD.setColor(0,0,0);
        myGLCD.fillRect(260,210,350,220);
        myGLCD.setColor(255, 255, 0);
        myGLCD.printNumI(sy_vorher[n+1],260,210);
        //myGLCD.printNumF(sy_vorher[n+1], 0, 260,210);
        
        delay(10);
       }
    
    delay(300);

    while(digitalRead(Pin_continue))
       {
        sensorValue = analogRead(Pin_value);
        vx_vorher[n+1] = map(sensorValue,0,1023,-100,0);
        vx_vorher[n+1] = vx_vorher[n+1] / 50.0;
        
        myGLCD.setColor(0,0,0);
        myGLCD.fillRect(260,250,350,260);
        myGLCD.setColor(255, 255, 0);
        //myGLCD.printNumI(vx_vorher[n+1],260,250);
        myGLCD.printNumF(vx_vorher[n+1], 2, 260,250);
        
        delay(10);
       }
    
    delay(300);

        while(digitalRead(Pin_continue))
       {
        sensorValue = analogRead(Pin_value);
        vy_vorher[n+1] = map(sensorValue,0,1023,-100,100);
        vy_vorher[n+1] = vy_vorher[n+1] / 50.0;
        
        myGLCD.setColor(0,0,0);
        myGLCD.fillRect(260,290,350,300);
        myGLCD.setColor(255, 255, 0);
        //myGLCD.printNumI(vy_vorher[n+1],260,290);
        myGLCD.printNumF(vy_vorher[n+1], 2, 260,290);
        
        delay(10);
       }

    delay(300);


    // ======================
    // ===   SIMULATION   === 
    // ======================
    
    myGLCD.clrScr();
    myGLCD.setColor(255, 255, 0);
    myGLCD.fillRect(0, 0, 479, 13);
    myGLCD.setColor(0, 0, 0);
    myGLCD.setBackColor(255, 255, 0);
    myGLCD.print("Galaxy vs. Galaxy - stoppi", CENTER, 1);

    
    sx_vorher[0] = 0.0;     // Start-Standort des Zentralkörpers 1
    sy_vorher[0] = 0.0;
    vx_vorher[0] = 0.0;
    vy_vorher[0] = 0.0;
   
    t = 0.0;
    delta_t = 0.2;
    Vergroesserung = 1;
    

    // Stern-Initialisierung
    // =====================
    
    for (i = 1; i <= n; i++)
       {
        m[i] = 0.01;     // Masse eines Sterns der ersten Galaxy
       }
    
    for (i = n+2; i <= 2*n+1; i++)
       {
        m[i] = 0.01;     // Masse eines Sterns der zweiten Galaxy
       }
    
    zaehler = 0;

    for (j = 1; j <= 4; j++)      // Festlegung der Stern-Startorte und Geschwindigkeiten der Galaxie 1
       {
        for (i = 0; i < n/4; i++)
           {
            zaehler = zaehler + 1;

            sx_vorher[zaehler] = 10.0 * j * cos(rad * 360.0 * i * 4 / n);
            sy_vorher[zaehler] = 10.0 * j * sin(rad * 360.0 * i * 4 / n);
            
            vx_vorher[zaehler] = -sin(rad * 360 * i * 4 / n) * sqrt(m[0] / sqrt(sx_vorher[zaehler] * sx_vorher[zaehler] + sy_vorher[zaehler] * sy_vorher[zaehler]));
            vy_vorher[zaehler] =  cos(rad * 360 * i * 4 / n) * sqrt(m[0] / sqrt(sx_vorher[zaehler] * sx_vorher[zaehler] + sy_vorher[zaehler] * sy_vorher[zaehler]));
            
           }
       }

    zaehler = n+1;

    for (j = 1; j <= 4; j++)      // Festlegung der Stern-Startorte und Geschwindigkeiten der Galaxie 2
       {
        for (i = 0; i < n/4; i++)
           {
            zaehler = zaehler + 1;

            sx_vorher[zaehler] = 10.0 * j * cos(rad * 360.0 * i * 4 / n);
            sy_vorher[zaehler] = 10.0 * j * sin(rad * 360.0 * i * 4 / n);
            
            vx_vorher[zaehler] = -sin(rad * 360 * i * 4 / n) * sqrt(m[n+1] / sqrt(sx_vorher[zaehler] * sx_vorher[zaehler] + sy_vorher[zaehler] * sy_vorher[zaehler])) + vx_vorher[n+1];
            vy_vorher[zaehler] =  cos(rad * 360 * i * 4 / n) * sqrt(m[n+1] / sqrt(sx_vorher[zaehler] * sx_vorher[zaehler] + sy_vorher[zaehler] * sy_vorher[zaehler])) + vy_vorher[n+1];

            sx_vorher[zaehler] = sx_vorher[zaehler] + sx_vorher[n+1];
            sy_vorher[zaehler] = sy_vorher[zaehler] + sy_vorher[n+1];
            
           }
       }

    
    for(i = 0; i <= 2*n+1; i++)    // Kraft auf sich selbst ist 0
       {
        Kraft_x [i] [i] = 0;
        Kraft_y [i] [i] = 0;
       }
    
    myGLCD.setBackColor(0, 0, 0);
    myGLCD.setColor(255, 255, 0);
    myGLCD.print("dt: ", 40, 310);
    myGLCD.setColor(0,0,0);
    myGLCD.fillRect(70,310,120,320);
    myGLCD.setColor(255, 255, 0);
    myGLCD.printNumF(delta_t, 3, 70, 310);

    myGLCD.print("zoom: ", 210, 310);
    myGLCD.setColor(0,0,0);
    myGLCD.fillRect(260,310,310,320);
    myGLCD.setColor(255, 255, 0);
    myGLCD.printNumF(Vergroesserung, 3, 260, 310);

    myGLCD.print("<Weiter>", 380, 310);
        

       
    while(digitalRead(Pin_continue))
       {
        // ===================
        // === Key pressed ===
        // ===================

        if(digitalRead(Pin_fast))
           {
            delta_t = delta_t * 2.0;

            myGLCD.setColor(0,0,0);
            myGLCD.fillRect(70,310,120,320);
            myGLCD.setColor(255, 255, 0);
            myGLCD.printNumF(delta_t, 3, 70,310);

            delay(300);
           }

        if(digitalRead(Pin_slow))
           {
            delta_t = delta_t / 2.0;

            myGLCD.setColor(0,0,0);
            myGLCD.fillRect(70,310,120,320);
            myGLCD.setColor(255, 255, 0);
            myGLCD.printNumF(delta_t, 3, 70,310);

            delay(300);
           }

        if(digitalRead(Pin_zoom_in))
           {
            myGLCD.setColor(0, 0, 0);
    
            if(abs(Vergroesserung * sx_vorher[0]) <= 240 && abs(Vergroesserung * sy_vorher[0]) <= 145)
               {
                myGLCD.drawCircle((int) 240 + Vergroesserung * sx_vorher[0], (int) 160 - Vergroesserung * sy_vorher[0], 3);
               }
    
            if(abs(Vergroesserung * sx_vorher[n+1]) <= 240 && abs(Vergroesserung * sy_vorher[n+1]) <= 145)
               {   
                myGLCD.drawCircle((int) 240 + Vergroesserung * sx_vorher[n+1], (int) 160 - Vergroesserung * sy_vorher[n+1], 3);
               }

            for(i = 1; i <= n; i++)
               {
                if(abs(Vergroesserung * sx_vorher[i]) <= 240 && abs(Vergroesserung * sy_vorher[i]) <= 145)
                   { 
                    myGLCD.drawPixel((int) 240 + Vergroesserung * sx_vorher[i], (int) 160 - Vergroesserung * sy_vorher[i]);        
                   }
               }

            for(i = n+2; i <= 2*n + 1; i++)
               {
                if(abs(Vergroesserung * sx_vorher[i]) <= 240 && abs(Vergroesserung * sy_vorher[i]) <= 145)
                   { 
                    myGLCD.drawPixel((int) 240 + Vergroesserung * sx_vorher[i], (int) 160 - Vergroesserung * sy_vorher[i]);        
                   }
               }
               
            Vergroesserung = Vergroesserung * 2.0;

            myGLCD.setColor(0,0,0);
            myGLCD.fillRect(260,310,310,320);
            myGLCD.setColor(255, 255, 0);
            myGLCD.printNumF(Vergroesserung, 3, 260,310);

            delay(300);
           }

        if(digitalRead(Pin_zoom_out))
           {
            myGLCD.setColor(0, 0, 0);
    
            if(abs(Vergroesserung * sx_vorher[0]) <= 240 && abs(Vergroesserung * sy_vorher[0]) <= 145)
               {
                myGLCD.drawCircle((int) 240 + Vergroesserung * sx_vorher[0], (int) 160 - Vergroesserung * sy_vorher[0], 3);
               }
    
            if(abs(Vergroesserung * sx_vorher[n+1]) <= 240 && abs(Vergroesserung * sy_vorher[n+1]) <= 145)
               {   
                myGLCD.drawCircle((int) 240 + Vergroesserung * sx_vorher[n+1], (int) 160 - Vergroesserung * sy_vorher[n+1], 3);
               }

            for(i = 1; i <= n; i++)
               {
                if(abs(Vergroesserung * sx_vorher[i]) <= 240 && abs(Vergroesserung * sy_vorher[i]) <= 145)
                   { 
                    myGLCD.drawPixel((int) 240 + Vergroesserung * sx_vorher[i], (int) 160 - Vergroesserung * sy_vorher[i]);        
                   }
               }

            for(i = n+2; i <= 2*n + 1; i++)
               {
                if(abs(Vergroesserung * sx_vorher[i]) <= 240 && abs(Vergroesserung * sy_vorher[i]) <= 145)
                   { 
                    myGLCD.drawPixel((int) 240 + Vergroesserung * sx_vorher[i], (int) 160 - Vergroesserung * sy_vorher[i]);        
                   }
               }
               
            Vergroesserung = Vergroesserung / 2.0;

            myGLCD.setColor(0,0,0);
            myGLCD.fillRect(260,310,310,320);
            myGLCD.setColor(255, 255, 0);
            myGLCD.printNumF(Vergroesserung, 3, 260,310);

            delay(300);
           }

         
        // =======================================================================
        // Berechnung der neuen Orte und Geschwindigkeiten mittels Euler-Verfahren
        // =======================================================================

        //time_step = millis();
    
        Euler();

        // Alte Bildpunkte löschen
        // =======================

        myGLCD.setColor(0, 0, 0);
    
        if(abs(Vergroesserung * sx_vorher[0]) <= 240 && abs(Vergroesserung * sy_vorher[0]) <= 145)
           {
            myGLCD.drawCircle((int) 240 + Vergroesserung * sx_vorher[0], (int) 160 - Vergroesserung * sy_vorher[0], 3);
           }
    
        if(abs(Vergroesserung * sx_vorher[n+1]) <= 240 && abs(Vergroesserung * sy_vorher[n+1]) <= 145)
           {   
            myGLCD.drawCircle((int) 240 + Vergroesserung * sx_vorher[n+1], (int) 160 - Vergroesserung * sy_vorher[n+1], 3);
           }

        for(i = 1; i <= n; i++)
           {
            if(abs(Vergroesserung * sx_vorher[i]) <= 240 && abs(Vergroesserung * sy_vorher[i]) <= 145)
               { 
                myGLCD.drawPixel((int) 240 + Vergroesserung * sx_vorher[i], (int) 160 - Vergroesserung * sy_vorher[i]);        
               }
           }

        for(i = n+2; i <= 2*n + 1; i++)
           {
            if(abs(Vergroesserung * sx_vorher[i]) <= 240 && abs(Vergroesserung * sy_vorher[i]) <= 145)
               { 
                myGLCD.drawPixel((int) 240 + Vergroesserung * sx_vorher[i], (int) 160 - Vergroesserung * sy_vorher[i]);        
               }
           }
    
        // Werteübergabe und Ortsverschiebung
        // ==================================

        for (i = 0; i <= 2*n + 1; i++)
           {
            sx_vorher[i] = sx_nachher[i] - sx_nachher[0];    // Werteübergabe der Orte und Verschiebung, da Zentralkörper immer im Punkt (0,0)
            sy_vorher[i] = sy_nachher[i] - sy_nachher[0];    // Werteübergabe der Orte und Verschiebung, da Zentralkörper immer im Punkt (0,0)
            vx_vorher[i] = vx_nachher[i];                    // Werteübergabe der Geschwindigkeiten
            vy_vorher[i] = vy_nachher[i];                    // Werteübergabe der Geschwindigkeiten
           }
         
        // neue Orte zeichnen
        // ==================

        myGLCD.setColor(0, 255, 0);
    
        if(abs(Vergroesserung * sx_vorher[0]) <= 240 && abs(Vergroesserung * sy_vorher[0]) <= 145)
           {
            myGLCD.drawCircle((int) 240 + Vergroesserung * sx_vorher[0], (int) 160 - Vergroesserung * sy_vorher[0], 3);
           }
    
        for(i = 1; i <= n; i++)
           {
            if(abs(Vergroesserung * sx_vorher[i]) <= 240 && abs(Vergroesserung * sy_vorher[i]) <= 145)
               { 
                myGLCD.drawPixel((int) 240 + Vergroesserung * sx_vorher[i], (int) 160 - Vergroesserung * sy_vorher[i]);        
               }
           }

        myGLCD.setColor(255, 255, 255);
    
        if(abs(Vergroesserung * sx_vorher[n+1]) <= 240 && abs(Vergroesserung * sy_vorher[n+1]) <= 145)
           {   
            myGLCD.drawCircle((int) 240 + Vergroesserung * sx_vorher[n+1], (int) 160 - Vergroesserung * sy_vorher[n+1], 3);
           }

        for(i = n+2; i <= 2*n + 1; i++)
           {
            if(abs(Vergroesserung * sx_vorher[i]) <= 240 && abs(Vergroesserung * sy_vorher[i]) <= 145)
               { 
                myGLCD.drawPixel((int) 240 + Vergroesserung * sx_vorher[i], (int) 160 - Vergroesserung * sy_vorher[i]);        
               }
           }

    
        t = t + delta_t;

        //Serial.println(millis() - time_step);

        //delay(500);

        //Serial.println(sx_nachher[2]);
       }
       
   }



// ==============================
// ===  UNTERPROGRAMM FORCE   ===
// ==============================

void Force()
   {
    float distance;                  // Distanz zwischen zwei Objekten i_Force und j_Force
    float einheit_x, einheit_y;      // Einheitsvektor in x- bzw. y-Richtung zwischen zwei Objekten i_Force und j_Force
    int k_Force, l_Force;            // Zählvariablen

    for(k_Force = 0; k_Force <= 2*n; k_Force++)
       {
        for(l_Force = k_Force + 1; l_Force <= 2*n + 1; l_Force++)
           {
            distance = sqrt((sx_vorher[l_Force] - sx_vorher[k_Force]) * (sx_vorher[l_Force] - sx_vorher[k_Force]) + (sy_vorher[l_Force] - sy_vorher[k_Force]) * (sy_vorher[l_Force] - sy_vorher[k_Force])); 

            // Berechnung des Einheitsvektors in Richtung der Verbindungsgeraden der beiden Massen

            einheit_x = (1.0 / distance) * (sx_vorher[l_Force] - sx_vorher[k_Force]);
            einheit_y = (1.0 / distance) * (sy_vorher[l_Force] - sy_vorher[k_Force]);

            Kraft_x [k_Force] [l_Force] = (m[k_Force] * m[l_Force] / (distance * distance)) * einheit_x;
            Kraft_y [k_Force] [l_Force] = (m[k_Force] * m[l_Force] / (distance * distance)) * einheit_y;
            
            /*
            Serial.print("F[");
            Serial.print(k_Force);
            Serial.print(",");
            Serial.print(l_Force);
            Serial.print("] = ");
            Serial.print(Kraft_x [k_Force] [l_Force]);
            Serial.print("    ");
            Serial.println(Kraft_y [k_Force] [l_Force]);
            delay(500);
            */
            
            Kraft_x [l_Force] [k_Force] = -Kraft_x [k_Force] [l_Force];
            Kraft_y [l_Force] [k_Force] = -Kraft_y [k_Force] [l_Force];
           }
       }   
   }


  
// =====================================
// ==  UNTERPROGRAMM EULER-Verfahren  ==
// =====================================

void Euler()
   {
    int i_Euler, j_Euler;                     // Zählvariablen

    Force();

    for (i_Euler = 0; i_Euler <= 2*n + 1; i_Euler++)
        {
         Force_x = 0;
         Force_y = 0;
         
         for (j_Euler = 0; j_Euler <= 2*n + 1; j_Euler++)
            {
             Force_x = Force_x + Kraft_x [i_Euler] [j_Euler];
             Force_y = Force_y + Kraft_y [i_Euler] [j_Euler];             
            }

         vx_nachher[i_Euler] = vx_vorher[i_Euler] + delta_t * (1.0 / m[i_Euler]) * Force_x;
         vy_nachher[i_Euler] = vy_vorher[i_Euler] + delta_t * (1.0 / m[i_Euler]) * Force_y;
         
         sx_nachher[i_Euler] = sx_vorher[i_Euler] + delta_t * 0.5 * (vx_vorher[i_Euler] + vx_nachher[i_Euler]);
         sy_nachher[i_Euler] = sy_vorher[i_Euler] + delta_t * 0.5 * (vy_vorher[i_Euler] + vy_nachher[i_Euler]);
        }
   }