*/
*  Written by Charlie Roberts 2006
*  HMC6352_read_heading function taken from:
*  https://www.ccsinfo.com/forum/viewtopic.php?t=27179&highlight=hmc6352
*
*  target for the PIC 18F452 using the CCS PIC C Compiler
*
*/

#include <18F452.H>
#device ADC=8
#fuses EC_IO, NOWDT, NOPROTECT, BROWNOUT, PUT, NOLVP
#use delay(clock=20000000)
#use rs232(stream=TERM, baud=9600, xmit=PIN_D2, rcv=PIN_D3,PARITY=N, BITS=8,INVERT)
#use I2C (master, sda=PIN_C3, scl=PIN_D0, FORCE_HW)

#define HMC6352_I2C_WRITE_ADDRESS 0x42
#define HMC6352_I2C_READ_ADDRESS  0x43
#define HMC6352_GET_DATA_COMMAND 0x41

// Read the compass heading.  This is in units
// of 1/10's of a degree, from 0 to 3599.
int16 HMC6352_read_heading(void)
{
  int8 lsb;
  int8 msb;

  i2c_start();
  i2c_write(HMC6352_I2C_WRITE_ADDRESS);
  i2c_write(HMC6352_GET_DATA_COMMAND);
  i2c_stop();

  delay_ms(7);  // Allow 1 ms extra for safety

  i2c_start();
  i2c_write(HMC6352_I2C_READ_ADDRESS);
  msb = i2c_read();
  lsb = i2c_read(0);
  i2c_stop();

  return (((int16)lsb | ((int16)msb << 8)) / 10);
}

int16 xTiltChange(void) {
  int16 xHeadingChangeAmount;
  unsigned int16 xTilt;
  int16 xTiltAmount;

  set_adc_channel(3);
  delay_us(30);

  xTilt = read_adc();
  fprintf(TERM, "X tilt = %lu || ", xTilt);

  if(xTilt > 128) {
     xTiltAmount = 128 - (255 - xTilt);
     if(xTiltAmount > 1) {
        xHeadingChangeAmount = xTiltAmount * -2.1;
     }else{
        xHeadingChangeAmount = 0;
     }
  }else{
     xTiltAmount = 128 - xTilt;
     xHeadingChangeAmount = xTiltAmount * 2.1;
  }

  fprintf(TERM, "X Tilt Amount =%lu || ", xTiltAmount);
  fprintf(TERM, "X HeadingChangeAmount = %li\n\r", xHeadingChangeAmount);

  return xHeadingChangeAmount;
}

int16 yTiltChange(void) {
  int16 yHeadingChangeAmount;
  unsigned int16 yTilt;
  int16 yTiltAmount;

  set_adc_channel(2);
  delay_us(30);

  yTilt = read_adc();
  fprintf(TERM, "Y tilt = %lu || ", yTilt);

  if(yTilt < 128) {
     yTiltAmount = 128 - yTilt;
     if(yTiltAmount > 1) {
        yHeadingChangeAmount = yTiltAmount * 1.75 ;
     }else{
        yHeadingChangeAmount = 0;
     }
  }else{
     yTiltAmount = 128 - (255 - yTilt);
     yHeadingChangeAmount = yTiltAmount * -1;
  }
  if(yTiltAmount == 0 || yTiltAmount == 1 || yTiltAmount == -1) {
     output_high(PIN_D3);
  }
  fprintf(TERM, "Y TiltAmount =%lu || ", yTiltAmount);
  fprintf(TERM, "Y HeadingChangeAmount = %li\n\r", yHeadingChangeAmount);
  return yHeadingChangeAmount;
}

int16 getStartHeading() {
  int16 startHeading;

  startHeading = HMC6352_read_heading();
  fprintf(TERM, "Heading in degrees = %lu\n\r", startHeading);
  startHeading += yTiltChange();
  startHeading += xTiltChange();

  if(startHeading > 359) {
     startHeading = startHeading - 360;
  }else if (startHeading < 0) {
     startHeading = 360 + startHeading;
  }

  fprintf(TERM, "\n\r\n\r");
  fprintf(TERM, "=====================================================\n\r");
  fprintf(TERM, "Thank you. Your front center position is %li degrees.\n\r", startHeading);
  fprintf(TERM, "=====================================================\n\r");
  fprintf(TERM, "\n\r\n\r");

  return startHeading;
}
//===================================

void main() {
  int16 heading;
  int16 finalHeading;
  int16 centerHeading;
  int1 isPressed;

  setup_adc_ports(ALL_ANALOG);
  setup_adc( ADC_CLOCK_INTERNAL );

  output_high(PIN_D1);

  fprintf(TERM, "Please orient the controller towards the front center of your space.");
  fprintf(TERM, "Make sure the controller is level and press the fire button.\n\r\n\r");

  while(!input(PIN_D5));

  centerHeading = getStartHeading();
  delay_ms(2000);

  isPressed = 0;

  while(1) {
     if(input(PIN_D5) && isPressed == 0) {
        isPressed = 1;

        heading = HMC6352_read_heading();
        fprintf(TERM, "Heading in degrees = %lu\n\r", heading);

        finalHeading = heading + yTiltChange();
        finalHeading += xTiltChange();
        fprintf(TERM, "1: final heading before center adjustment = %li\n\r", finalHeading);
        finalHeading -= centerHeading;
        fprintf(TERM, "2: final heading after center adjustment = %li\n\r", finalHeading);
        if(finalHeading > 359 && (finalHeading >> 15) != 1) {
           fprintf(TERM, "3: final heading wrapped to 0+   = %li\n\r", finalHeading);
           finalHeading = finalHeading - 360;
        }else if ((finalHeading >> 15) == 1) {
           fprintf(TERM, "4: final heading wrapped to 360-   = %li\n\r", finalHeading);
           finalHeading = 360 + finalHeading;
        }

        /*if(finalHeading > 359) {
           finalHeading = abs(finalHeading) - 360;
        }*/

        fprintf(TERM, "FINAL HEADING = %li\n\r\n\r", finalHeading);

        //delay_ms(2000);
     }else if(!input(PIN_D5)) {
        //fprintf(TERM, "button released");
        isPressed = 0;
     }
  }
}