#define speed_cnt 30
#define speed_delta 4


void hall_programme(void)
{
  int timer_reading;
  int throttle_reading;

  unsigned long last_swap = 0;
  unsigned long now;

  unsigned long swap1;
  unsigned long swap2;
  unsigned long t1;
  unsigned long t2;
  int phase = 0;
  unsigned long dt;
  unsigned long comp, comp2;
  
  long deltatime;
  long last_deltatime = 0;
  long duty_promille;

  long speeder[speed_cnt];
  for (int i = 0; i < speed_cnt; i++)
    speeder[i] = 0;
  int speed_ptr;
  long speed_sum = 0;
  
  delay(500);
  t1 = millis() - 300;
  t2 = millis() - 100;
  swap1 = millis() + 50;
  swap2 = millis() + 100;
  bool my_flag = false;
  
  while (digitalRead(mode2p) == LOW)
  {
    timer_reading = analogRead(timer_pin);
    throttle_reading = analogRead(throttle_pin);
    duty_promille = throttle_reading;
    now = millis();
  
    comp = hall_time; // comp holds the value of hall_time during the whole loop,
                      // we don't want a changing value during the loop.
                      // (hall_time gets updated in an interrupt)
    
    // When a signal comes from the Hall sensor,
    // it is time to update the last timestamps
    if (t2 != comp)
    {
      t1 = t2;
      t2 = comp;
      my_flag = false;
      deltatime = t2 - t1;
      speed_sum -= speeder[speed_ptr];
      speed_sum += (deltatime - last_deltatime);
      speeder[speed_ptr] = (deltatime - last_deltatime);
      last_deltatime = deltatime;
      speed_ptr++;
      speed_ptr %= speed_cnt;
      if (speed_sum > 2)
      {
        digitalWrite(pole1, HIGH);
        digitalWrite(pole2, LOW);
      }
      else 
      {
        if (speed_sum < -2)
        {
          digitalWrite(pole2, HIGH);
          digitalWrite(pole1, LOW);
        }
        else 
        {
          digitalWrite(pole1, LOW);
          digitalWrite(pole2, LOW);
        }
      }
    }
    
  
    // Apart from updating the timestamps, check
    // whether it's time to do the two swaps.
    switch (phase)
    {
      case 0: // waiting for the first swap
        if (now > swap1)
        {
          swap(true);
          duty(true);
          last_swap = now;
          
          phase = 1;
          my_flag = true;
        }
        break;
      case 1: // waiting for the second swap
        if (now > swap2)
        {
          swap(false);
          duty(true);
          
          last_swap = now;
          phase = 2;
        }
        break;
      case 2: // second swap has happened
        if (!my_flag)
        {
          swap1 = t2 + (t2 - t1) * timer_reading / 1024;
          swap2 = swap1 + (t2 - t1) / 2;
          
          phase = 0; 
        }
        break;
    }
  
    if (on_duty && (now > last_swap + duty_promille * deltatime / 1024))
      duty(false);
  }
}

void duty(bool du)
{
  on_duty = du;
  if (du)
    digitalWrite(duty_pin, HIGH);
  else
    digitalWrite(duty_pin, LOW);
  
}