/*
* The EyeBot-32 RoBIOS API. Please do not edit this file.
* Parham Bahrami, 2025
* Supervised by Thomas Braunl, The University of Western Australia
*/



#ifndef EYEBOT_H
#define EYEBOT_H

#include <Arduino.h>
#include <Wire.h>
#include <VL53L0X.h>
#include <TFT_eSPI.h>
#include <driver/spi_master.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/semphr.h>
#include <mesh_util.h>


// TTGO-S3 specific power pins
#define PIN_POWER_ON 15
#define PIN_LCD_BL   38

// Image taking constants.
#define WIDTH  160
#define HEIGHT 120
#define BYTES_PER_FRAME  (WIDTH * HEIGHT * 2)
#define CHUNK_SIZE       (BYTES_PER_FRAME / 8)
#define NUM_CHUNKS       8
#define QQVGA_WIDTH 160
#define QQVGA_HEIGHT 120
#define QQVGA (QQVGA_WIDTH * QQVGA_HEIGHT)
#define QQVGA_SIZE   (QQVGA * 3)
#define INTENSITY_THRESHOLD 10


// SPI pins
#define CAM_PIN_SCK   13
#define CAM_PIN_MISO  12
#define CAM_PIN_MOSI  10 // not used but needs to be defined as a real pin for the SPI to work.
#define CAM_PIN_SS    11

// ToF Sensors
#define I2C_SCL 2
#define I2C_SDA 1
#define XSHUT_FRONT  3
#define XSHUT_RIGHT 10

#define PSD_FRONT 1
#define PSD_LEFT 2
#define PSD_RIGHT 3

// Encoder and physical constants
#define ENCODER_CPR 144
#define MM_PER_TICK (2 * PI * WHEEL_RADIUS_MM / ENCODER_CPR)  // 2 * pi * r / cpr
#define ROBOT_WHEEL_BASE 100.0  // Distance between wheels in mm
#define WHEEL_RADIUS_MM  32.5

// Motor pins
#define R_PWM_A 21
#define R_PWM_B 16
#define R_ENC_A 17
#define L_PWM_A 44
#define L_PWM_B 18
#define L_ENC_A 43

// Buttons
#define BTN_LEFT  0
#define KEY_LEFT   (1 << 0)   // 1
#define BTN_RIGHT 14
#define KEY_RIGHT  (1 << 1)   // 2
#define NOKEY 0
#define BOTH_KEYS 3


// --- Launch Sync Tunables ---
#define START_PRETENSION_MS   60      // brief reverse to take up backlash
#define START_RAMP_INTERVAL   10      // ms between ramp steps
#define START_RAMP_STEP       6       // PWM step per interval
#define START_BASE_PWM        60      // where the ramp begins after pretension
#define START_MAX_PWM         200     // safety cap during ramp

// tiny PI to keep encoder deltas equal during launch
#define SYNC_KP               6.0     // pwm per tick-delta
#define SYNC_KI               0.0     // pwm per (tick-delta)*s (usually 0)

#define PIN_BAT_VOLT 4


extern double robotX, robotY, robotPhi;
extern SemaphoreHandle_t poseMutex;
extern SemaphoreHandle_t speedMutex;

extern volatile int leftCount, rightCount;
extern long leftTotalTicks, rightTotalTicks;

extern double leftSpeed, rightSpeed;
extern double setLeftSpeed, setRightSpeed;
extern double leftPWM, rightPWM;

extern TaskHandle_t motorTaskHandle;
extern int travelTargetMM;
extern bool motorsEnabled;
extern bool reverseDirection;

extern TFT_eSPI tft;

typedef uint16_t RGB565;
typedef uint32_t COLOR;
typedef uint8_t BYTE;


// System Init
bool eyebotStart();                                                 // Safety initialisation function, only included incase another function fails.

// ========= Camera ===========

/**
 * \brief CAMInit dummy function to match RoBIOS.
 * \param resolution of camera. Has to be QQVGA to pass as the resolution of the camera cannot be changed.
 * \return 0 on success, -1 otherwise
 */
int CAMInit(int resolution);

