#ifndef AMIRO_USERTHREAD_H_
#define AMIRO_USERTHREAD_H_

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


// Speed when driving towards the docking station
#define CHARGING_SPEED 5
#define DETECTION_SPEED 10
#define DIST_THRESH 100
#define RELEASE_COUNT 200
// Thresh to determain how much update steps should pass while alining
#define MAX_CORRECTION_STEPS 300
// Thresh for wheel proxy sensors, when summed values fall below the state changes
#define PROXY_WHEEL_THRESH 18000
// Thresh for detecting obsticles
#define PROXY_RING_THRESH 15000

#define PUSH_BACK_COUNT 5
// Thresh for how long (update steps) the front sensors are allowed to detect white
#define WHITE_COUNT_THRESH 150
// Rotation around 180 degrees in microradian
#define ROTATION_180 3141592
// Rotation around -20 degrees in microradian
#define ROTATION_20 -349065
#define ROTATION_DURATION 10000

#define RING_PROX_FRONT_THRESH 18000
#define PROX_MAX_VAL 65430

// Threshold for failing to dock
#define DOCKING_ERROR_THRESH 4

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 : uint8_t{
      IDLE                = 0,
      FOLLOW_LINE         = 1,
      DETECT_STATION      = 2,
      REVERSE             = 3,
      PUSH_BACK           = 4,
      CHECK_POSITIONING   = 5,
      CHECK_VOLTAGE       = 6,
      CHARGING            = 7,
      RELEASE             = 8,
      RELEASE_TO_CORRECT  = 9,
      CORRECT_POSITIONING = 10,
      ERROR               = 11,
      TURN                =12
    };

  struct ut_counter{
      int whiteCount = 0;
      int proxyCount = 0;
      // int correctionCount = 0;
      int errorCount = 0;
      int idleCount = 0;
      int stepCount = 0;
  };

  // static const struct ut_counter emptyUtCount;
  ut_counter utCount;

  explicit UserThread();

  virtual ~UserThread();

  virtual msg_t main();

private:
  void setRpmSpeed(const int (&rpmSpeed)[2]);
  void setRpmSpeedFuzzy(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();
  int getProxyRingSum();
  
  /**
   * Returns true when front sensors reaching high values
   * and all others are low. This indicates that the loading station is ahead.
   * If other sensores are blocked too, indicates that someone grabs the robot.
   */
  bool checkFrontalObject();

  states utState = states::IDLE;
  states newState = states::IDLE;
  bool continue_on_obstacle = true;
  /**
   * 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_