#pragma once
#include "packet.h"
#include <math.h>
#define _USE_MATH_DEFINES // To access M_PI constant for trig functions

extern packet_t pA, pB, safe;
extern packet_t *astate, *incoming;
extern comm_state cs;
extern char comm_ok;
extern long last_p;

/*
#define CLOCKWISE 0
#define ANTICLOCKWISE 1
*/

// Math things
#define DEGREES_PER_RADIAN (180.0 / M_PI)
#define RADIANS_PER_DEGREE (M_PI / 180.0)

// Drive modes
#define DRIVE_BASIC 0
#define DRIVE_TRANSLATION 1
#define DRIVE_ROTATION 2

// Basic mode conditions, specifies which direction and turning direction the robot is using
#define DRIVE_BASIC_STOP        0
#define DRIVE_BASIC_FORWARD     1
#define DRIVE_BASIC_FRONTLEFT   2
#define DRIVE_BASIC_FRONTRIGHT  3
#define DRIVE_BASIC_BACKWARD    4
#define DRIVE_BASIC_BACKLEFT    5
#define DRIVE_BASIC_BACKRIGHT   6

// Length of the buffer to monitor recent encoder positions to calculate speed
// The buffer will track the last N states of the encoders, and the times at which they were recorded, to determine the robot's current speed
#define ENCODER_BUFFER_ENTRY_COUNT 5



#define SerComm Serial1   //Serial port connected to Xbee
#define DIAMOND_LEFT 0
#define DIAMOND_DOWN 1
#define DIAMOND_RIGHT 2
#define DIAMOND_UP 3
#define SHOULDER_TOP_LEFT 4
#define SHOULDER_TOP_RIGHT 5
#define SHOULDER_BOTTOM_LEFT 6
#define SHOULDER_BOTTOM_RIGHT 7
#define SMALL_LEFT 8
#define SMALL_RIGHT 9
//10 and 11 are probably the stick buttons
//but we haven't checked recently
#define DPAD_UP 12
#define DPAD_RIGHT 13
#define DPAD_DOWN 14
#define DPAD_LEFT 15

// pins for motor controller 1 (right)
#define ALI1 0
#define AHI1 1
#define BHI1 2
#define BLI1 3
#define DENABLE1 8
//#define DREADY1 30

// and 2 (left)
#define ALI2 4
#define AHI2 5
#define BHI2 6
#define BLI2 7
#define DENABLE2 9
//#define DREADY2 31

#define try_enable_right(e,VBATT) try_enable_osmc(e,DENABLE1,VBATT,ALI1,BLI1,AHI1,BHI1)
#define try_enable_left(e,VBATT) try_enable_osmc(e,DENABLE2,VBATT,ALI2,BLI2,AHI2,BHI2)
#define drive_right(e,x) drive_osmc(e,DENABLE1,x,0,ALI1,BLI1,AHI1,BHI1)
#define drive_left(e,x) drive_osmc(e,DENABLE2,x,0,ALI2,BLI2,AHI2,BHI2)

#define DEADBAND_HALF_WIDTH 10  // Control input deadband radius
#define FAILTIME 200    //Failsafe timeout in milliseconds
#define DEBUGPRINT(x) SerCommDbg.println(x)
#define SerCommDbg Serial   //Serial port for debugging info