/**
 * \brief Takes the current camera image from feed when it is called, as a native RGB565 image.
 * \param out565 RGB565 image which is of the size QQVGA.
 * \return 0 if successful, -1 if unsuccessful.
 */
int CAMGet565(uint16_t* out565);

/**
 * \brief Takes the current camera image and converts it into grayscale. Performs vertical flip and two endian swaps so TFT can push to screen.
 * \param gray565 the grayscale image in 16 bit integer format such that it can be used in the same way as CAMGet565().
 * \return 0 on succes, -1 on failure
 */
int CAMGet565Gray(uint16_t* gray565);

/**
 * \brief CAMRelease dummy function to match RoBIOS.
 * \return 0
 */
int CAMRelease();

/**
 * \brief Takes the current camera image from feed when it is called. Performs a vertical flip to correct image direction.
 * \param img RGB888 image which is of the size QQVGA_SIZE.
 * \return 0 if successful, -1 if unsuccessful.
 */
int CAMGet(BYTE* img);

/**
 * \brief Takes the current camera image and converts it into grayscale.
 * \param gray 8 bit grayscale image.
 * \return 0 on succes, -1 on failure
 */
int CAMGetGray(BYTE* gimg);

 // Convert QQVGA RGB888 → RGB565
BYTE* RGB888ToRGB565(const BYTE* rgb888);                      


/**
 * \brief   Returns the mm distance value from the chosen distance sensor.
 * \param   psd PSD_FRONT is front, PSD_LEFT is left, PSD_RIGHT is right as according to old function.
 * \returns distance of chosen sensor if successful, -1 if unsuccessful.
 */
int PSDGet(int psd);


// ========= VW Motion API ===========

/**
 * \brief	    Start background drive: linear (mm/s) & angular (deg/s)
 * \param       linSpeed the straightline speed (mm/s)
 * \param       angSpeed the angular turning speed (degrees/s)
 * \return      0 on success
 */
int VWSetSpeed(int linSpeed, int angSpeed);

/**
 * \brief		Obtains the current left and right speeds.
 * \note        THIS IS NOT THE SAME AS old ROBIOS.
 * \param       leftOut the double used to store the left wheel speed (mm/s)
 * \param       rightOut the double used to store the right wheel speed (mm/s)
 * \return      0 on success, 1 otherwise
 */
int VWGetSpeed(double* leftOut, double* rightOut);

/**
 * \brief	    Sets the global odometry position wrt current position of the robot.
 * \param       x the horizontal component (mm)
 * \param       y the vertical component (mm)
 * \param       phi the orientation of the robot (degrees)
 * \return      0 on success, 1 otherwise
 */
int VWSetPosition(int x, int y, int phi);

/**
 * \brief	    Gets the position wrt original position
 * \param       x stores the horizontal component (mm)
 * \param       y stores the vertical component (mm)
 * \param       phi stores the turn angle (degrees)
 * \return      0 on success, 1 otherwise
 */
int VWGetPosition(int* x, int* y, int* phi);

/**
 * \brief	      Checks the remaining drive distance
 * \return      the remaining  distance (mm) if it exists, -1 if no distance control is enabled or if mutex could not be taken.
 */
int VWRemain();

/**
 * \brief	    Blocks until the drive is complete
 * \return      0 on success, -1 otherwise
 */
int VWWait();

/**
 * \brief		Checks to see if the current drive is complete
 * \return      0 on completion, 1 for still driving
 */
int VWDone();

/**
 * \brief		Drives straight for a specified distance. Non-blocking.  
 * \param       dist the distance to travel
 * \param       lin_speed the speed to travel at
 * \return      0 on success, -1 otherwise
 */
int VWStraight(int dist, int lin_speed);

/**
 * \brief	    Turns the vehicle on the spot. Is blocking.
 * \param       angle the angle to turn (degrees)
 * \param       ang_speed the speed to turn (degrees/s)
 * \return      0 on success, -1 othewrise
 */
int VWTurn(int angle, int ang_speed);

/**
 * \brief	    Turns the vehicle in a curve
 * \param       dist the radial distance (in mm)
 * \param       angle the angle to rotate (degrees)
 * \param       lin_speed the speed to rotate (mm/s)
 * \return      0 on success, -1 othewrise
 */
