/*
  SFBPins.cpp - Platform-independent part of the programmer visible 'pins' in SFB
  Copyright (C) 2008 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 "SFBReflex.h" /* for reflex_maybe_dispatch() */
#include "SFBWiring.h" /* for pinMode() */
#include "SFBAssert.h" /* for BUG_UNIMPLEMENTED */

/* Bit map of which pins the sketch is controlling */
u32 pinSketchMap[PIN_SKETCH_MAP_WORDS] IN_BODYRAM_BSS;

PinInfo_t pinInfoMap[VIRTUAL_PIN_COUNT] = {   /* Lives in ROM!  const goes into section .rodata! */
  { 0, 0,NORTH_RX_PIN, 0},      /* ARDUINO_RX */
  { 0, 0,NORTH_TX_PIN, 0},      /* ARDUINO_TX */
  { 0, 0,NORTH_D0_PIN, 0},      /* ARDUINO_DIGITAL_PIN2 */
  { 0, 0,NORTH_D1_PIN, 0},      /* ARDUINO_DIGITAL_PIN3 */
  { 0, 0,NORTH_D2_PIN, 0},      /* ARDUINO_DIGITAL_PIN4 */
  { 0, 0,NORTH_D3_PIN, 0},      /* ARDUINO_DIGITAL_PIN5 */
  { 0, 0,SOUTH_D0_PIN, 0},      /* ARDUINO_DIGITAL_PIN6 */
  { 0, 0,SOUTH_D1_PIN, 0},      /* ARDUINO_DIGITAL_PIN7 */
  { 0, 0,SOUTH_D2_PIN, 0},      /* ARDUINO_DIGITAL_PIN8 */
  { 0, 0,SOUTH_D3_PIN, 0},      /* ARDUINO_DIGITAL_PIN9 */
  { 0, 0,EAST_D0_PIN, 0},       /* ARDUINO_DIGITAL_PIN10 */
  { 0, 0,EAST_D1_PIN, 0},       /* ARDUINO_DIGITAL_PIN11 */
  { 0, 0,EAST_D2_PIN, 0},       /* ARDUINO_DIGITAL_PIN12 */
  { 0, 0,BODY_RGB_RED_PIN, 0},  /* ARDUINO_DIGITAL_PIN13 */

  { 0, 0,NORTH_ISENSE_PIN, 0},  /* ARDUINO_ANALOG_PIN1 */
  { 0, 0,SOUTH_ISENSE_PIN, 0},  /* ARDUINO_ANALOG_PIN2 */
  { 0, 0,EAST_ISENSE_PIN, 0},   /* ARDUINO_ANALOG_PIN3 */
  { 0, 0,WEST_ISENSE_PIN, 0},   /* ARDUINO_ANALOG_PIN4 */
  { 0, 0,NORTH_ISENSE_PIN, 0},  /* ARDUINO_ANALOG_PIN5 -- WHERE ARE THE EXTRA ANALOG PINS? */
  { 0, 0,SOUTH_ISENSE_PIN, 0},  /* ARDUINO_ANALOG_PIN6 -- WHERE ARE THE EXTRA ANALOG PINS? */

  /* NORTH */
  { 1, 0, INPUT, 0},            /* NORTH_D0_PIN */
  { 1, 1, INPUT, 0},            /* NORTH_D1_PIN */
  { 4,29, INPUT, 0},            /* NORTH_D2_PIN */
  { 4,28, INPUT, 0},            /* NORTH_D3_PIN */
  { 0, 3, HW_MODE, 0},          /* NORTH_RX_PIN */
  { 0, 2, HW_MODE, 0},          /* NORTH_TX_PIN */
  { 1,16, OUTPUT, 0},           /* NORTH_VIN_ENABLE_PIN */
  { 1,17, HW_MODE, 0},          /* NORTH_VOUT_ENABLE_PIN */
  { 0,23, INPUT, 0},            /* NORTH_ISENSE_PIN */
  { 2, 3, OUTPUT, 0},           /* NORTH_LED_PIN */
  { 0, 4, OUTPUT, 0},           /* NORTH_SIDE_LED_PIN */

  /* SOUTH */
  { 0,17, INPUT, 0},            /* SOUTH_D0_PIN */
  { 0,18, INPUT, 0},            /* SOUTH_D1_PIN */
  { 0,20, INPUT, 0},            /* SOUTH_D2_PIN */
  { 0,19, INPUT, 0},            /* SOUTH_D3_PIN */
  { 0,16, HW_MODE, 0},          /* SOUTH_RX_PIN */
  { 0,15, HW_MODE, 0},          /* SOUTH_TX_PIN */
  { 1,14, OUTPUT, 0},           /* SOUTH_VIN_ENABLE_PIN */
  { 1,15, HW_MODE, 0},          /* SOUTH_VOUT_ENABLE_PIN */
  { 0,24, INPUT, 0},            /* SOUTH_ISENSE_PIN */
  { 2, 4, OUTPUT, 0},           /* SOUTH_LED_PIN */
  { 0, 5, OUTPUT, 0},           /* SOUTH_SIDE_LED_PIN */

  /* EAST */
  { 2,13, INPUT, 0},            /* EAST_D0_PIN */
  { 1,24, INPUT, 0},            /* EAST_D1_PIN */      
  { 1,25, INPUT, 0},            /* EAST_D2_PIN */
  { 1,26, INPUT, 0},            /* EAST_D3_PIN */
  { 0,11, HW_MODE, 0},          /* EAST_RX_PIN */
  { 0,10, HW_MODE, 0},          /* EAST_TX_PIN */
  { 1, 9, OUTPUT, 0},           /* EAST_VIN_ENABLE_PIN */
  { 1,10, HW_MODE, 0},          /* EAST_VOUT_ENABLE_PIN */
  { 0,25, INPUT, 0},            /* EAST_ISENSE_PIN */
  { 2, 5, OUTPUT, 0},           /* EAST_LED_PIN */
  { 2, 6, OUTPUT, 0},           /* EAST_SIDE_LED_PIN */

  /* WEST */
  { 1,20, INPUT, 0},            /* WEST_D0_PIN */
  { 1,19, INPUT, 0},            /* WEST_D1_PIN */
  { 1,23, INPUT, 0},            /* WEST_D2_PIN */
  { 1,21, INPUT, 0},            /* WEST_D3_PIN */
  { 0, 1, HW_MODE, 0},          /* WEST_RX_PIN */
  { 0, 0, HW_MODE, 0},          /* WEST_TX_PIN */
  { 1, 4, OUTPUT, 0},           /* WEST_VIN_ENABLE_PIN */
  { 1, 8, HW_MODE, 0},          /* WEST_VOUT_ENABLE_PIN */
  { 0,26, INPUT, 0},            /* WEST_ISENSE_PIN */
  { 2,12, OUTPUT, 0},           /* WEST_LED_PIN */
  { 2, 7, OUTPUT, 0},           /* WEST_SIDE_LED_PIN */

  /* BODY */
  { 1,29, INPUT, 0},            /* BODY_SWITCH0_PIN */
  { 2, 1, OUTPUT, 0},           /* BODY_RGB_RED_PIN */
  { 2, 0, OUTPUT, 0},           /* BODY_RGB_GREEN_PIN */
  { 2, 2, OUTPUT, 0},           /* BODY_RGB_BLUE_PIN */

  { 0, 6, OUTPUT, 0},           /* BODY_EEPROM_SSEL1_PIN */
  { 0, 7, OUTPUT, 0},           /* BODY_EEPROM_SCK1_PIN */
  { 0, 8, INPUT, 0},            /* BODY_EEPROM_MISO1_PIN */
  { 0, 9, OUTPUT, 0},           /* BODY_EEPROM_MOSI1_PIN */

  { 1,30, INPUT, 0},            /* BODY_AMUX_READ_PIN */
  { 3,26, OUTPUT, 0},           /* BODY_AMUX_SELA_PIN */
  { 3,25, OUTPUT, 0},           /* BODY_AMUX_SELB_PIN */
  { 0,21, OUTPUT, 0}            /* BODY_AMUX_SELC_PIN */
};

