#include <wixel.h>
#include <radio_queue.h>
#include <servo.h>

/* PARAMETERS *****************************************************************/



/* VARIABLES ******************************************************************/

uint16 result1, result2, result3, step;

/* FUNCTIONS ******************************************************************/

void updateLeds()
{
    LED_GREEN(1);
    LED_YELLOW(1);
    LED_RED(0);
}


void timer3Init() //DO NOT TOUCH!!!!!
{
    // Start the timer in free-running mode and set the prescaler.
    //T3CTL = 0b01110000;   // Prescaler 1:8, frequency = (24000 kHz)/8/256 = 11.7 kHz
    T3CTL = 0b01010000; // Use this line instead if you want 23.4 kHz (1:4)

    // Set the duty cycles to zero.
    T3CC0 = T3CC1 = 0;

    // Enable PWM on both channels.  We choose the mode where the channel
    // goes high when the timer is at 0 and goes low when the timer value
    // is equal to T3CCn.
    T3CCTL0 = T3CCTL1 = 0b00100100;

    // Configure Timer 3 to use Alternative 1 location, which is the default.
    PERCFG &= ~(1<<5);  // PERCFG.T3CFG = 0;

    // Configure P1_3 and P1_4 to be controlled by a peripheral function (Timer 3)
    // instead of being general purpose I/O.
    P1SEL |= (1<<3) | (1<<4);

    // After calling this function, you can set the duty cycles by simply writing
    // to T3CC0 and T3CC1.  A value of 255 results in a 100% duty cycle, and a
    // value of N < 255 results in a duty cycle of N/256.
}


//T3CC0 is P1_3, T3CC1 is P1_4 (?)
//"step" is used to set the duty cycle of T3CC0

void updatePwm()
{
	uint8 XDATA * packet = radioQueueRxCurrentPacket(); //fetch the current packet sent over radio
		result1 = packet[1]; //reassign variables
		result2 = packet[2];
		result3 = packet[3];

/*set speed of motor*******************************************************************/
	while(result1 > 2510) //while motor joystick is pushed forward
		{
			step = (result1 - 2510) / 80;
		}

	while(result1 < 2450) //while motor joystick is pushed backward
		{
			step = (1 / result1) * 6000; //I know its weird. I couldn't figure out a better way to make 'step' lower at higher voltages
		}

	while(2450 < result1 && result1 < 2510) //while motor joystick is not in use
		{
			step = 0;
		}

    T3CC0 = step*25; //sets the duty cycle

/*control the direction of the motor*******************************************/
    if(result1 < 2460) //If the joystick is pushed backwards
    {
    	setDigitalOutput(15, HIGH); //make the motor run backward
    }
    else
    {
    	setDigitalOutput(15, LOW); //else let the motor run forwards as normal
    }

}


void servoControl()
{
/*initialize servo library***************************************************/
	uint8 CODE pins[] = {2};  //Use P0_2 for servos.
	servosStart((uint8 XDATA *)pins, sizeof(pins));
	servoSetSpeed(0, 0); //give the servo no speed limit

/*move servo*******************************/
	if(result2 > 900)
	{
		servoSetTarget(0, 1000);
	}
	if(result2 < 760)
	{
		servoSetTarget(0, -1000);
	}

	radioQueueRxDoneWithPacket(); //prepare for next packet
}

void main()
{
    systemInit();
    timer3Init();
    radioQueueInit();
    updateLeds();

    while(1)
    {
        boardService();
        updatePwm();
        servoControl();
    }
}