int VWCurve(int dist, int angle, int lin_speed);

/**
 * \brief	    Turns the vehicle in a curve
 * \param       dx the distance in front of the vehicle to drive (in mm)
 * \param       dy the distance perpendicular to the direction of travel (in mm)
 * \param       lin_speed the speed to rotate
 * \return      0 on success, 1 othewrise
 */
int VWDrive(int dx, int dy, int lin_speed);

/**
 * \brief		Checks to see if the vehicle is stalled
 * \note        A vehicle is stalled if it is trying to move but the speed is below a threshold. Given we have a kick at the start, this usually means the vehicle is stuck.
 * \return  	Number of stalled motor [1..2], 3 if both stalled, 0 if none
 */
int VWStalled();

/**
 * \brief     Moves the vehicle to a specific point in space. Turns to face the direction of travel, then moves in a straight line.
 * \param     dx the horizontal component (mm)
 * \param     dy the vertical component (mm)
 * \param     lin_speed the speed to travel at (mm/s)
 */
int VWGoTo(int dx, int dy, int lin_speed);

/**
 * \brief     Stops the robot and kills any VW task.
 * \return    0 on success.
 */
int VWStop();

/**
 * \brief   Set the given motor to a given speed
 * \param   motor the motor to set
 * \param   speed : motor speed
 * \details Valid values for speed :
 * - -100 to 100 (full backward to full forward)
 * - 0 for full stop
 * \return  0 on success, -1 on failure
 */
int MOTORDrive(int motor, int speed);

/**
 * \brief   Set the given motors with PID control
 * \param   motor the motor to set
 * \param   p the p setting
 * \param   i the i setting
 * \param   d the d setting
 * \note    Motor 1 is left, Motor 2 is right.
 * \return  0 on success, -1 on failure
 */
int MOTORPID(int motor, int p, int i, int d);

/**
 * \brief	    Turns PID off (no-op)
 * \note        This doesn't work, still in development.
 * \param       The motor to select
 * \return      0 on success, -1 othewrise
 */
int MOTORPIDOff(int motor); 			                            // Disable/clear PID for wheel (safe no-op)

/**
 * \brief	    printf at current cursor position
 * \param       format what you wish to print.
 * \return      0 on success, -1 othewrise
 */
int   LCDPrintf(const char* format, ...);                           // printf at current cursor.

/**
 * \brief	    Move cursor (row, column in px) then printf text.
 * \param       row the row that you wish to set the cursor to
 * \param       column the column that you wish to set the cursor to
 * \param       format what you wish to print.
 * \note        this is like tft.setcursor(x,y), but the order is (y,x)
 * \return      0 on success, -1 othewrise
 */
int   LCDSetPrintf(int row,int column,const char*,...);

/**
 * \brief	      Clears the screen (fills it black)
 * \return      0 on success, -1 othewrise
 */
int   LCDClear();

/**
 * \brief	      Move cursor (row, column in px)
 * \param       row the row that you wish to set the cursor to
 * \param       column the column that you wish to set the cursor to
 * \return      0 on success, -1 othewrise
 */
int   LCDSetPos(int row, int column);

/**
 * \brief	      Get cursor (row = y, column = x)
 * \param       row position of the row (y)
 * \param       column position of the column (x)
 * \return      0 on success, -1 othewrise
 */
int   LCDGetPos(int *row, int *column);

/**
 * \brief	      Set text colours (COLOR = 0xRRGGBB).
 * \param       fg foreground colour
 * \param       bg background colour
 * \return      0 on success, -1 othewrise
 */
int   LCDSetColor(COLOR fg, COLOR bg);

/**
 * \brief	    Select TFT_eSPI font index.
 * \param       font the font index number you wish to use.
 * \param       variation the variation of the font you wish to use.
 * \return      0 on success
 */
int   LCDSetFont(int font, int variation);

/**
 * \brief	    Select text scale factor
 * \param       fontsize size of the text (same as tft library)
 * \return      0 on success
 */
int   LCDSetFontSize(int fontsize);