void reflexDigitalWrite(int sfbPin, int value) {
  if (!GET_SKETCH_FLAG(sfbPin))
    implDigitalWrite(sfbPin,value);
}

void implDigitalWrite(int sfbPin, int value) {
  struct PinInfo_s info;
  u32 port, mask;
  SFB_GET_PIN_PORT_AND_MASK(sfbPin,info,port,mask);
  SFB_PORT_WRITE_BIT_MASK(port,mask,value);
}

int reflexDigitalRead(int sfbPin) {
  if (!GET_SKETCH_FLAG(sfbPin))
    return implDigitalRead(sfbPin);
  else
    return HIGH;
}

int implDigitalRead(int sfbPin) {
  struct PinInfo_s info;
  u32 port, mask;
  SFB_GET_PIN_PORT_AND_MASK(sfbPin,info,port,mask);
  return SFB_PORT_GET_BIT_MASK(port,mask);
}

void reflexPinMode(int sfbPin, int mode) {
  if (!GET_SKETCH_FLAG(sfbPin))
    return implPinMode(sfbPin,mode);
}

void implPinMode(int sfbPin, int mode) {
  struct PinInfo_s info;
  u32 port, mask;
  SFB_GET_PIN_PORT_AND_MASK(sfbPin,info,port,mask);

  if (mode==INPUT)
    SFB_PORT_DIR_REGISTER(port) &= ~mask;
  else
    SFB_PORT_DIR_REGISTER(port) |= mask;
}

