#include "navigator.h"
#include <math.h>
#include <avr/io.h>
#include "i2c.h"

i2c twi;

navigator::navigator()
{
    //UART1 ist Schnittstelle zum GPS
    UBRR1 = 103;         //9600 baud
    UCSR1B = 144;        //RX interrupt enable und rx enable
    UCSR1C = 6 | 128;    //bit 2(=4) und 1(=2) : 8 datenbit,
    kompasswinkel = 0;
}

void navigator::lies_kompass()
{
//PORTG = PORTG | 1;
    twi.start();            //start
    twi.write(192);         //moduladresse (default 0xc0)
    twi.write(1);           //register 1 bitte (winkel 0..255)
    twi.start();
    twi.write(193);         //read-modus
    kompasswinkel = twi.read_end();
    twi.stop();
//PORTG = PORTG & 254;
}

void navigator::resetkompass()
{
    switch (twi_state)
    {
    case 1:
        {
            twi.start();
            twi.write(192);
            twi.write(0);
            twi.write(0x20);
            twi.stop();
            twi_state=2;
            break;
        }
    case 2:
        {
            twi.start();
            twi.write(192);
            twi.write(0);
            twi.write(0x2A);
            twi.stop();
            twi_state=3;
            break;
        }
    case 3:
        {
            twi.start();
            twi.write(192);
            twi.write(0);
            twi.write(0x60);
            twi.stop();
            twi_state=4;
            break;
        }
    }
}

void navigator::request_position()
{
    LatB_minutes = (LatBstring[1]-48)*10;                       //Vorsicht, das sind ASCCI, nicht Zahlen !
    LatB_minutes = LatB_minutes + (LatBstring[2]-48);
    LatB_minutes = LatB_minutes + (LatBstring[4]-48)*0.1;
    LatB_minutes = LatB_minutes + (LatBstring[5]-48)*0.01;
    LatB_minutes = LatB_minutes + (LatBstring[6]-48)*0.001;
    LatB_minutes = LatB_minutes + (LatBstring[7]-48)*0.0001;

    LonL_minutes = (LonLstring[1]-48)*10;
    LonL_minutes = LonL_minutes + (LonLstring[2]-48);
    LonL_minutes = LonL_minutes + (LonLstring[4]-48)*0.1;
    LonL_minutes = LonL_minutes + (LonLstring[5]-48)*0.01;
    LonL_minutes = LonL_minutes + (LonLstring[6]-48)*0.001;
    LonL_minutes = LonL_minutes + (LonLstring[7]-48)*0.0001;

    LatB_meters = LatB_minutes * 1852.16;
    LonL_meters = LonL_minutes * 1249.06;
 }

void navigator::testmeldung()
{
    for (testpos=1; testpos<8; testpos ++)
    {
        while (!(UCSR0A & (1<<UDRE0)))  //warte bis register bereit
        {
        }
        UDR0 = LatBstring[testpos];              //raus damit !
    }
    while (!(UCSR0A & (1<<UDRE0)))  //warte bis register bereit
    {
    }
    UDR0 = 45;              //raus damit !
    for (testpos=1; testpos<8; testpos ++)
    {
        while (!(UCSR0A & (1<<UDRE0)))  //warte bis register bereit
        {
        }
        UDR0 = LonLstring[testpos];              //raus damit !
    }
    while (!(UCSR0A & (1<<UDRE0)))  //warte bis register bereit
    {
    }
    UDR0 = 10;              //raus damit !
}

void navigator::zielkurs()
{
    request_position();

    //---------------------------------------------------
    //Testziel : Faltergatter (IST) -> Petermichl (Soll = home)
    LatBhome_meters = 86337.0916;   //Koordinaten(m) für Perermichl
    LonLhome_meters = 24401.3383;
    //
    LatB_meters = 86210.6;
    LonL_meters = 24690.6067;
    //---------------------------------------------------

    d_LatB = LatBhome_meters - LatB_meters;
    d_LonL = LonLhome_meters - LonL_meters;

    if (d_LatB*d_LatB < 1)                  //damit die Mathematik keine Probleme kriegt
    {
        d_LatB = 1;
    }
    if (d_LonL*d_LonL < 1)
    {
        d_LonL = 1;
    }

    if (d_LatB>0 and d_LonL>0)          //1. Quadrant -> home
    {
        Kurswinkel_home = 90 - (atan(d_LatB/d_LonL)* 180 / 3.14);
    }
    if (d_LatB<0 and d_LonL>0)          //2. Quadrant -> home
    {
        Kurswinkel_home = 90 - (atan(d_LatB/d_LonL)* 180 / 3.14);
    }
    if (d_LatB<0 and d_LonL<0)          //3. Quadrant -> home
    {
        Kurswinkel_home = 270 - (atan(d_LatB/d_LonL)* 180 / 3.14);
    }
    if (d_LatB>0 and d_LonL<0)          //4. Quadrant -> home
    {
        Kurswinkel_home = 270 - (atan(d_LatB/d_LonL)* 180 / 3.14);
    }
}

void navigator::regeln()                //theorie : http://portal.ts-muenchen.de/Dateien/Digitalregler.pdf
{
    lies_kompass();
    kompassfloat = float(kompasswinkel) * 360 / 255;
    istwert = kompassfloat;
    sollwert = Kurswinkel_home;
    kp = 1;                             //Nur Testwerte
    ki = 0;
    kd = 0;

    if (sollwert > 180)                 //superfauler trick um 360/0 zu umgehen
    {
        sollwert = -(360-sollwert);
    }
    if (istwert > 180)
    {
        istwert = -(360-istwert);
    }
    regeldifferenz = sollwert - istwert;                //regeldifferenz soll - ist
    if (regeldifferenz > 180)
    {
        regeldifferenz = regeldifferenz - 360;
    }
    if (regeldifferenz < -180)
    {
        regeldifferenz = 360 + regeldifferenz;
    }
    differenz = regeldifferenz - regeldifferenz_alt;    //differenzieren für den d-teil
    regeldifferenz_alt = regeldifferenz;                //alten wert speichern fürs differenzieren
    integral = integral + regeldifferenz;               //integrieren
    if (integral > 100)                                 //das sollte man begrenzen, 50 ist "über den daumen"
    {
        integral = 100;
    }
    if (integral < -100)
    {
        integral = -100;
    }
    p = regeldifferenz * kp;                            //p-teil
    d = differenz * kd;                                 //d-teil
    i = integral * ki;                                  //i-teil

    stellwert = p + i + d;          //das ist ein float, betrag < 180
    stellwert = stellwert / 3.6;    //jetzt betrag < 50, also -50 ... +50
    ruderwinkel = stellwert + 50;     //jetzt 0..100, mit 50 als Mittelstellung des Ruders

};



