#include 
#include <Wire.h>
#include "LB_GPS.h"
#include  
#define pi    3.14159265358979323846
#define twopi (2*pi)
#define rad   (pi/180)
#define EarthMeanRadius     6371.01	// In km
#define AstronomicalUnit    149597890	// In km

int ledBetrieb = 1;
int pumpe = 2;
int ledGPSnok = 3;
int ledAusrichtungOk = 4;
int K1 = 5;
int K23 = 6;
int K4 = 7;
int poti = A2;

int qualitaet = 0;
boolean tag = true;
boolean nacht = false;
int nachtWinkel = 0;

int neigungsWert = 0;
boolean motorEle = false;
boolean ausrichtungEle = false;
boolean ausrichtungAzi = false;

int abweichungElevation = 0;
int abweichungAzimuth = 0;

float vorlauf = 20;
float ruecklauf = 20;
float deltaTemp = 0;

float abschaltTemperatur = 50;

int Year = 2011; //year
int Month = 2; //month
int Day = 19; //day
float Hours = 12; //hour
float Minutes = 20;//minutes
float Latitude = 46.88702; //enter latitude here
float Longitude = 7.524841; //enter longitude here
long zeit;
long datum;
double breitengrad;
double laengengrad;
boolean valid = false;
int azimuth;
	
float ZenithAngle;
float Azimuth;
float RightAscension;
float Declination;
float Parallax;
float ElevationAngle;
float ElapsedJulianDays;
float DecimalHours;
float EclipticLongitude;
float EclipticObliquity;


int HMC6352Address = 0x42;
int slaveAddress;
byte headingData[2];
int i, headingValue =  0;

/*****************************************************************************************************/
float temperatur(int ntc) { 
 double sensorValue = analogRead(ntc);
 double Temp = log(((10240000/sensorValue) - 10000));
 Temp = 1 / (0.001129148 + (0.000234125 * Temp) + (0.0000000876741 * Temp * Temp * Temp));
 Temp = Temp - 273.15;            // Convert Kelvin to Celcius
 return(Temp);
 }
 /****************************************************************************************************/



void setup() {
slaveAddress = HMC6352Address >> 1;
 Serial.begin(19200);
 Serial.println(GPS.getLibVersion());
 Serial.print("Setting up GPS...");
 delay(3000);     
 GPS.init();
 //GPS.setDate("16,07,2010");
 //GPS.setTime("21,05,00");

 Wire.begin();
 pinMode(pumpe, OUTPUT);
 //pinMode(ledBetrieb, OUTPUT);
 pinMode(pumpe, OUTPUT);
 pinMode(ledGPSnok, OUTPUT);
 pinMode(ledAusrichtungOk, OUTPUT);
 pinMode(K1, OUTPUT);
 pinMode(K23, OUTPUT);
 pinMode(K4, OUTPUT);
}

void compass () {
  Wire.beginTransmission(slaveAddress);
  Wire.send("A");            
  Wire.endTransmission();
  delay(10);                   
  Wire.requestFrom(slaveAddress, 2);      
  i = 0;
  while(Wire.available() && i < 2)
  { 
    headingData[i] = Wire.receive();
    i++;
  }
  headingValue = headingData[0]*256 + headingData[1];  // Put the MSB and LSB together
  headingValue = headingValue / 10;
  Serial.print("Azimuth ist: ");
  Serial.println(headingValue);
} 


void neigung() {
  float potiWert = analogRead(poti);
  neigungsWert = map(potiWert, 800, 880, 0, 90);
}

