#include <Wire.h>               //komunikace po I2C
#include <Stepper.h>            //krokove motory
#include <HMC5883L.h>           //magnetometr
#include <SPI.h>                //komunikace po SPI
#include <SD.h>                 //ctecka SD karet
#include <LiquidCrystal.h>      //LCD
#include <MPU6050.h>            //akcelerometr
#include <KalmanFilter.h>       //Kalmanuv filtr
#define DS3231_I2C_ADDRESS 0x68 //nastaveni adresy casoveho modulu
#define CS_PIN 2                //nastaveni datoveho pinu ctecky

int xPin = A0;      //osa x joysticku
int yPin = A1;      //osa y joysticku
int hpPin = A2;     //fotorezistor HP
int hlPin = A3;     //fotorezistor HL
int dlPin = A4;     //fotorezistor DL
int dpPin = A5;     //fotorezistor DP
int pinCas = 23;    //prepinac rezimu v poloze casovace
int pinMan = 25;    //prepinac rezimu v poloze manualni
int pinKal = 27;    //spinac kalibrace
int PinX0 = 3;      //piny motoru nataceni (osa x)
int PinX1 = 4;
int PinX2 = 5;
int PinX3 = 6;
int PinY0 = 7;      //piny motoru naklonu (osa y)
int PinY1 = 8;
int PinY2 = 9;
int PinY3 = 10;
int trimr_u = A6;   //trimr tolerance uhlu
int trimr_f = A7;   //trimr tolerance fotorezistoru

const byte zaznamu = 200;         //zaznamy na karte pro jeden den
float necitlivost_f = 0.08 * 256; //tolerance fotorezistoru
float necitlivost_u = 5;          //tolerance uhlu

//nastaveni pinu pro displej
LiquidCrystal lcd(39, 41, 43, 45, 47, 49);

//nastaveni motoru
Stepper motorX(200, PinX0, PinX2, PinX1, PinX3);
Stepper motorY(200, PinY0, PinY2, PinY1, PinY3);

//nastaveni snimacu polohy a Kalmanova filtru
HMC5883L hmc;
MPU6050 mpu;
KalmanFilter kalmanX(0.001, 0.003, 0.03);
KalmanFilter kalmanY(0.001, 0.003, 0.03);

float akc_naklon = 0;   //naklon dle akcelerometru
float azimut = 0;       //skutecny azimut
float zenit = 0;        //skutecny zenit
float azimut_n = 0;     //pozadovany azimut
float zenit_n = 0;      //pozadovany zenit
int xPozice = 0;        //vychylka joysticku v ose x
int yPozice = 0;        //vychylka joysticku v ose y
byte rezim = 1;   //rezim: 1 = automaticky, 2 = casovac, 3 = manualni
int hp = 0;             //hodnota fotorezistoru HP
int hl = 0;             //hodnota fotorezistoru HL
int dp = 0;             //hodnota fotorezistoru DP
int dl = 0;             //hodnota fotorezistoru DL
int nahore = 0;         //osvetleni horni casti
int dole = 0;           //osvetleni dolni casti
int vlevo = 0;          //osvetleni leve casti
int vpravo = 0;         //osvetleni prave casti
int rozdilX = 0;        //rozdil osvetleni v ose x
int rozdilY = 0;        //rozdil osvetleni v ose y
int i = 0;              //pomocna promenna
int j = -1;             //pomocna promenna

//promenne casu
byte second, minute, hour, dayOfWeek, dayOfMonth, month, year;
int cas_h, cas_m, den_m;
float pole[zaznamu][4]; //pole dat z SD karty
byte cislo = 0;         //pomocna promenna
byte radek = 0;         //pomocna promenna
char soubor[9] = "";    //nazev souboru na karte
File file;              //ukazatel souboru
size_t n;               //pomocna promenna
char str[20];           //pomocna promenna

//prace se soubory
size_t readField(File* file, char* str, size_t size, char* delim) {
  char ch;
  size_t n = 0;
  while ((n + 1) < size && file->read(&ch, 1) == 1) {
    if (ch == '\r') {
      continue;
    }
    str[n++] = ch;
    if (strchr(delim, ch)) {
        break;
    }
  }
  str[n] = '\0';
  return n;
}