void reflexSetPinResistor(int sfbPin, int pullupDownCode) {
  if (!GET_SKETCH_FLAG(sfbPin))
    return implSetPinResistor(sfbPin,pullupDownCode);
}

void implSetPinResistor(int sfbPin, int pullupDownCode) {
  struct PinInfo_s info;
  u32 port, mask;
  SFB_GET_PIN_PORT_AND_MASK(sfbPin,info,port,mask);

  int bitPairPos = info.bitNumber<<1;    /* Access bit#-within-port */
  int offset = port*0x8;                 /* Set up to find the right PINMODE reg */

  if (bitPairPos >= 32) {                /* If in second half of register */
    offset += 4;                         /* need to look at the next higher PINMODE */
    bitPairPos -= 32;                    /* and adjust the bit pair position. */
  }
  mask = 0x3<<bitPairPos;                /* Find the bits we want */

  u32 val = SFB_PORT_PINMODE_REGISTER(offset)&~mask; /* get register without old bits */
  SFB_PORT_PINMODE_REGISTER(offset) = val|(pullupDownCode<<bitPairPos);  /* insert new ones */
}


/**
 * debugRGB* -- Low-level last ditch output signallers.
 *
 */
void debugRGBNum(int num, int bits, int loops) {
  while (loops-- != 0) {
    int b;
    debugRGB(1,2);               /* blue startup */
    for (b = 1<<(bits-1); b != 0; b >>= 1) {
      debugRGB(1,1);             /* blue palate cleanser */
      if (num&b) debugRGB(4,2);  /* red is one */
      else debugRGB(2,2);        /* green is zero */
    }
  }
}


/* Actions needed to initialize on startup
 */
void pins_startup_initialization() {

  hwpins_startup_initialization();

  reflexLibraryFlags = 0;

  {
    int i;
    for (i = FIRST_SFB_PIN; i < VIRTUAL_PIN_COUNT; ++i) {
      pinMode(i,REFLEX_MODE);
    }
  }
}

const int faceBasePins[FACE_COUNT] = {
  NORTH_BASE_PIN, SOUTH_BASE_PIN, EAST_BASE_PIN, WEST_BASE_PIN
};

/* The painfully named.. */
int pinInFace(int facePin, int face) {
  API_ASSERT_VALID_FACE(face);
  API_ASSERT_MAX(facePin,FACE_PIN_COUNT);
  return faceBasePins[face]+facePin;
}

static const u8 basePins[] = { NORTH_BASE_PIN, SOUTH_BASE_PIN, EAST_BASE_PIN, WEST_BASE_PIN, WEST_BASE_PIN+FACE_PIN_COUNT };
bool isFacePin(int pin, u8 * face, u8 * facePin) {
  API_ASSERT_VALID_PIN(pin);

  struct PinInfo_s info;
  u32 port, mask;
  SFB_GET_PIN_PORT_AND_MASK(pin,info,port,mask); // May alter pin! (Dereferences Arduino aliases)
  for (u32 f = NORTH; f <= WEST; ++f) {
    if (pin >= basePins[f] && pin < basePins[f+1]) {
      if (face) *face = f;
      if (facePin) *facePin = pin-basePins[f];
      return true;
    }
  }
  return false;
}