void sunPos(){
	float dY;
	float dX;
	// Calculate difference in days between the current Julian Day 
	// and JD 2451545.0, which is noon 1 January 2000 Universal Time
	
		float JulianDate;
		long int liAux1;
		long int liAux2;
		// Calculate time of the day in UT decimal hours
		DecimalHours = Hours + (Minutes / 60.0);
		// Calculate current Julian Day
		liAux1 =(Month-14)/12;
		liAux2=(1461*(Year + 4800 + liAux1))/4 + (367*(Month 
			- 2-12*liAux1))/12- (3*((Year + 4900 
		+ liAux1)/100))/4+Day-32075;
		JulianDate=(float)(liAux2)-0.5+DecimalHours/24.0;
		// Calculate difference between current Julian Day and JD 2451545.0 
		ElapsedJulianDays = JulianDate-2451545.0;
	
	// Calculate ecliptic coordinates (ecliptic longitude and obliquity of the 
	// ecliptic in radians but without limiting the angle to be less than 2*Pi 
	// (i.e., the result may be greater than 2*Pi)
	
		float MeanLongitude;
		float MeanAnomaly;
		float Omega;
		Omega=2.1429-0.0010394594*ElapsedJulianDays;
		MeanLongitude = 4.8950630+ 0.017202791698*ElapsedJulianDays; // Radians
		MeanAnomaly = 6.2400600+ 0.0172019699*ElapsedJulianDays;
		EclipticLongitude = MeanLongitude + 0.03341607*sin( MeanAnomaly ) 
			+ 0.00034894*sin( 2*MeanAnomaly )-0.0001134
			-0.0000203*sin(Omega);
		EclipticObliquity = 0.4090928 - 6.2140e-9*ElapsedJulianDays
			+0.0000396*cos(Omega);
	
	// Calculate celestial coordinates ( right ascension and declination ) in radians 
	// but without limiting the angle to be less than 2*Pi (i.e., the result may be 
	// greater than 2*Pi)
	
		float Sin_EclipticLongitude;
		Sin_EclipticLongitude= sin( EclipticLongitude );
		dY = cos( EclipticObliquity ) * Sin_EclipticLongitude;
		dX = cos( EclipticLongitude );
		RightAscension = atan2( dY,dX );
		if( RightAscension < 0.0 ) RightAscension = RightAscension + twopi;
		Declination = asin( sin( EclipticObliquity )*Sin_EclipticLongitude );
	
	// Calculate local coordinates ( azimuth and zenith angle ) in degrees
	
		float GreenwichMeanSiderealTime;
		float LocalMeanSiderealTime;
		float LatitudeInRadians;
		float HourAngle;
		float Cos_Latitude;
		float Sin_Latitude;
		float Cos_HourAngle;
		GreenwichMeanSiderealTime = 6.6974243242 + 
			0.0657098283*ElapsedJulianDays 
			+ DecimalHours;
		LocalMeanSiderealTime = (GreenwichMeanSiderealTime*15 
			+ Longitude)*rad;
		HourAngle = LocalMeanSiderealTime - RightAscension;
		LatitudeInRadians = Latitude*rad;
		Cos_Latitude = cos( LatitudeInRadians );
		Sin_Latitude = sin( LatitudeInRadians );
		Cos_HourAngle= cos( HourAngle );
		ZenithAngle = (acos( Cos_Latitude*Cos_HourAngle
			*cos(Declination) + sin( Declination )*Sin_Latitude));
		dY = -sin( HourAngle );
		dX = tan( Declination )*Cos_Latitude - Sin_Latitude*Cos_HourAngle;
		Azimuth = atan2( dY, dX );
		if ( Azimuth < 0.0 ) 
	        Azimuth = Azimuth + twopi;
		Azimuth = Azimuth/rad;
                azimuth = int(Azimuth);
		// Parallax Correction
		Parallax=(EarthMeanRadius/AstronomicalUnit)
			*sin(ZenithAngle);
		ZenithAngle=(ZenithAngle //Zenith angle is from the top of the visible sky (thanks breaksbassbleeps)
			+ Parallax)/rad;
                ElevationAngle = (90-ZenithAngle); //Retrieve useful elevation angle from Zenith angle
                if (ElevationAngle < nachtWinkel) {
                  nacht = true;
                  tag = false;
                }
                else {
                  nacht = false;
                  tag = true;
                }
}