//prevod mezi soustavami
byte decToBcd(byte val){ return( (val/10*16) + (val%10) ); }
byte bcdToDec(byte val){ return( (val/16*10) + (val%16) ); }

//nastaveni casu
void setDS3231time(byte second, byte minute, byte hour, byte dayOfWeek, byte dayOfMonth, byte month, byte year){
    Wire.beginTransmission(DS3231_I2C_ADDRESS); //navazani spojeni
    Wire.write(0);            //nastaveni ukazatele na registr sekund
    Wire.write(decToBcd(second));               //nastaveni sekund
    Wire.write(decToBcd(minute));               //nastaveni minut
    Wire.write(decToBcd(hour));                 //nastaveni hodin
    Wire.write(decToBcd(dayOfWeek));            //nastaveni dne v tydnu
    Wire.write(decToBcd(dayOfMonth));           //nastaveni data
    Wire.write(decToBcd(month));                //nastaveni mesice
    Wire.write(decToBcd(year));                 //nastaveni roku
    Wire.endTransmission();                     //konec spojeni
}

//ziskani casu
void readDS3231time(byte *second, byte *minute, byte *hour, byte *dayOfWeek, byte *dayOfMonth, byte *month, byte *year){
    Wire.beginTransmission(DS3231_I2C_ADDRESS); //navazani spojeni
    Wire.write(0);            //nastaveni ukazatele na registr sekund
    Wire.endTransmission();                  //konec spojeni
    Wire.requestFrom(DS3231_I2C_ADDRESS, 7); //pozadavek na 7 bajtu dat
    *second = bcdToDec(Wire.read() & 0x7f);
    *minute = bcdToDec(Wire.read());
    *hour = bcdToDec(Wire.read() & 0x3f);
    *dayOfWeek = bcdToDec(Wire.read());
    *dayOfMonth = bcdToDec(Wire.read());
    *month = bcdToDec(Wire.read());
    *year = bcdToDec(Wire.read());
}

