#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <reg167.h>
#include "can_ext.h"
#include "global.h"

void main (void)
{

 /* Interrupts zulassen */
 //IEN = 1;

 /* Watchdog-Timer wurde bereits in cstart initialisiert (watchdog time = 419 ms); hier erstmals bedienen */
 //WDTsrv();

 /* Voreinstellungen */
 /* ================ */

 /* CAN-Controller */
 C167.CAN_fehler = FALSE;
 C167.CAN_rdy    = FALSE;

 /* Taktzeitueberschreitung */
 C167.zeitfehler = FALSE;

 /* Egofahrzeug-Daten */
 //EgoFzg.v = 0;
 //Regler.einschalten = FALSE;


 /* Initialisierung der Timer, Interupts, etc. */
 /* ========================================== */

 /* Initialisieren des kurzen Haupttimers : Timer 3 ist ein 16bit-Timer mit einer Zeitaufloesung von 6.4 us */
 // Gesamtdurchlaufzeit: 419 ms; raufzählend
 T3     = 0;      /* Timer 3 auf Null setzen */
 T3CON  = 0x44;      // binaer : 00 00 1 000 100; 0x44

 /* Watchdog-Timer wurde bereits in cstart initialisiert (watchdog time = 419 ms); hier nochmals bedienen */
 WDTsrv();

 /* Initialisierung CAN1 und CAN2 mit 500 kBit/s */
 StartCAN(0);
 //StartCAN(1);

 /* Watchdog-Timer wurde bereits in cstart initialisiert (watchdog time = 419 ms); hier nochmals bedienen */
 WDTsrv();

 /* Takt-Timer initialisieren : Timer 6 ist ein 16-bit Timer mit einer Zeitaufloesung von 3.2 us */
 T6CON  = 0;              /* Timer 6 stoppen */
 T6     = (unsigned int)6250;            /* Taktzeit 20 ms */
 CAPREL = (unsigned int)6250;            /* Taktzeit als Reload-Wert */
 T6CON  = 0x80C4;                        /* binaer : 1 0000 00 011 000 100 */
 T6IC   = 0x39;                          // Einstellen des Interruptlevels fuer Takt-Interrupt : enable, Level 14, Gruppenlevel 1 */39
 T6IE   = 1;                             /* Interrupt enablen */

  /*
   * Watchdog-Timer hier nochmals bedienen
   */
  WDTsrv();

 /* CAN steht zur Verfuegung */
 C167.CAN_rdy = TRUE;

  /*
   * Port 3.0 schaltet Flashlights
   * (Output, Push/Pull, passiv)
   */
  _putbit(1,DP3,0);
  _putbit(0,ODP3,0);
  _putbit(0,P3,0);

  /*
   * Port 3.1 schaltet Starkhorn
   * (Output, Push/Pull, passiv)
   */
  _putbit(1,DP3,1);
  _putbit(0,ODP3,1);
  _putbit(0,P3,1);

  /*
   * Port 3.2 schaltet LED-Matrix Bild 1
   * (Output, Push/Pull, passiv)
   */
  _putbit(1,DP3,2);
  _putbit(0,ODP3,2);
  _putbit(0,P3,2);

  /*
   * Port 3.3 schaltet LED-Matrix Bild 2
   * (Output, Push/Pull, passiv)
   */
  _putbit(1,DP3,3);
  _putbit(0,ODP3,3);
  _putbit(0,P3,3);

  /* Interrupts zulassen */
  IEN = 1;

 /* Hauptschleife */
 while (1)
 {
 ;
 }
}

#include <stdio.h>
#include <stdlib.h>
#include <reg167cs.h>
#include "can_ext.h"
#include "global.h"

