#include "WPILib.h"

#include <iostream.h>

#include "math.h"

#include "BTShooter.h"

#include "BTRobot.h"

BTShooter::BTShooter(Sensors* inputSensors, ControlBoard* inputCB)

{

sensors = inputSensors;

controlBoard = inputCB;

stageTwoMotor = new Jaguar(PORTS::SHOOTER_MOTOR_S2_T);

stageOneMotor = new Jaguar(PORTS::SHOOTER_MOTOR_S1);

turret = new BTTurret(controlBoard);

arbitrator = new Arbitrator();

stageTwoMotor->Set(0.0);

stageOneMotor->Set(0.0);

enabled = false;

}

bool BTShooter::Fire(int position)

{

if (enabled)

{

arbitrator->Lock_N_Load(controlBoard, position);

return true;

}

else

{

return false;

}

}

void BTShooter::ShootRampUp(Jaguar* jag, float targetSpeed, int position)

{

float incConst;

incConst = (SHOOT_MOTOR_SPEED / (2.5 * position));

float actualSpeed = 0.0;

double next_time = GetClock();

while(targetSpeed > actualSpeed)

{

if (GetClock() >= next_time)

{

actualSpeed += incConst;

next_time += 0.2;

jag->Set(actualSpeed * -1);

}

}

}

void BTShooter::Enable(int position)

{

printf("Enabling shooter motors.\n");

enabled = true;

ShootRampUp(stageOneMotor, SHOOT_MOTOR_SPEED, position);

ShootRampUp(stageTwoMotor, SHOOT_TURRET_SPEED, position);

}

void BTShooter::Disable(void)

{

enabled = false;

printf("Disabling shooter motors.\n");

stageTwoMotor->Set(0.0);

stageOneMotor->Set(0.0);

}

void BTShooter::SetSpeed(float speed)

{

if (enabled)

{

float cameraDistance = (turret->camera->distance)/18 + 0.3;

if (cameraDistance > 1.0)

cameraDistance = 1.0;

stageTwoMotor->Set(speed * cameraDistance *-1);

}

}

void BTShooter::SetSpeedToThrottle(void)

{

if (enabled)

{

float motorSpeed = 0.0;

float throttle = 0.0;

throttle = ((controlBoard->shootStick->GetThrottle() - 1) / (-2));

motorSpeed = ((throttle * (1 - SHOOT_TURRET_SPEED)) + SHOOT_TURRET_SPEED);

SetSpeed(motorSpeed);

}

}