#ifndef AMIRO_LINEFOLLOWING_H
#define AMIRO_LINEFOLLOWING_H

#include <ch.hpp>
#include "global.hpp"
#include <amiroosconf.h>

#define RAND_TRESH 16000
#define MAX_CORRECTED_SPEED 1000000*100

namespace amiro {
  
  enum LineFollowStrategy{
  EDGE_LEFT,      // driving on the left edge of a black line
  TRANSITION_L_R, // Transition from left to right edge
  TRANSITION_R_L, // transition from right to left edge
  EDGE_RIGHT,     // driving on the right edge of a black line
  REVERSE,
  MIDDLE,         // not working, use FUZZY instead
  FUZZY,
  NONE
  };

enum colorMember : uint8_t {
	BLACK=0,
	GREY=1,
	WHITE=2
};

class LineFollow
{
public:

  int biggestDiff = 0;
  Global *global;
  LineFollow(Global *global);
  LineFollow(Global *global, LineFollowStrategy strategy);
  /**
   * Entry method which should be called to follow a line.
   * 
   * @param rpmSpeed speed that will be determained
   * @return white flag, 1 if white is detected  
   */
  int followLine(int (&rpmSpeed)[2]);

  /**
   * Setter for LineFollowStrategy which triggers transition behavior.
   * 
   * To prevent random turns while changeing the strategy a transition state is set.
   * In case the strategy needs to be switched immediately use promptStrategyChange().
   * 
   * @param strategy 
   */
  void setStrategy(LineFollowStrategy strategy);
  void promptStrategyChange(LineFollowStrategy strategy);
  LineFollowStrategy getStrategy();
  void setGains(float Kp, float Ki, float Kd);


  const int rpmTurnLeft[2] = {-10, 10};
  const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
  const int rpmHalt[2] = {0, 0};
  // Definition of the fuzzyfication function
  //  | Membership
  // 1|_B__   G    __W__
  //  |    \  /\  /
  //  |     \/  \/
  //  |_____/\__/\______ Sensor values
  // SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
  // All values are "raw sensor values"
  /* Use these values for white ground surface (e.g. paper) */

  const int blackStartFalling = 0x1000; // Where the black curve starts falling
  const int blackOff = 0x1800; // Where no more black is detected
  const int whiteStartRising = 0x2800; // Where the white curve starts rising
  const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
  const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
  const int greyStartRising = blackStartFalling; // Where grey starts rising
  const int greyOff = whiteOn; // Where grey is completely off again

private:
  /**
   * Calculate the error from front sensors.
   */
  int getError();

  /**
   * Error calculation while changing from one EDGE_* strategy to another.
   * This prevents the AMiRo from random turns while switching strategies.
   * 
   * @param FL value of left front sensor
   * @param FR value of right front sensor
   * @param targetL left threshold 
   * @param targetR right threshold
   */
  int transitionError(int FL, int FR, int targetL, int targetR);

  /**
   * Use the error according to the strategy to calculate the correction speed.
   * Currently only the P part of the PID controller is used to calculate the 
   * correction speed.
   */
  int getPidCorrectionSpeed();

  // Fuzzy line following methods--------------
  void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]);
  Color memberToLed(colorMember member);
  void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
  colorMember getMember(float (&fuzzyValue)[3]);
  void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]);
  void copyRpmSpeed(const int (&source)[2], int (&target)[2]);
  int vcnl4020AmbientLight[4] = {0};
  int vcnl4020Proximity[4] = {0};
  // -----------------------------------------
  LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;
  char whiteFlag = 0;
  int trans = 0;

  // PID controller components ---------------
  int32_t K_p = 1900;
  float K_i = 0.15;
  float K_d = 0.6;
  int32_t accumHist = 0;
  int32_t oldError = 0;
  // ----------------------------------------
};



} // end of namespace amiro

#endif // AMIRO_LINEFOLLOWING_H