/**
 * \brief	      Unsupported
 */
int   LCDSetMode(int mode);

/**
 * \brief       Retrieves the dimensions of the TFT display.
 * \param       x pointer to store the display width in pixels.
 * \param       y pointer to store the display height in pixels.
 * \return      0 on success, -1 if input pointers are invalid.
 */
int   LCDGetSize(int *x, int *y);

/**
 * \brief       Draws a single pixel on the TFT display at the specified coordinates.
 * \param       x the horizontal pixel coordinate.
 * \param       y the vertical pixel coordinate.
 * \param       col the colour of the pixel in RGB888 format.
 * \return      always returns 0.
 */
int   LCDPixel(int x, int y, COLOR col);

/**
 * \brief	      Draw line on the screen.
 * \param       x1 starting x position of line
 * \param       x2 ending x position of line
 * \param       y1 starting y position of line
 * \param       y2 ending y position of line
 * \param       col colour of the line
 * \return      0 on success, -1 othewrise
 */
int   LCDLine(int x1,int y1,int x2,int y2,COLOR col);

/**
 * \brief	    Draws a rectangle with optional fill in using a colour.
 * \param       x1 starting x position of rectangle
 * \param       x2 ending x position of rectangle
 * \param       y1 starting y position of rectangle
 * \param       y2 ending y position of rectangle
 * \param       col colour of the rectangle
 * \param       fill choose whether the rectangle should be filled in or not.
 * \return      0 on success
 */
int   LCDArea(int x1,int y1,int x2,int y2,COLOR col,bool fill);

/**
 * \brief	      Draws a circle with optional fill in using a colour.
 * \param       x1 centre x position of circle
 * \param       y1 centre y position of circle
 * \param       col colour of the circle
 * \param       fill choose whether the circle should be filled in or not.
 * \return      0 on success
 */
int   LCDCircle(int x,int y,int radius,COLOR col,int fill);

/**
 * \brief	      Define target area for subsequent image draws.
 * \param       x starting x position of image (top left)
 * \param       y starting y position of image (top left)
 * \param       xs width of the image to be drawn. QQVGA_WIDTH by default
 * \param       ys height of the image to be drawn. QQVGA_HEIGHT by default
 * \return      0 on success
 */
int   LCDImageStart(int x,int y,int xs,int ys);

/**
 * \brief	    Draws an RGB888 image (QQVGA) into an area set by LCDImageStart
 * \param       img the RGB888 image that you wish to display.
 * \return      0 on success, -1 on failure
 */
int   LCDImage(BYTE *img);

/**
 * \brief	      Draws an RGB565 (QQVGA) image into an area set by LCDImageStart.
 * \param       img565 the RGB565 you wish to display.
 * \return      0 on success
 */
int   LCDImage565(const uint16_t* img565);

/**
 * \brief	    Draws an 8 bit grayscale image on the screen.
 * \param       gray the grayscale image you want to draw.
 * \return      0 on success, -1 on failure
 */
int   LCDImageGray(BYTE *g);

/**
 * \brief	    Draw binary (>=thr white, else black) from 8-bit gray.
 * \param       gray the gray image which you will threshold.
 * \param       threshold the threshold cutoff for the binarization (128 by default).
 * \return      0 on success
 */
int   LCDImageBinary(BYTE *b, uint8_t threshold=128);

/**
 * \brief	    The TFT automatically refreshes by default.
 * \return      0 on success
 */
int   LCDRefresh(void);


// ========== Keys ============

/**
 * \brief	     Waits for a key press. Incorporates debounce.
 * \return     KEY_LEFT (1), KEY_RIGHT (2) or BOTH_KEYS (3) depending on which key is pressed.
 */
int KEYGet();

/**
 * \brief	   Instantaneous key read.
 * \return     KEY_LEFT (1), KEY_RIGHT (2) or BOTH_KEYS (3) depending on which key is pressed. 0 if no keys pressed.
 */
int KEYRead();

/**
 * \brief	   Block until the wanted buttons are pressed.
 * \param      wantedMask the key that you want to wait for (KEY_LEFT or KEY_RIGHT or BOTH_KEYS)
 * \return     0 on success
 */
