#include <SoftwareSerial.h>
#include <TinyGPS++.h>
#include <MHZ19.h>
#include <Wire.h>
#include <math.h>
#include <SPI.h>
#include <SD.h>

#define BMP_ADDR 0x76
#define LIS_ADDR 0x19
const int chipSelect = 10;

int a = 1;
uint16_t dig_T1; int16_t dig_T2, dig_T3;
uint16_t dig_P1; int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9;
int32_t t_fine;
float offX = 0, offY = 0, offZ = 0;

TinyGPSPlus gps;

static const int RXPin = 3, TXPin = 4;
static const uint32_t GPSBaud = 9600;

SoftwareSerial gpsSerial(RXPin, TXPin);
SoftwareSerial co2Serial(A1, A2);
MHZ19 myMHZ19;

double lastLat = 0, lastLng = 0;
bool hasFix = false;

unsigned long lastSend = 0;
const unsigned long INTERVAL = 1000;

// --- POMOŽNA FUNKCIJA ZA ZAPISOVANJE ---
void logData(String data) {
  Serial.print(data); 
  File dataFile = SD.open("datalog.csv", FILE_WRITE);
  if (dataFile) {
    dataFile.print(data);
    dataFile.close();
  }
}

void logDataLn(String data) {
  Serial.println(data); 
  File dataFile = SD.open("datalog.csv", FILE_WRITE);
  if (dataFile) {
    dataFile.println(data);
    dataFile.close();
  }
}

// --- Funkcije za senzorje ---
void readBmpCalib() {
  Wire.beginTransmission(BMP_ADDR); Wire.write(0x88); Wire.endTransmission();
  Wire.requestFrom(BMP_ADDR, 24);
  dig_T1 = Wire.read()|Wire.read()<<8; dig_T2 = Wire.read()|Wire.read()<<8; dig_T3 = Wire.read()|Wire.read()<<8;
  dig_P1 = Wire.read()|Wire.read()<<8; dig_P2 = Wire.read()|Wire.read()<<8; dig_P3 = Wire.read()|Wire.read()<<8;
  dig_P4 = Wire.read()|Wire.read()<<8; dig_P5 = Wire.read()|Wire.read()<<8; dig_P6 = Wire.read()|Wire.read()<<8;
  dig_P7 = Wire.read()|Wire.read()<<8; dig_P8 = Wire.read()|Wire.read()<<8; dig_P9 = Wire.read()|Wire.read()<<8;
}

float getTemp(int32_t adc_T) {
  int32_t v1 = ((((adc_T>>3)-((int32_t)dig_T1<<1)))*((int32_t)dig_T2))>>11;
  int32_t v2 = (((((adc_T>>4)-((int32_t)dig_T1))*((adc_T>>4)-((int32_t)dig_T1)))>>12)*((int32_t)dig_T3))>>14;
  t_fine = v1 + v2;
  return (float)((t_fine * 5 + 128) >> 8) / 100.0;
}

float getPress(int32_t adc_P) {
  int64_t v1, v2, p;
  v1 = ((int64_t)t_fine) - 128000;
  v2 = v1 * v1 * (int64_t)dig_P6;
  v2 = v2 + ((v1 * (int64_t)dig_P5) << 17);
  v2 = v2 + (((int64_t)dig_P4) << 35);
  v1 = ((v1 * v1 * (int64_t)dig_P3) >> 8) + ((v1 * (int64_t)dig_P2) << 12);
  v1 = (((((int64_t)1) << 47) + v1)) * ((int64_t)dig_P1) >> 33;
  if (v1 == 0) return 0;
  p = 1048576 - adc_P; p = (((p << 31) - v2) * 3125) / v1;
  v1 = (((int64_t)dig_P9) * (p >> 13) * (p >> 13)) >> 25;
  v2 = (((int64_t)dig_P8) * p) >> 19;
  p = ((p + v1 + v2) >> 8) + (((int64_t)dig_P7) << 4);
  return (float)p / 25600.0;
}

