#ifndef AMIRO_LINEFOLLOWING_H
#define AMIRO_LINEFOLLOWING_H

#include <ch.hpp>

#include <amiroosconf.h>

namespace amiro {

class LineFollow
{
public:
  void printSensorData();
  void stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global);
  float SetPoint = 0x4000; // (0x1800+0x2800) >> 8
  float Kp = 0.001;
  float Ki = 0.00001;
  float Kd = 0.5
  ;
  int accSum = 0;
  float oldError = 0;
  int biggestDiff = 0;
  Global *global;
  LineFollow(Global *global);
  void calibrateZiegler(float KCrit, int rpmSpeed[2]);

  private:
    int delta();

};

} // end of namespace amiro

#endif // AMIRO_LINEFOLLOWING_H