#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

const int SCREEN_WIDTH = 128;
const int SCREEN_HEIGHT = 64;

const int OLED_RESET = -1;
const int maksAntall = 1; // 190 er maks på Arduino Uno R3 per 05.04.2024.

const int SCREEN_ADDRESS = 0x3c;
Adafruit_SSD1306 skjerm(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
Adafruit_MPU6050 mpu;

const int referanseForhold = 50;
const int skjermHZ = 40;

static const unsigned char PROGMEM logo_bmp[] =
{ 0b00000000, 0b11000000,
  0b00000001, 0b11000000,
  0b00000001, 0b11000000,
  0b00000011, 0b11100000,
  0b11110011, 0b11100000,
  0b11111110, 0b11111000,
  0b01111110, 0b11111111,
  0b00110011, 0b10011111,
  0b00011111, 0b11111100,
  0b00001101, 0b01110000,
  0b00011011, 0b10100000,
  0b00111111, 0b11100000,
  0b00111111, 0b11110000,
  0b01111100, 0b11110000,
  0b01110000, 0b01110000,
  0b00000000, 0b00110000 };

int xPosArray[maksAntall];
int yPosArray[maksAntall];
int xVelArray[maksAntall];
int yVelArray[maksAntall];

unsigned long deltaTid, forrigeDeltaTid, intervall = 200;

void setup() {

  skjerm.begin(SCREEN_ADDRESS);
  Serial.begin(115200);

   if(!skjerm.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }

  mpu.begin();
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);

  skjerm.display();
  delay(2000);
  skjerm.clearDisplay();

  for (int i = 0; i < maksAntall; i++){
  int xRand = random(0,((SCREEN_WIDTH-1)*referanseForhold));
  int yRand = random(0, ((SCREEN_HEIGHT-1)*referanseForhold));
  xPosArray[i] = xRand;
  yPosArray[i] = yRand;
  }
}

int gravitet[2] = {0, -500};                  // maks fart (x,y)
int aks = 20;                                 // akselerasjon
int mpuXYZ[3];                                // X-aksen er 0 når den ligger flatt.
float korrektData[3];

void loop() {
  gravitet[0] = 0;
  gravitet[1] = -500;

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  mpuXYZ[0] = a.gyro.x;
  mpuXYZ[1] = a.gyro.y;
  mpuXYZ[2] = a.gyro.z;

  for (int i = 0; i < 2; i++){
    if (i == 0){
      if (mpuXYZ[i] > 0){
      korrektData[i] = map(mpuXYZ[i], 0, 10, 0, 10) / 10;
    } else if (mpuXYZ[i] < 0){
      korrektData[i] = map(mpuXYZ[i], 0, -10, 0, 10) / 10;
    }
      continue;
    }
    if (mpuXYZ[i] > 0){
      korrektData[i] = ((map(mpuXYZ[i], 0, 10, 0, 10) / 10) + 1);
    } else if (mpuXYZ[i] < 0){
      korrektData[i] = ((map(mpuXYZ[i], 0, -10, 0, 10) / 10) + 1);
    }
  }
  
  deltaTid = millis();
    for (int i = 0; i < maksAntall; i++){
      yVelArray[i] += (aks * korrektData[1]);
      xVelArray[i] += (aks * korrektData[0]);
      if (yPosArray[i] > ((SCREEN_HEIGHT-1)*referanseForhold)){    // Sørger for at partikler holder seg innen skjermen.
        yVelArray[i] = 0;
        yPosArray[i] = ((SCREEN_HEIGHT-1)*referanseForhold);
      }
      if (yPosArray[i] <= 0){
        yVelArray[i] = 50;
      }
      if (xPosArray[i] > ((SCREEN_WIDTH-2)*referanseForhold)){    // Sørger for at partikler holder seg innen skjermen.
        xVelArray[i] = -200;
      }
      if (xPosArray[i] <= 10){
        xVelArray[i] = 200;
      }
      if (xVelArray[i] > -gravitet[1]/2){            // Sørger for at ingen partikler beveger seg for fort
        xVelArray[i]--;
        }   
      if (xVelArray[i] < gravitet[1]){           // ~~~||~~~
        xVelArray[i]++;
      }
      if (yVelArray[i] > -gravitet[1]/2){            // Sørger for at ingen partikler beveger seg for fort
        yVelArray[i] = -gravitet[1]/2;
        }   
      if (yVelArray[i] < gravitet[1]){           // ~~~||~~~
        yVelArray[i] = gravitet[1];
      }
      if (xVelArray[i] > 0){                  // Horisontal fart minker gradvis.
        xVelArray[i] -= 4;
      } else if (xVelArray[i] < 0){
        xVelArray[i] += 4;
      }
      if (skjerm.getPixel((xPosArray[i] + xVelArray[i]) / referanseForhold, (yPosArray[i] + yVelArray[i]) / referanseForhold) == true){
        yPosArray[i] -= referanseForhold;
        yVelArray[i] = 0;
        xVelArray[i] += random(-50, 50);
      }

      if(deltaTid - forrigeDeltaTid >= intervall){
        Serial.print("\n\n\n\n\n");
        Serial.print("X: ");
        Serial.println(korrektData[0]);
        Serial.print("Y: ");
        Serial.println(korrektData[1]);
        Serial.print("Z: ");
        Serial.println(korrektData[2]);
        forrigeDeltaTid = deltaTid;
      }

      xPosArray[i] += xVelArray[i];
      yPosArray[i] += yVelArray[i];
      
    skjerm.drawPixel((xPosArray[i]/referanseForhold), (yPosArray[i]/referanseForhold), SSD1306_WHITE);
 
    }

  skjerm.display();
  delay(1000/skjermHZ);
  skjerm.clearDisplay();
}


/*
Mål 1: Få en partikkel til å bevege seg. Ferdig

Mål 2: Få graviteten til å snu seg.

Problem 1: Partikler leker ut på et vis. Fiksa
Problem 2: X-fart minker for lite over tid. Fiksa


*/