//--------------------------------------------------------------------------

//---------------------------
//    Vendor Supplied
//---------------------------

#include  <msp430x20x3.h>

//---------------------------
//    Custom
//---------------------------
#define ENABLE_FET BIT7

static void Initialize_Processor(void);
static void Initialize_Ports(void);
static void initTimer_aPwm(void);
static void ClearWatchdog(void);
static void initA2d(void);
static void Delay_Time(long);


__interrupt void a2dISR(void);

static short control(short vOut, short iOut);


#define MIN_DUTY 0
#define MAX_DUTY (256)
#define V_SET 20000
#define MAX_V (short)(V_SET * 16L / 14)
#define MIN_V (short)(V_SET * 11L / 14)

#define V_17 (short)(V_SET * 17L / 14)

#define DWELL_D 2
#define DWELL_U 2

#define SEARCH_TIME 5000
#define SLOPE_TIME 100

static volatile short failSafeFlag;

//--------------------------------------------------------------------------
void main( void )
//--------------------------------------------------------------------------

{
      //Disable Global Interrupt
      _BIC_SR(GIE);
      
//      Delay_Time(20);    //Wait

      //Initialize Processor
      Initialize_Processor();

      //Initialize I/O Ports, ADC, and Timers
      Initialize_Ports();

      initTimer_aPwm();
       
      //Wait for P3V3 rail to stabilize
      Delay_Time(2000);    //Wait 50ms
      
     //Enable A2D Interrupt
      initA2d();

      //Enable Global Interrupt
      _BIS_SR(GIE);

       //Enable Watchdog Timer
      ClearWatchdog();

      P2OUT &= ~ENABLE_FET;

      while(1==1)
      {
        if(failSafeFlag++ < 10000)
          failSafeFlag = 10000;
        else
          ClearWatchdog();	//Endless main loop until reset
      }
        
}//end of main

//--------------------------------------------------------------------------
inline void Initialize_Processor(void)
//--------------------------------------------------------------------------
//This functions initializes the power processor (i.e clock module, watchdog
//, main system functions)

{
    //Disable watchdog function
    WDTCTL = (WDTPW + WDTHOLD);

    //Configure basic clock module for operation at 12 MHz from the DCO generator

    BCSCTL1 = CALBC1_16MHZ;               // Set DCO to factory calibrated
    DCOCTL = CALDCO_16MHZ;

}

//--------------------------------------------------------------------------
inline void Initialize_Ports(void)
//--------------------------------------------------------------------------
//This function configures ports for input/output and peripheral section.
{
  //Configure Port I/Os
  //Port 1

  P1DIR |= 0x20;                  //Configure Port as an output, initialize to logic low
  P1DIR |= 0x40;                  //TA1 out pwm
  P1OUT = 0x40;
  P1SEL |= (0x20);
  P1SEL |= 0x40;
//  P1IE |= SW_PWR_0;                     //Enable pin interrupt
//  P1IES |= SW_PWR_0;                    //Set for Hi -> Lo transition

  //Port 2
//  P2SEL &= ~(CHARGE_OK + RESET_PRI_0);  //Configure Port 2.6 and 2.7 as an input
//  P2DIR &= ~CHARGE_OK;
  P2SEL &= ~ENABLE_FET;
  P2DIR |= ENABLE_FET;
  P2OUT |= ENABLE_FET;
//  P2IES &= ~CHARGE_OK;                    //Set for Lo -> Hi transition
//  P2IE |= CHARGE_OK;                    //Enable pin interrupt

}


void initTimer_aPwm(void)
//--------------------------------------------------------------------------
//This function enables the secondary alarm at 400 Hz on the SEC_ALARM signal

{
      TACTL =
        (
         TASSEL_2                   /* Select SMCLK for source*/
         + MC_1                     /* Timer in UP mode */
        );

      TACCTL0 =
        (
         OUTMOD2                  /* Set output mode to toggle*/
        );
      
      TACCTL1 =
        (
         3<<5//OUTMOD2                  /* Set output mode to toggle/reset */
        );

      //SMCLK is 1MHz so no prescale is needed generate the 1.25us period for alarm
      TACCR0 = MAX_DUTY;                 /*Sets the period for Timer A PWM */
      TACCR1 = 0;                 /*Sets the off period for Timer A PWM */
//      AudioTimer = 0;
}


//--------------------------------------------------------------------------
static void ClearWatchdog(void)
//--------------------------------------------------------------------------
//This function clears the watchdog timer.
{
  //Clear watchdog timer
  WDTCTL = WDT_MRST_8 ;

}

