//
// BlimpBot
// CSE 466 final project
// Josh Green, Baron Oldenburg, Henry Baba-Weiss
//
#include <msp430f5510.h>
#include "../virtual_com_cmds.h"

void initPWM(void);
void initADC(void);
void setVerticalFanEnabled(int flag);
void setMotors(char direction, int speed);
void pollIRSensor(void);
void itoa(long num, char *str, int bufferSize);
char DigitToChar(unsigned int num);

// Blimp state
static int gActive = 0;
static char IR_CHAR = 0;
static long ADC_sum = 0;
static int ADC_count = 0;
static bool gAutoHeight = false;
static bool gGetSetPoint = true;
static int gSetPoint = 0;
static int gEpsilon = 1;  // to prevent chatter
static int counter = 0; // for the vertical motor's software PWM

// Vertical motor control
static int gUpSpeed = 0;
static int gDownSpeed = 0;

void main(void)
{
  // Stop the watchdog timer
  WDTCTL = WDTPW + WDTHOLD;

  initPWM();
  initADC();
  COM_Init();
  __enable_interrupt();

  while (1)
  {
  	// Process IR request if we've received one
    if (IR_CHAR != 0)
    {
      char str[8];
      str[0] = IR_CHAR;
      pollIRSensor();
      IR_CHAR = 0;
      itoa(ADC_sum, str+1, 7);
      TXString(str, 8);
    }

    if (gAutoHeight) {
      // poll the IR sensor for the current height
      pollIRSensor();

      if (gGetSetPoint) {
        // are we setting the set point?
        gSetPoint = ADC_sum;
        gGetSetPoint = false;
      } else {
        // we already have a set point
        // use a bang-bang controller to maintain that height
        int error = ADC_sum - gSetPoint;

        if (error > gEpsilon) {
          // motor on
          setMotors(UART_UP, 4);
        } else {
          // motor off
          setMotors(UART_DOWN, 0);
        }
      }
    }
  }
}

// Sets up necessary control registers for both hardware and software PWM
void initPWM(void)
{
  P1DIR |= 0x3F;                            // P1.0 - P1.5 output
  P1SEL |= 0x3C;                            // P1.0 - P1.5 TA1/2 options
  P1OUT |= 0x3F;
  TA0CTL = TASSEL_2 + MC_1;                 // SMCLK, up mode
  TA0CCTL1 = OUTMOD_7;                      // CCR1 reset/set
  TA0CCTL2 = OUTMOD_7;
  TA0CCTL3 = OUTMOD_7;
  TA0CCTL4 = OUTMOD_7;
  TA0CCR0 = 6000;

  TB0CTL = TASSEL_2 + MC_1;                  // SMCLK, upmode
  setVerticalFanEnabled(0);
}

// Sets up necessary control registers for enabling ADC sampling of the IR sensor
void initADC(void)
{
  // Set up IR sensor
  P1DIR |= 0x40;
  PJDIR |= 0x03;
  P2DIR |= 0x01;
  P2OUT &= ~0x01;  // P2.0 is the enable line for the IR sensor 
  P6SEL |= 0x01;

  // Set up ADC
  ADC10CTL0 |= ADC10SHT2 + ADC10ON;         // Sampling time, ADC12 on
  ADC10CTL1 |= ADC10SHP;                    // Use sampling timer
  ADC10IE = 0x01;                           // Enable interrupt
  ADC10CTL0 |= ADC10ENC;
  ADC10MCTL0 |= ADC10INCH0;                 // Select A0
}

// Enables or disables the PWM for the vertical motor, based on the given boolean flag
void setVerticalFanEnabled(int flag)
{
  if (flag)
  {
    TB0CCR0 = 1000;
    TB0CCTL0 = CCIE;
  }
  else
  {
    TB0CCR0 = 0;
    TB0CCTL0 = 0;
  }

  // This is basically a fan reset, so this ensures that the fans
  // aren't spinning by accident when we don't want them to.
  P1OUT |= 0x0C;
}

