/** 
 * @file
 * @brief KU Corobot API definition
 *
 * @author Chaiporn Jaikaeo <chaiporn.j@ku.ac.th>
 */
#ifndef COROBOT_H
#define COROBOT_H

#include <Arduino.h>
#include <Adafruit_TCS34725.h>

/*****************************************//**
 * Motor direction control
 */
enum Direction { BACKWARD, FORWARD };

/*****************************************//**
 * Color codes for color sensor and LEDs
 */
enum Color {
  BLACK   = 0,
  WHITE   = 1,
  RED     = 2,
  GREEN   = 3,
  BLUE    = 4,
  YELLOW  = 5,
  MAGENTA = 6,
  UNKNOWN = 7,
};

/*****************************************//**
 * Class definition for creating a Corobot controller object
 */
class CorobotC {
public:
  /** 
   * Serial object reference for communicating with the remote Corobot via
   * Bluetooth.
   * It has exactly the same API as Arduino's Serial object 
   * (https://www.arduino.cc/en/Reference/Serial).
   */
  HardwareSerial &remote = Serial1; 

  CorobotC();

  void init();

  void setLeftMotor(Direction dir,uint8_t speed) const;
  void setRightMotor(Direction dir,uint8_t speed) const;
  void setMotors(Direction dir_l,uint8_t speed_l,Direction dir_r,uint8_t speed_r);
  void calibrateMotors(uint8_t leftMotorPercent,uint8_t rightMotorPercent);

  bool getLeftEye() const;
  bool getRightEye() const;
  bool getCenterEye() const;

  Color getColor();
  const char* getColorName(Color c) const;
  void getRawColor(uint16_t *rr,uint16_t *rg,uint16_t *rb,uint16_t *rc);
  Color getColor(uint16_t rr,uint16_t rg,uint16_t rb,uint16_t rc) const;
  void calibrateColor(
          uint16_t   black_r, uint16_t   black_g, uint16_t   black_b, uint16_t   black_c,
          uint16_t   white_r, uint16_t   white_g, uint16_t   white_b, uint16_t   white_c,
          uint16_t     red_r, uint16_t     red_g, uint16_t     red_b, uint16_t     red_c,
          uint16_t   green_r, uint16_t   green_g, uint16_t   green_b, uint16_t   green_c,
          uint16_t    blue_r, uint16_t    blue_g, uint16_t    blue_b, uint16_t    blue_c,
          uint16_t  yellow_r, uint16_t  yellow_g, uint16_t  yellow_b, uint16_t  yellow_c,
          uint16_t magenta_r, uint16_t magenta_g, uint16_t magenta_b, uint16_t magenta_c);

  int8_t getPitch() const;
  int8_t getRoll() const;
  void getRawAccel(uint16_t *ax,uint16_t *ay,uint16_t *az);
  int8_t getPitch(uint16_t ax,uint16_t ay,uint16_t az) const;
  int8_t getRoll(uint16_t ax,uint16_t ay,uint16_t az) const;
  void calibrateAccel();
  void calibrateAccel(uint16_t ax,uint16_t ay,uint16_t az);

  void setLed(Color c,bool state);
  void setLedValue(uint8_t value);
  void setRightLed(bool state);
  void setLeftLed(bool state);
  bool switchPressed() const;
  void waitSwitchPress() const;
  void waitSwitchRelease() const;

  void waitRemote() const;

private:
  Adafruit_TCS34725 _tcs;
  uint8_t _leftMotorPercent;
  uint8_t _rightMotorPercent;
  uint8_t _color_map[UNKNOWN][4];  // [color][r,g,b,c]
  uint16_t _color_range[4][2];     // [r,g,b,c][min,max]
  uint16_t _accel_center_x;
  uint16_t _accel_center_y;
  uint16_t _accel_center_z;
};

////////////////////////////////////////////////////
extern CorobotC Corobot;

#endif