int KEYWait(int wantedMask);

/**
 * \brief Reads key presses on their rising edge (just-pressed events).
 *
 * This function detects when a key transitions from "not pressed" to "pressed".
 * Unlike KEYRead(), which returns the current state continuously, KEYReadEdge()
 * only returns a nonzero bitmask on the first loop iteration where the key is
 * newly pressed.
 *
 * - Each bit in the return value corresponds to one key.
 * - A '1' in a bit position means that key was just pressed during this call.
 * - If the key is held down, subsequent calls return 0 until the key is released
 *   and pressed again.
 *
 * \return Bitmask of newly pressed keys (0 if no new press detected).
 */
int KEYReadEdge();				                                    // Rising-edge bitmask since last call.

/**
 * \brief	   Simple Laplace edge detector
 * \param      grayIn the grayscale image you wish to perform laplace edge detection on.
 * \param      grayOut the filtered image.
 */
void IPLaplace(BYTE* grayIn, BYTE* grayOut);

/**
 * \brief	     Simple Sobel edge detector
 * \param      in the grayscale image you wish to perform sobel edge detection on.
 * \param      out the filtered image.
 */
void IPSobel(BYTE* grayIn, BYTE* grayOut);

/**
 * \brief	     Converts an RGB888 colour image to a grayscale image.
 * \param      imgIn the colour image you wish to convert to grayscale.
 * \param      grayOut the grayscale conversion of the image.
 */
void IPCol2Gray(BYTE* imgIn, BYTE* grayOut);

/**
 * \brief	     Converts a grayscale image back to colour.
 * \param      grayIn the grayscale image you want to convert back to colour.
 * \param      colOut the coloured image output (RGB888)
 */
void IPGray2Col(BYTE* imgIn, BYTE* colOut);

/**
 * \brief       Converts an entire RGB image into its HSI (Hue, Saturation, Intensity) representation.
 * \param       imgIn the input colour image in RGB format.
 * \param       H pointer to the output hue channel array (size = QQVGA).
 * \param       S pointer to the output saturation channel array (size = QQVGA).
 * \param       I pointer to the output intensity channel array (size = QQVGA).
 */
void IPCol2HSI(BYTE* img,BYTE* h,BYTE* s,BYTE* i);

/**
 * \brief	     Overlays a colour image onto another one, where a black pixel in C2 is perceived as transparency.
 * \param      c1 the background image
 * \param      c2 the foreground (overlayed) image, where a black pixel is perceived as transparency.
 * \param      cOut the resultant image.
 */  
void IPOverlay(BYTE* c1,BYTE* c2,BYTE* cOut);

/**
 * \brief       Overlays a coloured mask onto a grayscale image.
 * \param       g1 the base grayscale image.
 * \param       g2 the mask image, where non-zero pixels define the overlay regions (black pixels perceived as transparency)
 * \param       r the red channel value used for the overlay colour.
 * \param       g the green channel value used for the overlay colour.
 * \param       b the blue channel value used for the overlay colour.
 * \param       cOut the resultant colour image, where masked pixels take the overlay colour and others remain grayscale.
 */
void IPOverlayGray(BYTE* g1,BYTE* g2,BYTE r, BYTE g,BYTE b,BYTE* cOut);

/**
 * \brief       Converts an entire RGB image into its HSI (Hue, Saturation, Intensity) representation.
 * \param       imgIn the input colour image in RGB format.
 * \param       H pointer to the output hue channel array (size = QQVGA).
 * \param       S pointer to the output saturation channel array (size = QQVGA).
 * \param       I pointer to the output intensity channel array (size = QQVGA).
 */
void IPPCol2HSI(BYTE* imgIn,BYTE* H,BYTE* S,BYTE* I);

/**
 * \brief       Converts an RGB pixel value to its HSI (Hue, Saturation, Intensity) representation.
 * \param       r the red channel value of the input pixel.
 * \param       g the green channel value of the input pixel.
 * \param       b the blue channel value of the input pixel.
 * \param       H pointer to the resulting hue value (0–255).
 * \param       S pointer to the resulting saturation value (0–255).
 * \param       I pointer to the resulting intensity value (0–255).
 */
