/*
  SFBPower.cpp - The power handling routines for the SFB.
  Copyright (C) 2009 The Regents of the University of New Mexico.  All rights reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA

  $Id$
*/

#include "SFBPower.h"
#include "SFBAssert.h"          /* For BUG, etc */
#include "SFBWiring.h"          /* For pinMode */
#include "SFBPins.h"            /* For FACE_VOUT_ENABLE_PIN */
#include "SFBBootBlock.h"       /* For getBootBlockPowerIn, etc */

#include "LPC23xx.h"		/* LPC23xx/24xx Peripheral Registers	*/

uv32 adcSamples[ADC_SAMPLE_COUNT] IN_FASTRAM = { 0, 0, 0, 0, 0 };

uv16 adcSmooth[ADC_SAMPLE_COUNT] IN_FASTRAM = { 556, 556, 556, 556, 556 };

uv16 adcSmoother[ADC_SAMPLE_COUNT] IN_FASTRAM = { 556, 556, 556, 556, 556 };


/**
 * Setup the ADC converters
 */
void initADC(void)
{ 
  u32 val;
  PCONP |= PCAD;     /* heat 'em up */

  /*              AD0.0   AD0.1   AD0.2   AD0.3 */
  val = PINSEL1&~((3<<14)|(3<<16)|(3<<18)|(3<<20));
  PINSEL1 = val |((1<<14)|(1<<16)|(1<<18)|(1<<20));
  
  /*              AD0.4   AD0.5 */
  val = PINSEL3&~((3<<28)|(3<<30));
  PINSEL3 = val |((1<<28)|(1<<30));
  
  AD0CR |= /* Power on          */ (   1 << 21) | 
           /* Channels 0.0-0.5  */ (0x3F <<  0);
  AD0CR |= (1 <<  16); /* burst mode */
}


int powerDraw() {
  int res = 0;
  for (u32 f = NORTH; f <= WEST; ++f) 
    res += powerDraw(f);
  return res;
}

int powerDraw(u32 face) {
  API_ASSERT_VALID_FACE(face);
  return -1800*(((long) adcSmoother[face])-556)/467;
}

int rawPower(u32 face) {
  API_ASSERT_VALID_FACE(face);
  return adcSmooth[face];
}

int railVoltage() {
  return 0;
  /* XXX BUG_UNIMPLEMENTED(); */
  /* return (adcSmoother[RAIL_SAMPLE_INDEX]*??+??)/??;  */
}

int rawVoltage() {
  return adcSmooth[RAIL_SAMPLE_INDEX];
}

void powerOut(u32 face, int on) {

  int pin = pinInFace(FACE_VOUT_ENABLE_PIN,face);

  pinMode(pin, REFLEX_MODE);        /* Take back control of the power pin */ 

  if (on) {

    pinMode(pin,INPUT);       /* To activate a Vout: Make an input..*/
    reflexSetPinResistor(pin, 0x2); /* ..and disable the pull up */

  } else {

    //    reflexSetPinResistor(pin, 0x0);   /* (Reestablish pull up resistor?) */
    pinMode(pin,OUTPUT);      /* To deactivate a Vout: Make it an output..*/
    reflexDigitalWrite(pin, LOW);   /* ..and ground it */

  }
}

/** Control inbound power -- NOTE THIS CODE IS UNTESTED! */

void powerIn(u32 face, int on) {

  int pin = pinInFace(FACE_VIN_ENABLE_PIN,face);

  pinMode(pin, REFLEX_MODE);        /* Take back control of the power pin */ 

  if (on) {

    pinMode(pin,INPUT);       /* To activate a Vin: Make an input..*/
    reflexSetPinResistor(pin, 0x2); /* ..and disable the pull up */

  } else {

    //    reflexSetPinResistor(pin, 0x0);   /* (Reestablish pull up resistor?) */
    pinMode(pin,OUTPUT);      /* To deactivate a Vin: Make it an output..*/
    reflexDigitalWrite(pin, LOW);   /* ..and ground it */

  }
}

void initPowerOuts() {
  for (u32 f = NORTH; f <= WEST; ++f) 
    powerOut(f,getBootBlockPowerOut(f));
  for (u32 f = NORTH; f <= WEST; ++f) 
    powerIn(f,getBootBlockPowerIn(f));
}