void gps(){
  GPS.setSentences(GPGGA); 
  //Serial.println(GPS.getRaw(100));
  GPS.getRaw(100);
  GPS.GPSStringExplode(GPS.inBuffer,',');
  char * pEnd;
  double zeitDouble = strtod ((char*)GPS.arguments[1],&pEnd);
  zeit = long(zeitDouble);
  long Stunde = zeit / 10000;
  Hours = float(Stunde);
  long Minuten = (zeit - (Stunde*10000)) / 100;
  Minutes = float(Minuten);
   
  breitengrad = strtod ((char*)GPS.arguments[2],&pEnd);
  int ganzBreitenGrad = int(breitengrad/100);
  float teilBreitenGrad = (breitengrad - (ganzBreitenGrad*100))/60;
  breitengrad = ganzBreitenGrad + teilBreitenGrad;
  Latitude = float(breitengrad);
  
  laengengrad = strtod ((char*)GPS.arguments[4],&pEnd);
  int ganzLaengenGrad = int(laengengrad/100);
  float teilLaengenGrad = (laengengrad - (ganzLaengenGrad*100))/60;
  laengengrad = ganzLaengenGrad + teilLaengenGrad;
  Longitude = float(laengengrad);
  
  // Serial.println((char*)GPS.arguments[6]);
  qualitaet = strtod ((char*)GPS.arguments[6],&pEnd);
  if (qualitaet == 1) {
    digitalWrite(ledBetrieb, HIGH);
    digitalWrite(ledGPSnok, LOW);  
  }
  else {
    digitalWrite(ledBetrieb, LOW);
    digitalWrite(ledGPSnok, HIGH);
  }

  GPS.setSentences(GPRMC); 
  //Serial.println(GPS.getRaw(100));
  GPS.getRaw(100);
  GPS.GPSStringExplode(GPS.inBuffer,',');
  
  double datumDouble = strtod ((char*)GPS.arguments[9],&pEnd);
  datum = long(datumDouble);
  Day = datum / 10000;
  Month = (datum - (Day*10000)) / 100;
  Year = datum - (Day*10000) - (Month*100) + 2000;
}

void abweichungAzi () {
  abweichungAzimuth = azimuth - headingValue;
}

void motorAzimuth () {
  if (4 < abweichungAzimuth) {
    while (1 < abweichungAzimuth) {
      digitalWrite(K23, LOW);
      digitalWrite(K4, LOW);
      digitalWrite(K1, HIGH);
      ausrichtungAzi = false;
      statusAusrichtung();
      compass();
      abweichungAzi ();
      Serial.println("Azi zu klein");
      Serial.println(abweichungAzimuth);
      delay(10);
    }   
    digitalWrite(K1, LOW);
    digitalWrite(K4, LOW);
    digitalWrite(K23, LOW);
    ausrichtungAzi = true;
    statusAusrichtung();
    Serial.println("Azi nicht zu gross"); 
  }
 
  else if (-4 > abweichungAzimuth) {
    while (-1 > abweichungAzimuth) {
      digitalWrite(K23, HIGH);
      digitalWrite(K4, LOW);
      digitalWrite(K1, HIGH);
      ausrichtungAzi = false;
      statusAusrichtung();
      compass();
      abweichungAzi();
      Serial.println("Azi zu gross");
      Serial.println(abweichungAzimuth);
      delay(10);
    }
    digitalWrite(K1, LOW);
    digitalWrite(K4, LOW);
    digitalWrite(K23, LOW);
    ausrichtungAzi = true;
    statusAusrichtung();
    Serial.println("Azi nicht zu klein");
  }
    
  else {
    digitalWrite(K1, LOW);
    digitalWrite(K4, LOW);
    digitalWrite(K23, LOW);
    ausrichtungAzi = true;
    statusAusrichtung();
    Serial.println("Azi zentriert");
  }
  
}

void abweichungEle() {
  abweichungElevation = ElevationAngle - neigungsWert;
}