// Reads 256 samples from the ADC, which will read the value from the requested IR
// in IR_CHAR. This sets ADC_sum to the average value of the these samples.
void pollIRSensor(void)
{
  UCA1IE &= ~UCRXIE;                         // disable USCI_A0 RX interrupt

  switch (IR_CHAR)
  {
    case UART_IR_DOWN:
    {
      P1OUT &= ~0x40;
      PJOUT |= 0x02;
      PJOUT &= ~0x01;
      break;
    }
    case UART_IR_LEFT:
    {
      P1OUT |= 0x40;
      PJOUT &= ~0x02;
      PJOUT |= 0x01;
      break;
    }
    case UART_IR_RIGHT:
    {
      P1OUT &= ~0x40;
      PJOUT &= ~0x02;
      PJOUT |= 0x01;
      break;
    }
    case UART_IR_FORWARD:
    {
      P1OUT &= ~0x40;
      PJOUT &= ~0x03;
      break;
    }
    case UART_IR_BACK:
    {
      P1OUT |= 0x40;
      PJOUT &= ~0x03;
      break;
    }
  }

  ADC_sum = 0;
  ADC_count = 0;
  int i;
  for(i = 0; i< 256; i++)
  {
    P2OUT |= 0x01;                          // enable IR

    // wait a while
    __delay_cycles(10000);

    ADC10CTL0 |= ADC10SC;                   // Start sampling/conversion
    __bis_SR_register(LPM0_bits + GIE);

    P2OUT &= ~0x01;                         // disable IR

     // wait a while
    __delay_cycles(10000);

    ADC10CTL0 |= ADC10SC;                   // Start sampling/conversion
    __bis_SR_register(LPM0_bits + GIE);
  }

  ADC_sum >>= 8;

  UCA1IE |= UCRXIE;                         // Enable USCI_A0 RX interrupt
}

// Makes the blimp go in the given direction at the given speed. Direction must be one
// of the UART direction commands defined in virtual_com_cmds.h (UART_FORWARD, etc.).
// Speed must be between 0 and 4, with 0 indicating that the motor should be stopped.
void setMotors(char direction, int speed)
{
  switch (direction)
  {
    case UART_FORWARD:
    {
      TA0CCR2 = 0;
      TA0CCR4 = 0;

      if (TA0CCR1 < 5000)
      {
        speed *= 1000;
        TA0CCR1 = speed;
        TA0CCR3 = speed;
      }

      break;
    }
    case UART_BACKWARD:
    {
      TA0CCR1 = 0;
      TA0CCR3 = 0;

      if(TA0CCR2 < 5000)
      {
        speed *= 1000;
        TA0CCR2 = speed;
        TA0CCR4 = speed;
      }

      break;
    }
    case UART_LEFT:
    {
      speed *= 1000;

      TA0CCR1 = 0;
      TA0CCR2 = 0;
      TA0CCR3 = 0;
      TA0CCR4 = 0;
      TA0CCR2 = speed;
      TA0CCR3 = speed;
      break;
    }
    case UART_RIGHT:
    {
      speed *= 1000;
      TA0CCR1 = 0;
      TA0CCR2 = 0;
      TA0CCR3 = 0;
      TA0CCR4 = 0;
      TA0CCR1 = speed;
      TA0CCR4 = speed;
      break;
    }
    case UART_UP:
    {
      gDownSpeed = 0;

      if (gUpSpeed < 5)
        gUpSpeed = speed;

      break;
    }
    case UART_DOWN:
    {
      gUpSpeed = 0;

      if (gDownSpeed < 5)
        gDownSpeed = speed;

      break;
    }
  }
}