void IPPRGB2HSI(BYTE r,BYTE g,BYTE b,BYTE* H,BYTE* S,BYTE* I);

/**
 * \brief	     Converts an RGB565 colour image to its Hue, Saturation and Intensity Values.
 * \param      src the colour RGB565 image you wish to convert to HSI
 * \param      H the hue values of the image
 * \param      S the saturation values of the image.
 * \param      I8 the intensity values of the image.
 */  
void IPRGB565toHSI(uint16_t* src,int w,int h, uint8_t* H,uint8_t* S,uint8_t* I8);

/**
 * \brief	     Converts an RGB565 colour image to grayscale.
 * \param      src the colour RGB565 image you wish to convert to grayscale.
 * \param      gray the grayscale image.
 */  
void IPRGB565toGray(const uint16_t* src, BYTE* gray);

/**
 * \brief       Adjusts brightness and contrast of an RGB565 image buffer.
 * \param       buf pointer to the image buffer in RGB565 format.
 * \param       pixels the number of pixels in the buffer.
 * \param       bright brightness adjustment offset (positive = brighter, negative = darker).
 * \param       contrast contrast scaling factor (1.0 = unchanged, >1.0 = higher contrast, 0.0 = flat).
 * \param       swapBytes whether to swap the byte order of each pixel (true for TFT screen compatibility).
 */
void IP565Adjust(uint16_t* buf,size_t pixels, int bright,float contrast,bool swapBytes);

/**
 * \brief       Adjusts the brightness of an RGB565 image buffer.
 * \param       buf pointer to the image buffer in RGB565 format.
 * \param       pixels the number of pixels in the buffer.
 * \param       bright brightness adjustment factor (positive = brighter, negative = darker).
 * \param       swapBytes whether to swap the byte order of each pixel (true for TFT-screen compatibility).
 */
void IP565Brightness(uint16_t* buf,size_t pixels, int bright, bool swapBytes);

/**
 * \brief       Adjusts the brightness of an RGB565 image buffer.
 * \param       buf pointer to the image buffer in RGB565 format.
 * \param       pixels the number of pixels in the buffer.
 * \param       contrast contrast scaling factor (1.0 = unchanged, >1.0 = higher contrast, 0.0 = flat).
 * \param       swapBytes whether to swap the byte order of each pixel (true for TFT-screen compatibility).
 */
void IP565Contrast(uint16_t* buf,size_t pixels, float contrast,bool swapBytes);

/**
 * \brief	     Performs morphological image opening to reduce salt and pepper noise on a binary image.
 * \param      binaryImg the image you wish to perform opening on.
 */
void IPOpen(uint8_t* binaryImg);

/**
 * \brief       Adjusts brightness and contrast of an RGB888 image buffer.
 * \param       rgb pointer to the image buffer in RGB888 format (3 bytes per pixel).
 * \param       pixels the number of pixels in the buffer.
 * \param       bright brightness adjustment offset (positive = brighter, negative = darker).
 * \param       contrast contrast scaling factor (1.0 = unchanged, >1.0 = higher contrast, 0.0 = flat).
 */
void IPAdjust(BYTE* rgb,size_t pixels, int bright,float contrast);

/**
 * \brief       Adjusts the brightness of an RGB888 image buffer.
 * \param       rgb pointer to the image buffer in RGB888 format (3 bytes per pixel).
 * \param       pixels the number of pixels in the buffer.
 * \param       bright brightness adjustment offset (positive = brighter, negative = darker).
 */
void IPBrightness(BYTE* rgb,size_t pixels,int bright);

/**
 * \brief       Adjusts the contrast of an RGB888 image buffer.
 * \param       rgb pointer to the image buffer in RGB888 format (3 bytes per pixel).
 * \param       pixels the number of pixels in the buffer.
 * \param       contrast contrast scaling factor (1.0 = unchanged, >1.0 = higher contrast, 0.0 = flat).
 */
void IPContrast(BYTE* rgb,size_t pixels,float contrast);            // RGB888 contrast only.


#endif // EYEBOT