void setup() {
  Wire.begin();           //komunikace po I2C
  Serial.begin(115200);   //komunikace po seriove lince
  
  //nehraje roli, ale musi byt zavolano
  motorX.setSpeed(30);
  motorY.setSpeed(30);
  
  //nastaveni rezimu pinu
  pinMode(xPin, INPUT);
  pinMode(yPin, INPUT);
  pinMode(pinCas, INPUT_PULLUP);
  pinMode(pinMan, INPUT_PULLUP);
  pinMode(hpPin, INPUT);
  pinMode(hlPin, INPUT);
  pinMode(dpPin, INPUT);
  pinMode(dlPin, INPUT);
  pinMode(trimr_u, INPUT);
  pinMode(trimr_f, INPUT);

  //nastaveni pracovniho rezimu
  if (digitalRead(pinCas) == LOW){
    rezim = 2;
    Serial.println("Rezim casovace");     //vypis na seriovou linku
  }else if (digitalRead(pinMan) == LOW){
    rezim = 3;
    Serial.println("Manualni rezim");
  }else{
    rezim = 1;
    Serial.println("Automaticky rezim");
  }

  //nastaveni casu (seconds, minutes, hours, day, date, month, year)
  //setDS3231time(0,37,16,4,26,04,17);

  Serial.println("Nastaveni displeje");
  lcd.begin(16, 2);
  
  Serial.println("Spousteni kompasu");
  while (!hmc.begin()){
    Serial.println("Kompas nenalezen!");
    lcd.clear();              //vyprazdneni displeje
    lcd.setCursor(0,0);       //nastaveni kurzoru
    lcd.print("Kompas");      //vypis na displej
    lcd.setCursor(0,1);       //presun kurzoru
    lcd.print("nenalezen!");
    delay(2000);              //dvousekundova pauza
  }
  Serial.println("Spousteni akcelerometru");
  while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)){
    Serial.println("Akcelerometr nenalezen!");
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Akcelerometr");
    lcd.setCursor(0,1);
    lcd.print("nenalezen!");
    delay(2000);
  }
  
  hmc.setRange(HMC5883L_RANGE_1_3GA);         //nastaveni rozsahu
  hmc.setMeasurementMode(HMC5883L_CONTINOUS); //nastaveni rezimu
  hmc.setDataRate(HMC5883L_DATARATE_15HZ);    //frekvence mereni
  hmc.setSamples(HMC5883L_SAMPLES_4);       //pocet vzorku pro vypocet

  //proces kalibrace
  if (digitalRead(pinKal) == LOW){
    hmc.setOffset(0, 0);    //magnetometr
    mpu.calibrateGyro();    //akcelerometr
  }

  if (rezim == 2){
    Serial.println("Pripojovani karty");
    while (!SD.begin(CS_PIN)){
      Serial.println("Kartu se nepodarilo pripojit.");
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Kartu nelze");
      lcd.setCursor(0,1);
      lcd.print("pripojit!");
      delay(2000);
    }
    readDS3231time(&second, &minute, &hour, &dayOfWeek, &dayOfMonth, &month, &year);                   //ziskani data a casu
    sprintf(soubor, "%02d-%02d.txt", month, dayOfMonth);
    file = SD.open(soubor);           //otevreni souboru pro dany den
    Serial.println("Otevirani souboru dat");
    while (!file){
      Serial.println("Soubor nelze otevrit.");
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Soubor nelze");
      lcd.setCursor(0,1);
      lcd.print("otevrit!");
      delay(2000);
    }
    file.seek(0);               //nastaveni bajtu v souboru
    while (true) {              //cteni carkami oddelenych hodnot
      n = readField(&file, str, sizeof(str), ",\n");
      if (n == 0) break;                    //konec souboru
      if (str[n-1] == ',' || str[n-1] == '\n') {
      //i = sloupcovy index, j = radkovy index
          if (i == 0) j++;                  //novy radek
          if (i == 3) i = -1;
          i = i + 1;
        str[n-1] = 0;                       //odstraneni carky
      } else{
        if (file.available()){
          Serial.print("Chyba!");
        }else{                              //konec souboru
          i = 0;
        }
      }
      float cislo = atof(str);              //prevod na cislo
      pole[j][i == 0 ? 3 : i - 1] = cislo;  //zapis do pole hodnot
    }
    file.close();                           //uzavreni souboru
    cas_h = 100 * hour + minute;            //aktualni cas
    cas_m = minute;
    den_m = dayOfMonth;
    for (i = 0; i < zaznamu; i++){          //prochazeni pole zaznamu
      if (100 * pole[i][0] + pole[i][1] <= cas_h){
        radek = i;                //vyhledani radku dle aktualniho casu
      }else{
        break;
      }
    }
    azimut_n = pole[radek][3];    //nastaveni pozadovaneho azimutu
    zenit_n = pole[radek][2];     //nastaveni pozadovaneho zenitu
  }
  i = 0;                          //reset pomocne promenne
  if (rezim == 3){          //manualni rezim
    lcd.clear();
    lcd.setCursor(4,0);
    lcd.print("MANUALNI");
    lcd.setCursor(5,1);
    lcd.print("REZIM!");
  }
}

