#include <stdio.h> 
#include "pins_arduino.h" 

#define UTI_PIN 5  

// ------------- Legg inn verdier for kalibrering av temperatur ---------------//
int U_Lav_A0 = 382;  // U_L (digital verdi for spenningen på A0, registert lav temperatur)
int U_Hoy_A0 = 686;  // U_H (digital verdi for spenningen på A0, registert ved høy temperatur)
float t_Lav = 12;    // t_L (lav kalibreringstemperatur i grader C)
float t_Hoy = 41;    // t_H (høy kalibreringstemperatur i grader C)

// På bakgrunn av disse verdiene leser programmet av og beregner følgende verdier:
  
int U_A0 = 0;        // U (avlest digitalverdi for spenning på A0 ved aktuell posisjon)
float t_Beregn = 0;  // t (beregnet temperatur ved aktuell posisjon)

// -------------Legg inn verdier for kalibrering av dybde ---------------//

float U_0 = 4.00;    // U_0 (digital verdi for trykk ved overflata)
float U_1 = 12.00;   // U_1 (digital verdi for trykk ved bunnen)
float D_0 = 0.00;    // D_0 (Kalibreringsdybde ved overflata, 0,00 meter)
float D_1 = 0.70;    // D_1 (Kalibreringsdybde ved bunnen, ca. 0,7 meter)

// På bakgrunn av disse verdiene leser programmet av og beregner følgende verdier:

float U_D5     = 0;  // U (avlest digitalverdi for spenning ved aktuell posisjon)
float D_beregn = 0;  // D (beregnet dybde ved aktuell posisjon)

int i = 0;


void setup() 
{ 
  Serial.begin(9600); 
  pinMode(UTI_PIN, INPUT); 
} 

void loop() 
{ 
  double Toff, Tref, Tx; 
  i = i+1;

 //----------- Leser pulsbreddene og beregner kalibrert dybde -----------//
  
   if(ReadUTI(UTI_PIN,&Toff,&Tref,&Tx)) 
   {   
      U_D5 = 500.0 * (float)(Tx - Toff)/ (float)(Tref - Toff);

      D_beregn = ((D_1 - D_0)/(U_1 - U_0))*
                 (U_D5 - U_0) + D_0;

//------------ Leser inn spenningen for temperaturen og beregner kalibrert temperatur ---------------//
    
  U_A0 = analogRead(A0);
  t_Beregn = ((t_Hoy - t_Lav)/(U_Hoy_A0 - U_Lav_A0))*
                (U_A0 - U_Lav_A0) + t_Lav;
                 

//-------------Skriv ut for test og kalibrering --------------//

      Serial.print("Nr.: ");
      Serial.print(i);
      Serial.print(", Analogt trykk: ");
      Serial.print(U_D5, 2);
      Serial.print(", Beregnet dybde: ");
      Serial.print(D_beregn, 2);
      Serial.println(" m"); 
      Serial.print("Digital temp.: ");
      Serial.print(U_A0); 
      Serial.print(", Beregnet temp.: ");
      Serial.print(t_Beregn);
      Serial.println(" C");

//--------------- Skriv ut for datainnsamling ----------------//
   /* Serial.print(i);
      Serial.print(", ");
      Serial.print(D_beregn, 2);
      Serial.print(", ");
      Serial.print(t_Beregn, 2);
      Serial.println(",");  */
    } 
    else 
    { 
      Serial.print("Not Found"); 
    } 


  delay(5000); 
} 

int ReadUTI(uint8_t pin, double * Toff, double * Tref, double * Tx) 
{ 
    int state = HIGH; 
    int i,startindex=-1; 
    uint8_t bit = digitalPinToBitMask(pin); 
    uint8_t port = digitalPinToPort(pin); 
    uint8_t stateMask = (state ? bit : 0); 
    unsigned long width[9] = {0,0,0,0,0,0,0,0,0}; 
    double noff=0, nref=0, nx=0; 
        
    while ( (*portInputRegister(port) & bit) != stateMask); 
    
    for(i=0;i<9;i++) 
    { 
      while ( (*portInputRegister(port) & bit) == stateMask) 
          width[i]++; 
      while ( (*portInputRegister(port) & bit) != stateMask) 
          width[i]++; 
    } 
    
    for(i=0;i<9;i++) 
    { 
      if(i<6) 
      { 
      if(width[i]<width[i+2]&&width[i]<width[i+3]&&width[i+1]<width[i+2]&&width[i+1]<width[i+3]) 
        { 
          startindex=i; 
          i=9; 
          //Serial.print("Startindex: ");
          //Serial.println(startindex);
        } 
      } 
    } 
    if(startindex!=-1) { 
      *Toff=width[startindex]+width[startindex+1]; 
      *Tref=width[startindex+2]; 
      *Tx=width[startindex+3]; 
 
      return 1; 
    } 
    else 
    { 
      return 0; 
    } 
}