//--------------------------------------------------------------------------
static void Delay_Time(long delay)
//--------------------------------------------------------------------------
//This function creates a delay based on the parameter passed.
//Due to the compiler optimization, the actual delay can vary based on the
//size of the parameter passed, i.e. the function is not linear, if the value
//is a char, short, or long
{
  for (volatile long i = 0; i < delay; i+=1) //SMCLK has 1us period
  {
    ClearWatchdog();
  }
}

//--------------------------------------------------------------------------

void initA2d(void)
{
    SD16CTL |=
    (
            SD16SSEL_0                                      /* MCLK */
        |   SD16REFON                                       /* Turn on internal reference */
        |   SD16XDIV_2                                      /* Divide by 1 */
    );

    SD16AE |= SD16AE1 | SD16AE2 | SD16AE0;                             /* Enable P1.0, P1.1 and P1.2 analog inputs */

    SD16INCTL0 |=
     (
            SD16INTDLY_0                                    /* Interrupt on the 4th sample (Allow filter to settle */
        +   SD16INCH_0                                      /* A0 input */
     );


    SD16CCTL0 |=
     (
            SD16OSR_1024                                    /* 1024 Oversampling */
        +   SD16IE                                          /* Enable the interrupt */
        +   SD16SC                                          /* Start conversion */
        +   SD16DF                                          /* 2's Complement output */
        +   SD16SNGL                                        /* Single Conversion */
     );
}

/* This interrupt is generated 244 times a seconds when a sample is completed by the A/D. */
#pragma vector = SD16_VECTOR
__interrupt void a2dISR(void)
{
  static char skip;
  short vOut;
  unsigned short duty;
  static char chan;
  static short offset;
  static short iOut;
  
  failSafeFlag = 0;

  if(SD16IV == 4)
  {
     switch(chan)
    {
        case 0:
          vOut = SD16MEM0 - offset - (iOut >> 4); // sub out the v drop on the conductors going to gnd and bat.
          
          SD16INCTL0 =
            (
                  SD16INTDLY_0                                    /* Interrupt on the 4th sample (Allow filter to settle */
              +   SD16INCH_4                                      /* Switch to input */
            );
//SD16AE |= SD16AE2 | SD16AE0;                             /* Enable P1.0, P1.1 and P1.2 analog inputs */
          
          duty = control(vOut, iOut);
          if(duty > MAX_DUTY) duty = MAX_DUTY;
          TACCR1 = MAX_DUTY - duty;
//          TACCR1 = duty;
          chan = 1;
 
        break;
        
       case 1:
          iOut = offset - SD16MEM0;
          
          if(++skip == 0)
          {
            SD16INCTL0 =
              (
                    SD16INTDLY_0                                    /* Interrupt on the 4th sample (Allow filter to settle */
                +   SD16INCH_7                                      /* Switch to input */
              );
            
            chan = 2;
          }
          else
          {
            SD16INCTL0 =
              (
                    SD16INTDLY_0                                    /* Interrupt on the 4th sample (Allow filter to settle */
                +   SD16INCH_0                                     /* Switch to input */
              );
           
            chan = 0;
          }
        break;
        
        case 2:
          offset = SD16MEM0;
//offset = 0;          
          SD16INCTL0 =
            (
                  SD16INTDLY_0                                    /* Interrupt on the 4th sample (Allow filter to settle */
              +   SD16INCH_0                                     /* Switch to input */
            );
         
          chan = 0;
        break;      
    }
    SD16CCTL0 |= SD16SC;
  }


}


