/*
% Název BP: 
% CS: Analýza zakřivení páteře s využitím moderních senzorů
% EN: Back-bone deformation analysis using modern sensors

% Kód pro funkci zařízení, získání dat, předzpracování a záslání popř. uložení na SD kartu
% Autor: Vojtěch Fiala
% Datum: 26. 4. 2018

-----------------------------------------------------------------------------------------------------------------
Tento kód vznikl vhodným sestavením a odladěním výše zmíněným autorem. Původní kódy, ze kterých bylo čerpáno, jsou
uvedeny ve zdrojích v rámci textu k BP, k němuž je přiložen i tento program. Tento kód byl sestaven pro danou práci
a zařízení, které bylo v rámci ní sestrojeno.
*/


#include "Wire.h"

extern "C" { 
    #include "utility/twi.h"  // from Wire library, so we can do bus scanning
}

#include "SD.h"
#include "MPU6050.h"
//#include "MPU60501.h"
#include "Math.h"
#include "I2Cdev.h"


#define TCAADDR 0x70
#define MPUADDR 0x68

#include "SoftwareSerial.h"
//-------------------------------------
//RX аnd TX for Bluetooth board
SoftwareSerial BTS(0, 1); //RX, TX
//-------------------------------------

//---------------------------------------------
// PŘEDZPRACOVÁNÍ DAT !!!!!
// Zadejte hondnotu pro sumaci naměřených dat 
// (z kolika vzorků bude výsledná hodnota pro záznam)
const int pocet_vzorku = 60;
//---------------------------------------------

const float pi = 3.141592;
// pojmenování a inicializování proměných
int16_t ax, ay, az;
float x, y, z;
int  pocet[3]={0,0,0};
int i=0;
float _angle_x, angle_x, _angle_y, angle_y;
float ax_p[3]={0,0,0}, ay_p[3]={0,0,0}, az_p[3]={0,0,0};

MPU6050 mpu;
File myFile;

//=========================
// FOUND IMU SENSORS
//=========================
bool foundIMU[8] = {false};
unsigned long timer = 0;

void(* resetFunc) (void) = 0;

void tcaselect(uint8_t i) {
    if (i > 7) 
        return;

    Wire.beginTransmission(TCAADDR);
    Wire.write(1 << i);
    Wire.endTransmission();  
}


//==========================
// Standard Arduino setup()
//==========================
void setup()
{
int i=0;
  BTS.begin(38400);
    //while (!Serial);

    Wire.begin();
    Serial.println("\nTCAScanner ready!");
    Serial.begin(115200);
    Serial.println("\nTCAScanner ready!");
    //IdentifyIMU_Addresses();
   

    // Looking for multiplexer´s addresses for gyroscopes
    for (uint8_t t=0; t<8; t++) {
        tcaselect(t);
        Serial.print("TCA Port #"); Serial.println(t);

        for (uint8_t addr = 0; addr<=127; addr++) {
            if (addr == TCAADDR) continue;

            uint8_t data;
            if (! twi_writeTo(addr, &data, 0, 1, 1)) {
                Serial.print("Found I2C 0x");  Serial.println(addr,HEX);
                
                //Initialize MPU
                InitMPU(t);
                foundIMU[t] = true;
            }
           // else
           // {
              //Set the IMU Address as disabled
           //   foundIMU[t]=false;
            
          //  }
        }
    }
    setupsd();
    Serial.println("\ndone");
}


//======================
// Setup for SD card
//======================
void setupsd() {
  // Open serial communications and wait for port to open:
  Serial.begin(9600);
  /*while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }*/

  Serial.println("Zacatek inicializace a mereni...");
  Serial.println("Initializing SD card...");

  if (!SD.begin(PB0)) {//8 
    Serial.println("Initialization failed!");
    return;
  }
  Serial.println("Initialization SD done.");
  delay(1000);
  }

//=================================================
// Initialize The gyroscopes on the multiplexer
//=================================================
void InitMPU(uint8_t gyroID)
{
    tcaselect(gyroID);


    Serial.print("Initialize MPU6050: ");
    Serial.println(gyroID);

    mpu.initialize();
    // Při chybě nevypíšeme hlášku
    if (mpu.testConnection());
    Serial.println("Spojeni OK...");  
}

//====================================================
// Looking for multiplexer´s addresses for gyroscopes
//====================================================
void IdentifyIMU_Addresses()
{
    for (uint8_t t=0; t<8; t++) {
        tcaselect(t);
        Serial.print("TCA Port #"); Serial.println(t);

        for (uint8_t addr = 0; addr<=127; addr++) {
            if (addr == TCAADDR) continue;

            uint8_t data;
            if (! twi_writeTo(addr, &data, 0, 1, 1)) {
                Serial.print("Found I2C 0x");  Serial.println(addr,HEX);
            }
        }
    }
    Serial.println("\ndone");
}

