// Projekt 13 - Antygrawitacja
// 15 niebezpiecznie szalonych projektw. Szalony Geniusz

#define coilPin 11
#define irPin 13
#define sensorPin 0

int A = 2;
// Wyreguluj B w celu poprawy stabilnoci
int B = 60;
int C = 20;
int D = 1000;

int maxPower = 255; 
long powerCountThreshold = 300000;
int objectPresent = 0;
int monitoring = false;

void setup()
{
   pinMode(coilPin, OUTPUT);
   pinMode(irPin, OUTPUT);
   pinMode(sensorPin, INPUT);
   Serial.begin(9600);
   Serial.println("Gotowy");
   Serial.println("m - uruchamia monitorowanie");
   Serial.println("B - zwiksza B");
   Serial.println("b - zmniejsza B");
}

void loop()
{
   static int count = 0;
   static int oldPosition = 0;
   static int ambient = 0;
   static long powerCount = 0;
   count ++;
   
   if (count == 1000)
   {
      ambient = readAmbient();
      count = 0;
      objectPresent = (powerCount < powerCountThreshold);
      powerCount = 0;
   }
   int raw = 1024 - analogRead(sensorPin);
   // wykrywanie pozycji obiektu w obszarze dziaania czujnika: gra (0), d (650)
   int position = raw - ambient; 
   // warto dodatnia oznacza poruszanie si w d, a ujemna poruszanie si do gry
   int velocity = position - oldPosition; 
   int power = position / A + velocity * B + C;

   powerCount += power;
   oldPosition = position;
   
   // ograniczenie
   if (power > maxPower) power = maxPower;
   if (power < 0) power = 0;

   checkSerial();
   
   if (monitoring)
   {
      Serial.print(position);  Serial.print(","); 
      Serial.println(velocity); 
   }
   
   analogWrite(coilPin, power * objectPresent);
   delayMicroseconds(D);
}

int readAmbient()   
{
   digitalWrite(irPin, LOW);
   // daje czas diodzie LED i fototranzystorowi na rozpoczcie pracy
   delayMicroseconds(100);
   int ambient = 1024 - analogRead(sensorPin);
   digitalWrite(irPin, HIGH);
   return ambient;
}

void checkSerial()
{
   if (Serial.available())
   {
      char ch = Serial.read();
      if (ch == 'm')
      {
         monitoring = ! monitoring;
      }
      if (ch == 'B') 
      {
         B += 5;
         Serial.println(B);
      }
      if (ch == 'b')
      {
         B -= 5;
         Serial.println(B);
      }
   }
}