// Assumes that num is in base 10, and assumes that bufferSize is (a) the size of
// str, and (b) big enough to hold the string representation of the given number.
void itoa(long num, char *str, int bufferSize)
{
  char *result = str;

  // Only do conversion if we have a non-null string
  if (result)
  {
    bool negative = false;

    // If the number is negative, we want to add the minus sign after
    // conversion is done.
    if (num < 0)
    {
      negative = true;
      num = -num;
    }

    // We build the string backward, which makes our math easier. So we
    // start by writing the null byte at the end of the string.
    result += bufferSize - 1;
    *result = 0;

    if (num == 0)
    {
      // Since we normally use 0 to signal that we've run out of digits in
      // the number, we first handle the special case where the number is
      // actually a 0.
      result--;
      *result = DigitToChar(num);
    }
    else
    {
      // Keep chopping the ones digit off and append it to end of the string
      while (num > 0)
      {
        // We decrement before writing so result points to the first valid
        // character when we're done building the string.
        result--;
        *result = DigitToChar(num % 10);

        num /= 10;
      }
    }

    // Add minus sign to beginning if number was negative
    if (negative)
    {
      result--;
      *result = '-';
    }
  }

  // Shift the string over so the first digit is at the start of the string
  while (*result)
    *str++ = *result++;

  *str = 0;  // Copy null byte into end
}

// Simple function to encode a digit into an ASCII character
char DigitToChar(unsigned int num)
{
  return (num > 9) ? '9' : (char)num + '0';
}

#pragma vector=USCI_A1_VECTOR
__interrupt void USCI_A1_ISR(void)
{
  static char direction = 0;
  char ch = UCA1RXBUF;

  if (ch == UART_DEACTIVATE)
  {
    gActive = 0;
    setVerticalFanEnabled(0);
  }
  else if (ch == UART_ACTIVATE)
  {
    gActive = 1;
    setVerticalFanEnabled(1);
  }

  if (!gActive)
    return;

  if (direction)
  {
    // We send the speed as a single digit, so if we are waiting for a speed,
    // then once we receive a char, we just stop processing data and set the speed.
    setMotors(direction, ch - '0');
    direction = 0;
  }
  else
  {
    switch(ch)
    {
      case UART_RESET_MOTORS:
      {
        TA0CCR1 = 0;
        TA0CCR2 = 0;
        TA0CCR3 = 0;
        TA0CCR4 = 0;
        gUpSpeed = 0;
        gDownSpeed = 0;
        break;
      }
      case UART_VERT_ONLY:
      {
        TA0CCR1 = 0;
        TA0CCR2 = 0;
        TA0CCR3 = 0;
        TA0CCR4 = 0;
        break;
      }
      case UART_LEFT:
      case UART_RIGHT:
      {
        setMotors(ch, 5);
        break;
      }
      case UART_FORWARD:
      case UART_BACKWARD:
      case UART_UP:
      case UART_DOWN:
      {
        direction = ch;
        break;
      }
      case UART_IR_DOWN:
      case UART_IR_LEFT:
      case UART_IR_RIGHT:
      case UART_IR_FORWARD:
      case UART_IR_BACK:
      {
        IR_CHAR = ch;
        break;
      }
      // autonomous altitude control
      case UART_AUTOHEIGHT:
        if (!gAutoHeight) {
          gAutoHeight = true;
        } else {
          gAutoHeight = false;
        }
        break;
      case UART_SET_AUTOHEIGHT:
        gGetSetPoint = true;
        break;
    }
  }
}

//
// Interrupt vectors
//

// Sample the ADC to read the IR sensor values
#pragma vector=ADC10_VECTOR
__interrupt void ADC10_ISR(void)
{
  int val = ADC10MEM0;

  if (ADC_count % 2 == 0)
    ADC_sum -= val;
  else
    ADC_sum += val;

  ADC_count++;
  __bic_SR_register_on_exit(CPUOFF);
}

// Timer B interrupt to emulate PWM in software for the vertical motor
#pragma vector=TIMERB0_VECTOR
__interrupt void Timer_B(void)
{
  unsigned int pin = (gUpSpeed == 0) ? 0x02 : 0x01;

  if (counter < gUpSpeed || counter < gDownSpeed)
    P1OUT &= ~pin;
  else
    P1OUT |= pin;

  // Continuously keep the other pin high
  P1OUT |= (3 - pin);

  counter++;

  if (counter >= 6)
    counter = 0;
}