short control(short vOut, short iOut)
{
  static short duty;
  static short count;
  static char searchFlag;
  static short searchTimer, delay;
  static short bestPower, startPower;
  static short bestDuty;
  static short lastBestDuty;
static signed char slopeDir;
static short slopeTimer;
static long powerAcc;
  short power, filPower;
  short maxDuty = MAX_DUTY;
  
  
  if(delay > 0)
  {
    delay -= 1;
  }
  
  power = iOut;
//    power = vOut;
  powerAcc += power;
  filPower = (powerAcc >> 6);
  powerAcc -= filPower;
  
  if(searchFlag)
  {
   
    if(delay == 0)
    {
      if(bestPower < power)
      {
        bestPower = power;
        bestDuty = duty;
        if(duty == 0) bestDuty = 1;
      }
    }
    else
    {
      startPower = power;
    }
    
    if(duty >= MAX_DUTY)
      searchFlag = 0;
  }
  else
  {
    if(bestDuty > 0)  // fist time after search is done.
    {
      short dif = lastBestDuty - bestDuty;
      
      if(dif < 0) dif = -dif;
      if(dif > ((lastBestDuty>>3) + 3))
        searchTimer = 1;        // do it again.
      
      lastBestDuty = bestDuty;
      duty = bestDuty;
      bestDuty = 0;
      
      dif = bestPower - startPower;
      
      if(dif < (bestPower>>2) || dif < 50)
      {
         lastBestDuty = 0;
         duty = 0;
         searchTimer = SEARCH_TIME/5;
      }
      
      slopeTimer = SLOPE_TIME;
      startPower = 0;
    }
    
//    if(lastBestDuty > 0)
    {
      maxDuty = lastBestDuty;
    }
    
    if(--searchTimer <= 0)
    {
      searchFlag = 1;
      searchTimer = SEARCH_TIME;
      bestPower = -32000;
      duty = duty/3;
      delay = 10;
    }
    else if(--slopeTimer <= 0)
    {
      if(filPower <= startPower)
      {
        if(slopeDir >= 0) slopeDir = -1;
        else slopeDir = 1;
//        duty += slopeDir;
      }
      slopeTimer = SLOPE_TIME;
      startPower = filPower;
      if(duty < (MAX_DUTY - 2) && duty > (MIN_DUTY))
      {
        duty += slopeDir;
      }
    }
  }
  
 P2OUT ^= ENABLE_FET;
         
  if(delay == 0)
  {
    if(vOut > MAX_V)
    {
      duty = 0;
      count = 0;
      searchFlag = 0;
    }
    else
    {
      if(vOut > V_SET)
      {
        count -= 1;
        searchTimer = SEARCH_TIME;
        slopeTimer = SLOPE_TIME;
        searchFlag = 0;
      }
      else
      {
        if(searchFlag)
          count += 1;
      }
    }
  }
  
  
  if(count > DWELL_U)
  {
    if(duty < maxDuty)
    {
      if(searchFlag)
        duty += 5;
      duty += 1;
    }
    count = 0;
  }
  else if(count < -DWELL_D)
  {
    duty -= 1;
    count = 0;
    searchFlag = 0;
}
  
  if(duty > MAX_DUTY) duty = MAX_DUTY;
  if(duty < MIN_DUTY) duty = MIN_DUTY;
   
  return duty;  
}

short old_control(short vOut, short iOut)
{
  static short duty;
  static short count;
  static char searchFlag;
  static short searchTimer, delay;
  static short bestPower, startPower;
  static short bestDuty;
  static short lastBestDuty;
  short power;
  short maxDuty = MAX_DUTY;
  
  
  if(delay > 0)
  {
    delay -= 1;
  }
  
  if(searchFlag)
  {
    power = iOut;
//    power = vOut;
    
    if(delay == 0)
    {
      if(bestPower < power)
      {
        bestPower = power;
        bestDuty = duty;
        if(duty == 0) bestDuty = 1;
      }
    }
    else
    {
      startPower = power;
    }
    
    if(duty >= MAX_DUTY)
      searchFlag = 0;
  }
  else
  {
    if(bestDuty > 0)  // fist time after search is done.
    {
      short dif = lastBestDuty - bestDuty;
      
      if(dif < 0) dif = -dif;
      if(dif > ((lastBestDuty>>3) + 3))
        searchTimer = 1;        // do it again.
      
      lastBestDuty = bestDuty;
      bestDuty = 0;
      
      dif = startPower - bestPower;
      if(dif < 0) dif = -dif;
      if(dif < (bestPower>>2))
      {
         lastBestDuty = 0;
         searchTimer = SEARCH_TIME;
      } 
    }
    
    if(lastBestDuty > 0)
    {
      maxDuty = lastBestDuty;
    }
    
    if(--searchTimer <= 0)
    {
      searchFlag = 1;
      searchTimer = SEARCH_TIME;
      bestPower = -32000;
      duty = duty/3;
      delay = 10;
    }  
  }
  
 P2OUT ^= ENABLE_FET;
         
  if(vOut > MAX_V)
  {
    duty = 0;
    count = 0;
    searchFlag = 0;
  }
  else
  {
    if(vOut > V_SET)
    {
      count -= 1;
      searchTimer = SEARCH_TIME;
      searchFlag = 0;
    }
    else
    {
      if(delay == 0)
        count += 1;
    }
  }
    
  if(count > DWELL_U)
  {
//    if(searchFlag)
//      duty += 4;
    duty += 1;
    count = 0;
  }
  else if(count < -DWELL_D)
  {
    duty -= 1;
    count = 0;
    searchFlag = 0;
}
  
  if(duty > maxDuty) duty = maxDuty;
  if(duty < MIN_DUTY) duty = MIN_DUTY;
   
  return duty;  
}


void __exit(int i);   //Custom exit function created to reduce code size of default exit function created by compiler
void __exit(int i){}