#ifndef AMIRO_USERTHREAD_H_
#define AMIRO_USERTHREAD_H_

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



// TODO: Clean up and sort defines!
// Speed when driving towards the docking station
#define CHARGING_SPEED 5
#define DETECTION_SPEED 10
#define DIST_THRESH 100
#define RELEASE_COUNT 200
#define SPEED_CONVERSION_FACTOR 1000000 //Convert value to um/s
// Thresh to determain how much update steps should pass while alining
// #define MAX_CORRECTION_STEPS 200
#define DOCKING_CORRECTION_TIMEOUT 200
#define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT
#define REVERSE_ADJUSTMENT_TIMEOUT 200
// #define MAX_RING_PROX_VALUE_DEVIATION

// 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
#define PUSH_BACK_TIMEOUT 5
// Thresh for how long (update steps) the front sensors are allowed to detect white
// #define WHITE_COUNT_THRESH 150
#define WHITE_DETETION_TIMEOUT 150
// #define RING_PROX_COUNT_THRESH 1000
#define RING_PROX_DETECTION_TIMEOUT 400
// Rotation around 180 degrees in microradian
// #define ROTATION_180 3141592
#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
#define PROX_MAX_VAL 0xFFF0


// Threshold for failing to dock
#define DOCKING_ERROR_THRESH 3
#define CAN_TRANSMIT_STATE_THRESH 50
#define PROX_DEVIATION_MEAN_WINDOW 3
#define MAX_DEVIATION_CORRECTIONS 4
#define MAX_DEVIATION_FACTOR 35
#define DEVIATION_CORRECTION_DURATION 900
#define DEVIATION_CORRECTION_SPEED 2000000
#define DEVIATION_CORRECTION_VALUE (DEVIATION_CORRECTION_SPEED / 2)
#define DEVIATION_DIST_THRESH 25000

namespace amiro {


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

  // Messages which can be received and trigger state changes
  public:

    // States of user thread state machine
    enum states : int8_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,
      TURN                = 12,
      INACTIVE            = 13,
      CALIBRATION         = 14,
      CALIBRATION_CHECK   = 15,
      DEVIATION_CORRECTION = 16,
      DOCKING_ERROR       = -1,
      REVERSE_TIMEOUT_ERROR   = -2,
      CALIBRATION_ERROR   = -3,
      WHITE_DETECTION_ERROR   = -4,
      PROXY_DETECTION_ERROR   = -5,
      NO_CHARGING_POWER_ERROR   = -6,
      UNKNOWN_STATE_ERROR   = -7
    };

  struct ut_counter{
      int whiteCount = 0;
      int ringProxCount = 0;
      // int correctionCount = 0;
      int errorCount = 0;
      int stateCount = 0;
      int stepCount = 0;
      uint32_t stateTime = 0;
  };

  struct proxy_ctrl {
    int threshLow = 100;
    int threshMid = 500;
    int threshHigh = 1500;
    // int threshHigh = 1500;
    int penalty = 100000;
    int pFactor = 310000;
    int staticCont = 0;
  };


  struct bottom_prox_calibration {
    bool calibrateBlack = true;
    uint32_t buf = 0;
    uint8_t meanWindow = 150;
  };

  struct deviation_correction {
    bool RCase = true;
    int8_t pCount = 0;
    int32_t proxbuf[PROX_DEVIATION_MEAN_WINDOW] = { 0 };
    int32_t currentDeviation = 0;
  };

  // static const struct ut_counter emptyUtCount;
  ut_counter utCount;
  proxy_ctrl pCtrl;
  bottom_prox_calibration proxCalib;
  deviation_correction devCor;


  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 percentage of mean deviation between two given values.
  * It is intended to calculate the mean deviation between two proxy sensor
  * values. PROX_DEVIATION_MEAN_WINDOW determains the size of the mean window.
  * Keep in mind that initial results are wrong.
  * */
  int32_t meanDeviation(uint16_t a, uint16_t b);

  /**
   * Check sectors around and stop if a thresh in one sector is detected.
   */
  void preventCollision(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]);

  /**
   * Same as prevent collision but also lowers the speed when object is detected.
   */
  void proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]);
  void getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]);
  void getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax);
  void chargeAsLED();

  /**
   * 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();

  /**
   * 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();

  // State Variables
  states prevState = states::IDLE;
  states currentState = states::IDLE;
  states newState = states::IDLE;

  bool continue_on_obstacle = true;
  uint16_t rProx[8]; // buffer for ring proxy values
  int rpmSpeed[2] = {0};
  int stop[2] = {0};
  int turn[2] = {5,-5};

};

} // end of namespace amiro

#endif // AMIRO_USERTHREAD_H_