------------------------------------------------------------------------------
-- Template                                                                         --
------------------------------------------------------------------------------

with LSM303; use LSM303;

with MicroBit.Display;
with MicroBit.Accelerometer;
with MicroBit.Console;
with MicroBit.Time;
with MicroBit.Buttons; use MicroBit.Buttons;
with HAL.GPIO;
with nRF.GPIO; use nRF.GPIO;

use MicroBit;


procedure Main is

   -- PROGRAM VARIABLES
   Data : LSM303.All_Axes_Data;
   -- Value : MicroBit.IOs.Analog_Value;
   FreeFallCounter : Integer;
   FreeFallDetectionCycles : Integer;
   FreeFallCondition : Boolean;
   MotorStarted : Boolean;
   Threshold : Axis_Data;

   -- IO VARIABLES
   DO_1_Pt : GPIO_Point;
   DO_2_Pt : GPIO_Point;
   DO_3_Pt : GPIO_Point;
   DO_4_Pt : GPIO_Point;
   DI_Conf_Down : GPIO_Configuration;


begin

   -- PROGRAM STARTUP
   Console.Put_Line ("Jumper - starting up ..");
   FreeFallCondition := False;
   MotorStarted := False;
   FreeFallCounter := 0;
   FreeFallDetectionCycles := 100;  -- 70 * 5ms = 350ms
   Threshold := 150;  -- acceleration sensor threshold

   -- IO CONFIGURATION
   DO_1_Pt := MB_P2;
   DO_2_Pt := MB_P8;
   DO_3_Pt := MB_P9;
   DO_4_Pt := MB_P16;


   DI_Conf_Down.Mode := Mode_In;
   DI_Conf_Down.Resistors := Pull_Down;
   DI_Conf_Down.Input_Buffer := Input_Buffer_Connect;
   DI_Conf_Down.Sense := Sense_Disabled;

   -- APPLY IO CONFIGURATION
   DO_1_Pt.Configure_IO(DI_Conf_Down);
   DO_2_Pt.Configure_IO(DI_Conf_Down);
   DO_3_Pt.Configure_IO(DI_Conf_Down);
   DO_4_Pt.Configure_IO(DI_Conf_Down);

   Display.Clear;
   loop
      if Buttons.State (Button_A) = Released and Buttons.State (Button_B) = Released and not FreeFallCondition then
         DO_1_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Down);
         DO_2_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Down);
         DO_3_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Down);
         DO_4_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Down);
      end if;
      if Buttons.State (Button_A) = Pressed then
         DO_1_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Up);
         DO_3_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Up);

         Display.Clear;
         Display.Display ('X');
      end if;
      if Buttons.State (Button_B) = Pressed then
         DO_2_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Up);
         DO_4_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Up);
         Display.Clear;
         Display.Display ('0');
      end if;


      --  Read the accelerometer data
      Data := Accelerometer.Data;

      --  Print the data on the serial port
      Console.Put_Line ("X:" & Data.X'Img & ASCII.HT &
                        "Y:" & Data.Y'Img & ASCII.HT &
                        "Z:" & Data.Z'Img);

      -- FREEFALL DETECTION
      -- check wether all axis are sub-threshold => increment counter
      if abs(Data.X) < Threshold and
         abs(Data.Y) < Threshold and
         abs(Data.Z) < Threshold then
         FreeFallCounter := FreeFallCounter + 1;
      -- otherwise reset counter
      else
         FreeFallCounter := 0;
      end if;



      -- FREEFALL DETECTION
      if FreeFallCounter >= FreeFallDetectionCycles then
         -- set free fall condition
         FreeFallCondition := True;
      end if;

      --  Clear the LED matrix
      --Display.Clear;

      -- Check, wether FreeFallCondition was triggered
      if FreeFallCondition then
         if not MotorStarted then
            MotorStarted := True;
            -- set outputs -> motor on
            DO_1_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Up);
            DO_2_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Up);
            DO_3_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Up);
            DO_4_Pt.Set_Pull_Resistor(Pull => HAL.GPIO.Pull_Up);
            -- maybe timeout and stop motor
            -- deep sleep afterwards
            -- Display.Display ('X');
         end if;
      -- else
         -- Display.Display ('0');
      end if;


      Time.Sleep (5);  -- 200 samples / s
   end loop;
end Main;