void getRawAcc(int16_t &x, int16_t &y, int16_t &z) {
  Wire.beginTransmission(LIS_ADDR); Wire.write(0x28 | 0x80); Wire.endTransmission();
  Wire.requestFrom(LIS_ADDR, 6);
  if(Wire.available()>=6) {
    x = Wire.read()|Wire.read()<<8; y = Wire.read()|Wire.read()<<8; z = Wire.read()|Wire.read()<<8;
  }
}

void setup() {
  Serial.begin(9600);
  
  // INICIALIZACIJA SD KARTICE
  if (!SD.begin(chipSelect)) {
    logData(";;SD napaka!");
  } else {
    logData(";;SD pripravljen.");
  }

  Wire.begin();
  gpsSerial.begin(GPSBaud);
  
  // Tukaj je bila odstranjena "while(!hasFix)" zanka, tako da 
  // program ne blokira več in takoj nadaljuje.

  co2Serial.begin(9600);
  readBmpCalib();

  Wire.beginTransmission(BMP_ADDR); Wire.write(0xF4); Wire.write(0x27); Wire.endTransmission();
  Wire.beginTransmission(LIS_ADDR); Wire.write(0x20); Wire.write(0x57); Wire.endTransmission();

  long sx=0, sy=0, sz=0;
  for(int i=0; i<30; i++) {
    int16_t tx,ty,tz;
    getRawAcc(tx,ty,tz);
    sx+=tx; sy+=ty; sz+=tz;
    delay(10);
  }
  offX = (sx/30.0)/16384.0; offY = (sy/30.0)/16384.0; offZ = (sz/30.0)/16384.0;

  myMHZ19.begin(co2Serial);
  gpsSerial.listen();
}

void loop() {
  gpsSerial.listen();

  while (gpsSerial.available() > 0) {
    if (gps.encode(gpsSerial.read())) {
      if (gps.location.isValid()) {
        lastLat = gps.location.lat();
        lastLng = gps.location.lng();
        hasFix = true;
      }
    }
  }

  if (millis() - lastSend >= INTERVAL) {
    static int lastCo2Val = 0;
    
    co2Serial.listen();
    int currentCo2 = myMHZ19.getCO2();
    if (currentCo2 > 0) lastCo2Val = currentCo2;

    Wire.beginTransmission(BMP_ADDR); Wire.write(0xF7); Wire.endTransmission();
    Wire.requestFrom(BMP_ADDR, 6);

    int32_t p_raw = ((uint32_t)Wire.read() << 12) | ((uint32_t)Wire.read() << 4) | (Wire.read() >> 4);
    int32_t t_raw = ((uint32_t)Wire.read() << 12) | ((uint32_t)Wire.read() << 4) | (Wire.read() >> 4);

    int16_t rx, ry, rz;
    getRawAcc(rx, ry, rz);

    float ax = ((rx/16384.0) - offX) * 9.81;
    float ay = ((ry/16384.0) - offY) * 9.81;
    float az = ((rz/16384.0) - offZ) * 9.81;
    float acc_total = sqrt(ax*ax + ay*ay + az*az);

    // 1. VRSTICA: ZAPISOVANJE PODATKOV
    logData(String(a) + ",,");
    logData(String(getTemp(t_raw), 1) + ",");
    logData(String(lastCo2Val) + ",");
    logData(String(getPress(p_raw), 1) + ",");
    logData(String(acc_total, 2) + ",");

    // Če imamo GPS fix zapišemo podatke, drugače pustimo prazna polja (štiri vejice)
    if(hasFix) {
      logData(String(gps.speed.mps(), 1) + ",");
      logData(String(gps.altitude.meters(), 1) + ",");
      logData(String(lastLat, 5) + ",");
      logData(String(lastLng, 5));
    } else {
      logData(",,,,");
    }
    
    // Zaključimo prvo vrstico
    logDataLn(""); 
    
    // 2. VRSTICA: ZAPIS ŠTEVILA SATELITOV
    logDataLn(";;Sateliti: " + String(gps.satellites.value()));
    
    lastSend = millis();
    gpsSerial.listen();
    a += 1;
  }
}