void loop() {
  //cteni normalizovanych hodnot z akcelerometru
  Vector norm = hmc.readNormalize();
  Vector acc = mpu.readNormalizeAccel();
  Vector gyr = mpu.readNormalizeGyro();
  
  //cteni hodnot z joysticku
  xPozice = analogRead(xPin);
  yPozice = 1023 - analogRead(yPin);

  //vypocet uhlu (rad)
  azimut = atan2(norm.YAxis, norm.XAxis);
  akc_naklon = -(atan2(acc.XAxis, sqrt(acc.YAxis*acc.YAxis + acc.ZAxis*acc.ZAxis))*180.0)/PI;
  zenit = -kalmanY.update(akc_naklon, gyr.YAxis);
  
  //deklinace: (deg + (min / 60)) / (180 / PI);
  float deklinace = (4 + (32 / 60)) / (180 / PI);
  azimut += deklinace;
  
  //korekce uhlu
  if (azimut < 0){azimut += 2 * PI;}
  if (azimut > 2 * PI){azimut -= 2 * PI;}
  azimut = azimut * 180/PI;

  //ziskani hodnot z fotorezistoru a vypocet prumernych hodnot
  hp = analogRead(hpPin);
  hl = analogRead(hlPin);
  dl = analogRead(dlPin);
  dp = analogRead(dpPin);
  nahore = (hp + hl) / 2;
  dole = (dp + dl) / 2;
  vlevo = (dl + hl) / 2;
  vpravo = (hp + dp) / 2;
  rozdilX = vlevo - vpravo;
  rozdilY = nahore - dole;
  
  //nacteni hodnot trimru
  necitlivost_f = map(analogRead(trimr_f), 0, 1023, 1, 256);
  necitlivost_u = map(analogRead(trimr_u), 0, 1023, 1, 360);

  switch(rezim){
    case 1: //automaticky
      if (i >= 300){      //aktualizace displeje kazdou tristou iteraci
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print(hl);        //udaje z hornich fotorezistoru
        lcd.print("/");
        lcd.print(hp);
        lcd.setCursor(9,0);
        lcd.print("(~");
        lcd.print(necitlivost_f, 0);  //aktualni nastaveni tolerance
        lcd.print(")");
        lcd.setCursor(0,1);
        lcd.print(dl);        //udaje ze spodnich fotorezistoru
        lcd.print("/");
        lcd.print(dp);
        lcd.setCursor(9,1);
        lcd.print("(~");
        lcd.print(necitlivost_f, 0);  //opet tolerance
        lcd.print(")");
        i = 0;                        //restart pocitadla iteraci
      }
      if (rozdilX > necitlivost_f){   //kontrola proti toleranci
        motorX.step(-1);              //otoceni motoru
        delay(1);
      }else if (rozdilX < -necitlivost_f){
        motorX.step(1);              //opacny smer
        delay(1);
      }
      if (rozdilY > necitlivost_f){
        motorY.step(-1);
        delay(1);
      }else if (rozdilY < -necitlivost_f){
        motorY.step(1);
        delay(1);
      }
    break;
    case 2: //casovac
      readDS3231time(&second, &minute, &hour, &dayOfWeek, &dayOfMonth, &month, &year);      //aktualni cas
      if (den_m != dayOfMonth){       //po prechodu na novy den
        asm volatile ("  jmp 0");     //restart Arduina
      }
      if (minute != cas_m){           //kontrola kazdou minutu
        cas_m = minute;
        cas_h = 100 * hour + minute;
        if (pole[radek + 1][0] != 0){   //kontrola pole dat
          if (100 * pole[radek + 1][0] + pole[radek + 1][1] <= cas_h){
            radek++;
            azimut_n = pole[radek][3];  //nastaveni pozadovanych hodnot
            zenit_n = pole[radek][2];
          }
        }
      }
      if (i >= 300){                    //kazda trista iterace
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print(azimut, 1);           //vypis skutecneho azimutu
        lcd.print("/");
        lcd.print(zenit, 1);            //vypis skutecneho zenitu
        lcd.setCursor(12,0);
        lcd.print("~");
        lcd.print(necitlivost_u, 0);    //tolerance uhlu
        lcd.setCursor(0,1);
        lcd.print("~");
        lcd.print(azimut_n);            //pozadovany azimut
        lcd.print("/");
        lcd.print(zenit_n);             //pozadovany zenit
        i = 0;
      }
      if (azimut_n > azimut + necitlivost_u){
        motorX.step(-1);
        delay(1);
      }else if (azimut_n < azimut - necitlivost_u){
        motorX.step(1);
        delay(1);
      }
      if (zenit_n > zenit + necitlivost_u){
        motorY.step(1);
        delay(1);
      }else if (zenit_n < zenit - necitlivost_u){
        motorY.step(-1);
        delay(1);
      }
    break;
    case 3: //manualni
      if (xPozice < 300){         //kontrola vychylky joysticku
        motorX.step(-1);
        delay(1);
      }else if (xPozice > 700){
        motorX.step(1);
        delay(1);
      }
      if (yPozice < 300){
        motorY.step(1);
        delay(1);
      }else if (yPozice > 700){
        motorY.step(-1);
        delay(1);
      }
    break;
  }
  i++;                            //pripocitani iterace
}