#ifndef AMIRO_USERTHREAD_H_
#define AMIRO_USERTHREAD_H_

#include <ch.hpp>
#include <amiroosconf.h>
#include <amiro/Color.h>
// #include "global.hpp"

// Speed when driving towards the docking station
#define CHARGING_SPEED 5
// Thresh to determain how much update steps should pass while alining
#define MAX_CORRECTION_STEPS 250
// Thresh for wheel proxy sensors, when summed values fall below the state changes
#define PROXY_WHEEL_THRESH 18000
// Thresh for how long (update steps) the front sensors are allowed to detect white
#define WHITE_COUNT_THRESH 120
// Rotation around 180 degrees in microradian
#define ROTATION_180 3141592
// Rotation around -20 degrees in microradian
#define ROTATION_20 -349065
#define ROTATION_DURATION 10000


namespace amiro {


class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
{

  // Messages which can be received and trigger state changes
  public:
    enum msg_content{
      STOP, 
      START,
      EDGE_LEFT,
      EDGE_RIGHT,
      FUZZY,
      DOCK,
      UNDOCK,
      CHARGE
    };

    // States of user thread state machine
    enum states{
      FOLLOW_LINE,
      RELEASE,
      CHARGING,
      DETECT_STATION,
      CORRECT_POSITIONING,
      REVERSE,
      CHECK_POSITIONING,
      IDLE
    };



  explicit UserThread();

  virtual ~UserThread();

  virtual msg_t main();

private:
  void setRpmSpeed(const int (&rpmSpeed)[2]);
  void lightOneLed(Color color, int idx);
  void lightAllLeds(Color color);
  /**
   * Uses light ring indicate the state of charge.
   */
  void showChargingState();
  void checkForMotion();
  bool checkPinVoltage();
  bool checkPinEnabled();

  /**
   * Check if current position changes when the wheel are deactivated.
   * 
   * When AMiRo drives towards the loading station, it stops when a specific marker is reached.
   * In order to validate that the AMiRo is correctly positioned in the loading station
   * the wheels are turned off. When the position remains the same the docking procedure
   * was successful (return 1) otherwise a correction is needed (return 0). 
   */
  int checkDockingSuccess();
};

} // end of namespace amiro

#endif // AMIRO_USERTHREAD_H_