void motorElevation () {
  if (4 < abweichungElevation ) {
    while (1 < abweichungElevation ) {
      digitalWrite(K23, LOW);
      digitalWrite(K4, HIGH);
      digitalWrite(K1, HIGH);
      motorEle = false;
      ausrichtungEle = false;
      statusAusrichtung();
      neigung();
      abweichungEle();
      Serial.println("Ele zu klein");
      Serial.println(abweichungElevation); 
    }  
    digitalWrite(K1, LOW);
    digitalWrite(K4, LOW);
    digitalWrite(K23, LOW);
    motorEle = true;
    ausrichtungEle = true;
    statusAusrichtung();
    Serial.println("Ele nicht zu gross");
  }  
 
  else if (-4 > abweichungElevation) {
    while (-1 > abweichungElevation) {
      digitalWrite(K23, HIGH);
      digitalWrite(K4, HIGH);
      digitalWrite(K1, HIGH);
      motorEle = false;
      ausrichtungEle = false;
      statusAusrichtung();
      neigung();
      abweichungEle();
      Serial.println("Ele zu gross");
      Serial.println(abweichungElevation);
    }
    digitalWrite(K1, LOW);
    digitalWrite(K23, LOW);
    digitalWrite(K4, LOW);
    motorEle = true;
    ausrichtungEle = true;
    statusAusrichtung();
    Serial.println("Ele nicht zu klein");
  }
  
  else {
    digitalWrite(K1, LOW);
    digitalWrite(K23, LOW);
    digitalWrite(K4, LOW);
    motorEle = true;
    ausrichtungEle = true;
    statusAusrichtung();
    Serial.println("Ele nicht zu klein");
  }
}

void statusAusrichtung() {
  if ((ausrichtungEle && ausrichtungAzi)) {
    digitalWrite(ledAusrichtungOk, HIGH);
  }
  else {
    digitalWrite(ledAusrichtungOk, LOW);
  }
}

void notStellung () {
  while ((vorlauf > abschaltTemperatur) || (ruecklauf > abschaltTemperatur) || (Latitude == 0) || (Longitude == 0) || (qualitaet == 0) || (nacht)) {
    digitalWrite(ledAusrichtungOk, LOW);
    Serial.println("Notstellung aktiv");
    // Motor Azimuth auf 180 Grad, Motor Elevation auf 85 Grad stellen
    pumpenBetrieb();
    gps();
    sunPos();
    if ((nacht)) {
      Serial.println("Nacht");
      Serial.println(ElevationAngle);
    }
    if (qualitaet == 0) {
      Serial.println("kein GPS");
    }
    if (Latitude == 0) {
      Serial.println("Latitude = 0");
    }
    if (Longitude == 0) {
      Serial.println("Longitude = 0");
    }
    
    ElevationAngle = 85;
    neigung();
    abweichungEle();
    statusAusrichtung();
    motorElevation();
    
    azimuth = 180;
    compass();
    abweichungAzi();
    statusAusrichtung();
    if (motorEle) {
      motorAzimuth();
    }
    pumpenBetrieb();    
  }
}


void pumpenBetrieb() {
  vorlauf = temperatur(0);
  ruecklauf = temperatur(1);
  if ((vorlauf > abschaltTemperatur) || (ruecklauf > abschaltTemperatur)) {
    notStellung();
  }
  deltaTemp = ruecklauf - vorlauf;
  
  if (deltaTemp > 10) {
    digitalWrite(pumpe, HIGH);
    Serial.println("Pumpe: ein");
  }
  else {
   digitalWrite(pumpe, LOW);
   Serial.println("Pumpe: aus");
  }
}
  

void loop(){
  gps();
  sunPos();
  Serial.println(qualitaet);
  if (((Latitude != 0) && (Longitude != 0) && (qualitaet == 1) && (tag))) {
  neigung();
  abweichungEle();
  motorElevation();
  
  compass();
  abweichungAzi();
  statusAusrichtung();
  if (motorEle) {
    motorAzimuth();
  }
  pumpenBetrieb();
  }
  else {
    notStellung();
  }
  
  Serial.print("Breitengrad: ");
  Serial.println(Latitude);
  Serial.print("Laengengrad: ");
  Serial.println(Longitude);
  
  Serial.print("Azimuth soll:  ");
  Serial.println(Azimuth);
  Serial.print("Azimuth ist:  ");
  Serial.println(headingValue);
  
  Serial.print("Elevation soll:  ");
  Serial.println(ElevationAngle); 
  Serial.print("Elevation ist:  ");
  Serial.println(neigungsWert);
  
  Serial.print("Vorlauf = ");
  Serial.println(vorlauf);
   Serial.print("Ruecklauf = ");
  Serial.println(ruecklauf);
  
  delay(1000);
}