/* BTCamera

* camera-servo hybrid class

* manages camera and servos, including tracking functions

*/

#ifndef BTCAMERA_H_

#define BTCAMERA_H_

#include "WPILib.h"

#include <stdio.h>

#include "math.h"

#include "VisionAPI.h"

#include "TrackAPI.h"

#include "AxisCamera.h"

#include "FrcError.h"

#include "BTRobot.h"

#include "ControlBoard.h"

// for 160x120, 50 pixels = .38 %

#define MIN_PARTICLE_TO_IMAGE_PERCENT 0.25 // target is too small

#define MAX_PARTICLE_TO_IMAGE_PERCENT 20.0 // target is too close

class BTCamera

{

public:

TrackingThreshold td1, td2; // color thresholds

// ParticleAnalysisReport par; // particle analysis report

ColorReport cReport, cReport2; // color report

static const int SHOOTER_RANGE_LIMIT = 20;

enum WHICH_SERVO

{

VERT,

HORIZ

}whichServo;

bool foundColor;

double savedImageTimestamp;

bool staleImage;

float distance;

BTCamera(void);

bool IsTooClose(ParticleAnalysisReport*);

bool IsBigEnough(ParticleAnalysisReport*);

float GetCameraDistance(ParticleAnalysisReport*);

double Track(ControlBoard*);

bool IsTargetInRange(void);

//float GetServoValue(int);

private:

Servo* horizontalServo;   // first servo object

Servo* verticalServo; // second servo object

int count;

float horizontalDestination;

float verticalDestination;

float horizontalPosition, verticalPosition;

float horizontalServoPosition, verticalServoPosition;

float servoDeadband; // percentage servo delta to trigger move

int framesPerSecond; // number of camera frames to get per second

float panControl; // to slow down pan

float panPosition;

double sinStart;

static double panStartTime;

// search variables

// initialize pan variables

// incremental tasking toward dest (-1.0 to 1.0)

float incrementH, incrementV;

// pan needs a 1-up number for each call

int panIncrement;

// for controlling loop execution time

float loopTime;

double currentTime;

double lastTime;

void SetServoPositions(float, float);

void AdjustServoPositions(float, float);

};

#endif