interrupt 0x26
void TAKT(void) {

  unsigned int t6ic_alt = 0;
  //static int counter = 1;

  /* waehrend der Abarbeitung keinen neuen Interrupt zulassen */
  t6ic_alt = T6IC;
  T6IC = 0;


/*  if (counter < 100)
    counter++;
  else
    counter = 1;
*/

  //if(counter%2==0) { // alle 20 ms; Taktung: 10 ms (Timer 6)

    vRel = vEigen - vZielUebergabe;  // Relativgeschwindigkeit berechenen
    vZiel = vEigen + vRel;      // Tatsächtiche Zielgeschwindigkeit

    aEigen = (vEigen - vEigenVortakt) * (1000 / taktzeit); //Eigenbeschleunigung
    aZiel = (vZiel - vZielVortakt) * (1000 / taktzeit);    // Zielbeschleunigung

    aRel = aZiel - aEigen;    // Relativbeschleunigung
    aRelMax = -800 - aEigen;  // Maximale Relativbeschleunigung

    dReferenz1 = vEigen * vEigen * 7 / 12500;  // Referenzabstand für die 1. Warnstufe [cm/s]
    dReferenz2 = vEigen * vEigen / 2500;      // Referenzabstand für die 2. Warnstufe [cm/s]

    radikand = vRel * vRel + aRel * abstand * 2;    // Wurzelberechnung
    wurzel = isqrt_heron(radikand);            // Wurzelberechnung
    tBrems1 = ((vRel * 1000 / - aRelMax) / 2) + 1600 + 2000;  // Bremszeit1; Vergleichszeit Stufe 1
    tBrems2 = ((vRel * 1000 / - aRelMax) / 2) + 1600;       // Bremszeit2; Vergleichszeit Stufe 2
    ttcBesch = (wurzel - vRel) * 1000 / aRel;
    ttcConst = (abstand / vRel) * 1000;

    switch (sensorAus)  {

      case 1:  fehlerIDIS(); break;  // IDIS hat sich abgeschaltet

      default :

               if (aRelMax >= 0) {    // Das Egofahrzeug führt eine Notbremsung durch

                  notbremsung();

               }// else if (vRel < -100 && aRelMax < 0) {}      // Ziel entfernt sich

                 else if (vRel >= -100 && vRel <= 100 && aRelMax < 0) {  // Ziel folgt

                 if (abstand <= dReferenz2) stufe2();

                 if (abstand <= dReferenz1 && abstand > dReferenz2) stufe1();

                 //if (abstand > dReferenz1) allesOk();

               } else {    // Ziel fährt auf

                 if (vRel * vRel >= - aRel * abstand * 2 && aRel != 0 && aRelMax < 0) {    // TTC mit beschleunigten Fahrzeugen

                   if (ttcBesch <= tBrems2 && ttcBesch > 0) stufe2();

                   if (ttcBesch > tBrems2 && ttcBesch <= tBrems1) stufe1();

                   //if (ttcBesch > tBrems1 || ttc < 0) allesOk();

                 } else if (aRel == 0 && aRelMax < 0) {    // Keine Relativbeschleunigung --> Formel für unbeschleunigte Bewegung

                   if (ttcConst <= tBrems2) stufe2();

                   if (ttcConst > tBrems2 && ttcConst <= tBrems1) stufe1();

                   //if (ttcConst > tBrems1) allesOk();

                 }
    }

    vEigenUebergabe = vEigen * 36 / 100;  // Umrechnung der Einheiten und Auflösung für IDIS

    daten[0] = (gierrate&0x00FF);
    daten[1] = ((gierrate&0xFF00)>>8);
    daten[2] = (vEigenUebergabe&0x00FF);
    daten[3] = ((vEigenUebergabe&0xFF00)>>8);
    daten[4] = (lenkwinkel&0x00FF);
    daten[5] = ((lenkwinkel&0xFF00)>>8);
    daten[6] = (bremslicht);
    daten[7] = 0;

    def_mo_16x(0,14, 0, 0x333, 1, 8, 0, 0);
    ld_modata_16x(0,14, (unsigned char*)&daten);
    send_mo_16x(0,14);

    }
  //counter++;
  vZielVortakt = vZiel;
  vEigenVortakt = vEigen;

  //}

  T6IC = t6ic_alt; // Interrupt wieder zulassen

}

T6IC = 0x79;

tBrems1 = (((long)vRel * (long)1000 / - aRelMax) / 2) + 1600 + 2000;