// Inicializace proměnných pro snímané hodnoty
int16_t pitch, roll, yaw;

//===================================
// Displays the gyro data per gyro
//===================================
void DisplayGyroData(uint8_t gyroID)
{
    tcaselect(gyroID);

      // zjistí všechny hodnoty z akcelerometru
     mpu.getAcceleration(&roll, &pitch, &yaw);
}


//=================================================================
// Funkce pro sumaci, kalibraci a přepočet akcelerometrických dat na úhly
//=================================================================
void endgyro(uint8_t gyroID)
{
   float m=0,m1=0,m2=0;
   float n=0,n1=0,n2=0;
    ax_p[gyroID] = ax_p[gyroID] + roll;//ax
    ay_p[gyroID] = ay_p[gyroID] + pitch;//ay
    az_p[gyroID] = az_p[gyroID] + yaw;//az
    // pomocné určení počru vzorků
    pocet[gyroID]++;
    // když se dosáhne určeného počtu vzorků  
    if (pocet[gyroID] == pocet_vzorku)
    {
     //zjistíme průmerné hodnoty
     x = ax_p[gyroID]/pocet_vzorku;
     y = ay_p[gyroID]/pocet_vzorku;
     z = az_p[gyroID]/pocet_vzorku;
     // vypočteme sklon a náklon
     angle_x = atan2(x, sqrt(square(y) + square(z)))/(pi/180);
     angle_y = atan2(y, sqrt(square(x) + square(z)))/(pi/180);

     //                        !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
     // Automatická kalibrace - nutnost být při prvním záznamu ve správné poloze, kterou chceme měřit 
     // Pochyby ohledně funkčnosti, protože získaná data se po kalibraci nepohybují kolem 0
     if (i<3)
     {
        if (gyroID == 0)
          {
              m = angle_x;
              n = angle_y;
              i=i+1;
          }
        else if (gyroID == 1)
          {
              m1 = angle_x;
              n1 = angle_y;
              i=i+1;
          }
        else if (gyroID == 2)
          {
              m2 = angle_x;
              n2 = angle_y;
              i=i+1;
          }
     }
     if (gyroID == 0)
     {
          angle_x = angle_x - m;
          angle_y = angle_y - n;
     }
     else if (gyroID == 1)
     {
          angle_x = angle_x - m1;
          angle_y = angle_y - n1;
     }
     else if (gyroID == 2)
     {
          angle_x = angle_x - m2;
          angle_y = angle_y - n2;
     }
     
     // vynulujeme hodnoty pro další použití    
     pocet[gyroID] = 0;
     ax_p[gyroID] = 0;
     ay_p[gyroID] = 0;
     az_p[gyroID] = 0;
     
    // vypíšeme výsledky na seriový port      
    Serial.println(gyroID+1);
    Serial.print(angle_x);
    Serial.print("\t"); // \t = tabulátor
    Serial.println(angle_y);

    // Zápis na SD kartu
    zapis(gyroID);

    // Vypsání výsledků na komunikační port pro Bluetooth
    BTS.print(String(gyroID+1));
    BTS.print("sen:");
    BTS.print(String(angle_x));
    BTS.print(":");
    BTS.println(String(angle_y));
    zapis(gyroID);
    }
}

//========================================
// Zapsání získaných dat na SD kartu
//========================================
void zapis(uint8_t gyroID)
{
  // open the file. note that only one file can be open at a time,
  // so you have to close this one before opening another.
  myFile = SD.open("data.txt", FILE_WRITE);

  // if the file opened okay, write to it:
  if (myFile) {
    Serial.println("Zapis na SD");
    myFile.print(gyroID+1);
    myFile.print(";"); myFile.print(angle_x);
    myFile.print(";"); myFile.println(angle_y);
    // close the file:
    myFile.close();
    Serial.println("Hotovo.");
  } else {
    // if the file didn't open, print an error:
    Serial.println("chyba zapisu na SD");
  }
}

//============================================================
// Hlavní programová smyčka
//============================================================
void loop() 
{
    for (int i = 0; i < 8;  i++)
    {
        if (foundIMU[i] == true)
        {
            DisplayGyroData(i);
            switch(i)
            {
              case 0: {endgyro(2);break;} // Bederní senzor (3)
              case 6: {endgyro(1);break;} // Hrudní senzor (2)
              case 7: {endgyro(0);break;} // Krční senzor (1)
            }
        }
    }

 delay(5);
}
