/*
   Program: geo_energia.nxc
   Author: Team Kabby (Abigail Beblava, Katarina Farbulova)
   Date: 29.2.2018
   Description: bla bla bla
   */
   // konstanty
   #define PRAH_SENZOR 400
   #define MOTOR_SILA  60
   #define PRAH_TROJUHOLNIK 35
   
   //premenne
   long tacho;
   int velkosttrojuholnika;
   int x;
   unsigned int T1;
   unsigned int test1;
   unsigned int T2;
   unsigned int T3;
   unsigned int T4;
   unsigned int front_sensor;
   unsigned int back_sensor;
   task main () {
        // init
        SetSensorType (S1, SENSOR_TYPE_LIGHT_ACTIVE);
        SetSensorMode (S1, SENSOR_MODE_RAW);
        ResetSensor (S1);
        SetSensorType (S4, SENSOR_TYPE_LIGHT_ACTIVE);
        SetSensorMode (S4, SENSOR_MODE_RAW);
        ResetSensor (S4);

        while    ( (x>(0-PRAH_TROJUHOLNIK)&&(x<0+PRAH_TROJUHOLNIK))
                  || (x>(320-PRAH_TROJUHOLNIK)/*&&(x<300+PRAH_TROJUHOLNIK)*/)
                  ) {

                 //program ktory sa bude opakovat

                 OnFwdReg (OUT_AB, MOTOR_SILA, OUT_REGMODE_SYNC);
                   //OnFwd(OUT_AB,MOTOR_SILA);
                  //prva ciara
                  //predny senzor
                  front_sensor = Sensor (S4);
                  while (front_sensor>PRAH_SENZOR) {
                         front_sensor = Sensor (S4);
                  }
                  T1 = MotorTachoCount (OUT_A);

                  //zadny senzor
                  back_sensor = Sensor (S1);
                  while (back_sensor>PRAH_SENZOR) {
                        back_sensor = Sensor (S1);
                  }
                  T2 = MotorTachoCount (OUT_A);


                  //druha ciara
                  //predny senzor
                  front_sensor = Sensor (S4);
                  while (front_sensor>PRAH_SENZOR) {
                        front_sensor = Sensor (S4);
                  }
                  T3 = MotorTachoCount (OUT_A);

                  //zadny senzor
                  back_sensor = Sensor (S1);
                  while (back_sensor>PRAH_SENZOR) {
                        back_sensor = Sensor (S1);
                  }
                  T4 = MotorTachoCount (OUT_A);
                  x = (T2-T1)-(T4-T3);
        }

        //koniec
        Off(OUT_AB);
        Wait(1000);
        velkosttrojuholnika = ((T4-T2)/2)+60 ;
        OnRevReg (OUT_AB, MOTOR_SILA, OUT_REGMODE_SYNC);
        while (tacho>(-1*velkosttrojuholnika)) {
              tacho = MotorTachoCount (OUT_A);
        }
        Off(OUT_AB) ;
        Wait (20000);

}