//-----------------------------------------------------------------------------
// Solar Tracker firmware - MidnightMaker 2013
//-----------------------------------------------------------------------------
// Interfacing with the Compass and Accelerometer LSM303DLM PCB
// Uses SX1509 I/O Extender to drive the stepper motors for tracking
server.show("CompassAccelSolar");
 
const logEnabled    = 1;

// External SmartPhone can disable autotracking and manually drive
// the elevation and azimuth axis
local autoTrack = 0;

const A_FILTER                  = 1.0;
const M_FILTER                  = 1.0;

const INSTALLATION_ROLL_ANGLE   = 48.0;   // Angle between the Panel and the Accelerometer as installed.
const AZIMUTH_AXIS_RESOLUTION   = 2.22222 // 800.0/360.0 Steps per degree. Gearbox is 4:1 and stepper is 200 steps/revolution
const ELEVATION_AXIS_RESOLUTION = 787.87; // Steps per degree. This will need to be determined based on the length of the lever, fulcrum distance etc. Lead screw is 13 TPI
                                          // Since there are a bunch of circle arcs involved, I expect the function to be non linear which
                                          // is too much for my limited brain to handle. I will thus resolve this empirically at some point
                                          
const PI                        =  3.141592653589;
const TWOPI                     =  6.283185307179;
const RAD                       =  0.017453292519; // PI/180.0;
const TO_DEG                    =  57.29577951308
const dEarthMeanRadius          =  6371.01     // In km
const dAstronomicalUnit         =  149597890.0 // In km

// Output ports for sending data to COSM
local compassPort       = OutputPort("compass","string");
local headingPort       = OutputPort("heading");
local rollPort          = OutputPort("roll");
local pitchPort         = OutputPort("pitch");
local sunAzimuthPort    = OutputPort("sunAzimuth", "number");
local sunZenithPort     = OutputPort("sunZenith", "number");

// Variable for geographic location to be provided by SmartPhone
udtLocationdLongitude  <- -78.7814;
udtLocationdLatitude   <- 35.7914;
cycleCount             <- 0;
headingCalibrate       <- 0.0;  // 
// This is the calculated output. Zenith is angle down from vertical
udtSunCoordinatesdZenithAngle  <- 17.54; // Elevation Angle = 90 - Zenith
udtSunCoordinatesdAzimuth      <- 234.32;

//--------------------------------------------------------------------------------------------------------
// Algorithm in C++ available from http://www.psa.es/sdg/sunpos.htm
// Test online version at http://pvcdrom.pveducation.org/SUNLIGHT/sunPSA.HTM
// Converted to Squirrel May 2013
//
// See http://www.science.smith.edu/~jcardell/Courses/EGR100/protect/slides/L18.pdf for a simpler model
// Simpler version also at pvcdrom (google it!)
//
//--------------------------------------------------------------------------------------------------------
function sunpos( )
{
    // Read RTC 
    // Date function returns GMT (UTC)time. We are 4 hours behind GMT
    
    local dateTime = date();
    
    // Month is zero referenced 0-11
    dateTime.month = dateTime.month + 1;
    
    
    server.log(format("YEAR %d MONTH %d DAY %d   HOUR %d MIN %d SEC %d",dateTime.year,dateTime.month,dateTime.day, dateTime.hour, dateTime.min, dateTime.sec) );
	// Main variables
	local dElapsedJulianDays    = 0.000000000;
	local dDecimalHours         = 0.000000000;
	local dEclipticLongitude    = 0.000000000;
	local dEclipticObliquity    = 0.000000000;
	local dRightAscension       = 0.000000000;
	local dDeclination          = 0.000000000;

	// Auxiliary variables
	local dY                    = 0.000000000;
	local dX                    = 0.000000000

	// Calculate difference in days between the current Julian Day 
	// and JD 2451545.0, which is noon 1 January 2000 Universal Time
	{
		local dJulianDate       = 0.000000000
		local liAux1;
		local liAux2;
		// Calculate time of the day in UT decimal hours
		dDecimalHours = dateTime.hour + (dateTime.min 
			+ dateTime.sec / 60.0 ) / 60.0;
		// Calculate current Julian Day
		liAux1 =((dateTime.month+1)-14)/12; // Squirrel month is 0 to 11
		liAux2=(1461*(dateTime.year + 4800 + liAux1))/4 + (367*(dateTime.month 
			- 2-12*liAux1))/12- (3*((dateTime.year + 4900 
		+ liAux1)/100))/4+dateTime.day-32075;
		dJulianDate=(liAux2)-0.5+dDecimalHours/24.0;
		// Calculate difference between current Julian Day and JD 2451545.0 
		dElapsedJulianDays = dJulianDate-2451545.0;
        if( logEnabled == 1 )
            server.log(format("dElapsedJulianDays %f",dElapsedJulianDays));
	}

	// Calculate ecliptic coordinates (ecliptic longitude and obliquity of the 
	// ecliptic in radians but without limiting the angle to be less than 2*Pi 
	// (i.e., the result may be greater than 2*Pi)
	{
		local dMeanLongitude    = 0.000000000;
		local dMeanAnomaly      = 0.000000000;
		local dOmega            = 0.000000000;
		dOmega=2.1429-0.0010394594*dElapsedJulianDays;
		dMeanLongitude = 4.8950630+ 0.017202791698*dElapsedJulianDays; // Radians
		dMeanAnomaly = 6.2400600+ 0.0172019699*dElapsedJulianDays;
		dEclipticLongitude = dMeanLongitude + 0.03341607*math.sin( dMeanAnomaly ) 
			+ 0.00034894*math.sin( 2*dMeanAnomaly )-0.0001134
			-0.0000203*math.sin(dOmega);
		dEclipticObliquity = 0.4090928 - 6.2140e-9*dElapsedJulianDays
			+0.0000396*math.cos(dOmega);
        if( logEnabled == 1 )
            server.log(format("dEclipticObliquity %f",dEclipticObliquity));
	}

	// Calculate celestial coordinates ( right ascension and declination ) in radians 
	// but without limiting the angle to be less than 2*Pi (i.e., the result may be 
	// greater than 2*Pi)
	{
		local dSin_EclipticLongitude = 0.000000000;
		dSin_EclipticLongitude= math.sin( dEclipticLongitude );
		dY = math.cos( dEclipticObliquity ) * dSin_EclipticLongitude;
		dX = math.cos( dEclipticLongitude );
		dRightAscension = math.atan2( dY,dX );
		if( dRightAscension < 0.0 ) 
      dRightAscension = dRightAscension + TWOPI;
		dDeclination = math.asin( math.sin( dEclipticObliquity )*dSin_EclipticLongitude );
        if( logEnabled == 1 )
            server.log(format("dDeclination %f",dDeclination));
	}

	// Calculate local coordinates ( azimuth and zenith angle ) in degrees
	{
		local dGreenwichMeanSiderealTime    = 0.000000000;
		local dLocalMeanSiderealTime        = 0.000000000;
		local dLatitudeInRadians            = 0.000000000;
		local dHourAngle                    = 0.000000000;
		local dCos_Latitude                 = 0.000000000;
		local dSin_Latitude                 = 0.000000000;
		local dCos_HourAngle                = 0.000000000;
		local dParallax                     = 0.000000000;

		dGreenwichMeanSiderealTime = 6.6974243242 + 
			0.0657098283*dElapsedJulianDays 
			+ dDecimalHours;
		dLocalMeanSiderealTime = (dGreenwichMeanSiderealTime*15 
			+ udtLocationdLongitude)*RAD;
		dHourAngle = dLocalMeanSiderealTime - dRightAscension;
		dLatitudeInRadians = udtLocationdLatitude*RAD;
		dCos_Latitude = math.cos( dLatitudeInRadians );
		dSin_Latitude = math.sin( dLatitudeInRadians );
		dCos_HourAngle= math.cos( dHourAngle );
		udtSunCoordinatesdZenithAngle = (math.acos( dCos_Latitude*dCos_HourAngle
			*math.cos(dDeclination) + math.sin( dDeclination )*dSin_Latitude));
		dY = -math.sin( dHourAngle );
		dX = math.tan( dDeclination )*dCos_Latitude - dSin_Latitude*dCos_HourAngle;
		udtSunCoordinatesdAzimuth = math.atan2( dY, dX );
		if ( udtSunCoordinatesdAzimuth < 0.0 ) 
			udtSunCoordinatesdAzimuth = udtSunCoordinatesdAzimuth + TWOPI;
		udtSunCoordinatesdAzimuth = udtSunCoordinatesdAzimuth/RAD;
		// Parallax Correction
		dParallax=(dEarthMeanRadius/dAstronomicalUnit)
			*math.sin(udtSunCoordinatesdZenithAngle);
		udtSunCoordinatesdZenithAngle=(udtSunCoordinatesdZenithAngle 
			+ dParallax)/RAD;
        if( logEnabled == 1 )
        {
            server.log(format("udtSunCoordinatesdZenithAngle %f",udtSunCoordinatesdZenithAngle));
            server.log(format("udtSunCoordinatesdAzimuth %f",udtSunCoordinatesdAzimuth));
        }
	}
}

//--------------------------------------------------------------------------------------------------------
// Base class for an I2C port. Refactored from 
// http://devwiki.electricimp.com/doku.php?id=codewebcolor&s[]=sx1509
// and added function for readMulti to support the LSM303 chip
//--------------------------------------------------------------------------------------------------------
class I2C
{
    i2cPort     = null;
    i2cAddress  = null;
    
    constructor(port, address)
    {
        if(port == I2C_12)
        {
            // Configure I2C bus on pins 1 & 2
            hardware.configure(I2C_12);
            // set the I2C clock speed. We can do 10 kHz, 50 kHz, 100 kHz, or 400 kHz
            hardware.i2c12.configure(CLOCK_SPEED_400_KHZ);
            i2cPort = hardware.i2c12;
            server.log("Configured Port 12.");
        }
        else if(port == I2C_89)
        {
            // Configure I2C bus on pins 8 & 9
            server.log("Configured Port 89.");
            hardware.configure(I2C_89);
            // set the I2C clock speed. We can do 10 kHz, 50 kHz, 100 kHz, or 400 kHz
            hardware.i2c89.configure(CLOCK_SPEED_400_KHZ);
            i2cPort = hardware.i2c89;
        }
        else
        {
            server.log("Invalid I2C port specified.");
        }
 
        i2cAddress = address << 1;
    }
    
    // Read a byte
    function read(register)
    {
        local data = i2cPort.read(i2cAddress, format("%c", register), 1);
        if(data == null)
        {
            imp.log("I2C Read Failure");
            return -1;
        }
 
        return data[0];
    }
    
    function readMulti( register, numberToRead )
    {
        local data = i2cPort.read( i2cAddress, format("%c", register), numberToRead );
        if(data == null)
        {
            server.log("I2C Read Failure");
            return -1;
        }
 
        return data;
    }
 
    // Write a byte
    function write(register, data)
    {
        i2cPort.write(i2cAddress, format("%c%c", register, data));
    }
 
    // Write a bit to a register
    function writeBit(register, bitn, level)
    {
        local value = read(register);
        value = (level == 0)?(value & ~(1<<bitn)):(value | (1<<bitn));
        write(register, value);
    }
 
    // Write a masked bit pattern
    function writeMasked(register, data, mask)
    {
       local value = read(register);
       value = (value & ~mask) | (data & mask);
       write(register, value);
    }
 
}

//--------------------------------------------------------------------------------------------------------
class AccelerometerLSM303DLM extends I2C
{
    Ax      = 0;
    Ay      = 0;
    Az      = 0; 
    
    Ax1     = 0.0;
    Ay1     = 0.0;
    Az1     = 0.0;
    
    pitch   = 0.0;
    roll    = 0.0;
    

    constructor(port, address)
    {
        base.constructor( port, address );
        
        // Initialize the chip
        write( 0x20, 0x27 );
        write( 0x23, 0x80 );
    }
    
    function readHW()
    {
        // It is possible to read multiple bytes using the i2c lib function. LSM303 needs the SUB Address (register addr)
        // to be read with the bit 7 set to be set into multi read mode. Instead of reading register x28, we read xA8 for
        // bulk transfer. Variable Accel captures the result as a string of bytes, 2 bytes per axis.
        // Strings are immutable so create a new string every read.
        local Accel = readMulti( 0xA8, 6 ); 
                
        // Check to see that the readings are ok before conversion
        if( Accel != -1 )
        {
            // All i2c Reads come back as type string even if you are reading a single byte. 
            // First index is the first byte. Indexing the string allows conversion to integer which is not possible with type string. 
            server.log(format("Raw %02X%02X  %02X%02X  %02X%02X", Accel[1],Accel[0],Accel[3],Accel[2],Accel[5],Accel[4]));
            
            // Accelerometer returns 12 bits left justified in 2 bytes. After combining the 2 bytes into an
            // integer, shift right 4 bits to get the 12 relevant bits
            Ax = ( (Accel[1]  << 8 ) | Accel[0] ) >> 4 ;
            Ay = ( (Accel[3]  << 8 ) | Accel[2] ) >> 4 ;
            Az = ( (Accel[5]  << 8 ) | Accel[4] ) >> 4 ;
            
            
            server.log(format("Ax %d  Ay %d  Az %d", Ax,Ay,Az));
            
            // Output is 2's complement. Convert to integer
            if( Ax & 0x0800 )
            {
                Ax = ~Ax + 1;
                Ax = (Ax & 0x0FFF) * -1;
            }
            
            if( Ay & 0x0800 )
            {
                Ay = ~Ay + 1;
                Ay = (Ay & 0x0FFF) * -1;
            }
            
            if( Az & 0x0800 )
            {
                Az = ~Az + 1;
                Az = (Az & 0x0FFF) * -1;
            }
            
            
            server.log(format("After Int Conv Ax %d  Ay %d  Az %d", Ax,Ay,Az));
            
            Ax1 = Ax * (2.0/2048.0);
            Ay1 = Ay * (2.0/2048.0);
            Az1 = Az * (2.0/2048.0);
            
            // Divide by the sensitivity to convert to mG    
            //Ax1 = Ax1 + ( Ax / 2000.0 - Ax1) * A_FILTER;
            //Ay1 = Ay1 + ( Ay / 2000.0 - Ay1) * A_FILTER;
            //Az1 = Az1 + ( Az / 2000.0 - Az1) * A_FILTER; 
            
            server.log(format("Ax1 %f  Ay1 %f  Az1 %f", Ax1,Ay1,Az1));
            
            pitch = math.asin(-Ax1 );
            roll  = math.asin(Ay1/math.cos(pitch));
            
            
            server.log(format("Accel Mag %f", math.sqrt( Ax1 * Ax1 + Ay1* Ay1 + Az1 * Az1 )));
    
        }
        else
        {
            server.log("I2C Read Failure");
        }
    
    }
    
}

//--------------------------------------------------------------------------------------------------------
class magneticSenseLSM303DLM extends I2C
{
    Mx      = 0;
    My      = 0;
    Mz      = 0;
    
    Mx1     = 0.0;
    My1     = 0.0;
    Mz1     = 0.0;

    constructor(port, address)
    {
        base.constructor( port, address )
        // Initialize the magneticSense chip
        write( 0x00, 0x14 );
        write( 0x02, 0x00 );
    }
    
    function readHW()
    {
        // wait for the magnetometer readings to be ready
        local testBit = 0;
        while( (testBit & 0x01) == 0 )
        {
            testBit = read(0x09);
            if( testBit == null )
                break;
        }
        
        testBit = read( 0x02 );

        local mag = readMulti( 0x03,6 );    
        if( mag == null)
        {
            return;
        }

        // Note: Z register before Y register
        Mx = ( ( mag[0]  << 8 ) | mag[1] ) ;
        My = ( ( mag[4]  << 8 ) | mag[5] ) ;
        Mz = ( ( mag[2]  << 8 ) | mag[3] ) ;
        
        if( Mx & 0x8000 )
        {
            Mx = ~Mx + 1;
            Mx = (Mx & 0xFFFF) * -1;
        }
        
        if( My & 0x8000 )
        {
            My = ~My + 1;
            My = (My & 0xFFFF) * -1;
        }
        
        if( Mz & 0x8000 )
        {
            Mz = ~Mz + 1;
            Mz = (Mz & 0xFFFF) * -1;
        }
        
        
        // Conversion factors are from the App Note AN 3192
        //Mx1 = Mx1 + (Mx / 1055.0 - Mx1) * M_FILTER;
        //My1 = My1 + (My / 1055.0 - My1) * M_FILTER;
        //Mz1 = Mz1 + (Mz / 950.0 - Mz1)  * M_FILTER;
        
        
        Mx1 = Mx / 1055.0;
        My1 = My / 1055.0;
        Mz1 = Mz / 950.0;
        
        server.log(format("Mx %f  My %f  Mz %f", Mx1,My1,Mz1));
        
        
        local magnitude = math.sqrt( Mx1 * Mx1 + My1 * My1 + Mz1 * Mz1 );
        
        server.log(format("Mag Magnitude %f", magnitude));
        
        
    }

}

//--------------------------------------------------------------------------------------------------------
class TiltCompensatedCompass
{
    heading         = 0.0;
    
    accelerometer   = AccelerometerLSM303DLM( I2C_89, 0x18 );
    magneticSense   = magneticSenseLSM303DLM( I2C_89, 0x1E );

    function calcHeading()
    {
        accelerometer.readHW();
        magneticSense.readHW();
        
        // See http://blog.solutions-cubed.com/lsm303-compass-tilt-compensation/ for calibration co-efficients
        local Mxc = (magneticSense.Mx1-(-0.505)) / (0.375- (-0.505)) * 2.0 -1.0;
        local Myc = (magneticSense.My1-(-0.494)) / (0.432- (-0.494)) * 2.0 - 1.0;
        local Mzc = (magneticSense.Mz1-(-0.503)) / (0.533 -(-0.503)) * 2.0 -1.0;

        local Mx2 = Mxc * math.cos(accelerometer.pitch) + Mzc * math.sin(accelerometer.pitch);
        local My2 = Mxc * math.sin(accelerometer.roll)*math.sin(accelerometer.pitch) + Myc * math.cos (accelerometer.roll) - Mzc * math.sin(accelerometer.roll)*math.cos(accelerometer.pitch);
        //local Mz2 = -magneticSense.Mx1 * math.cos(accelerometer.roll)*math.sin(accelerometer.pitch) + magneticSense.My1*math.sin(accelerometer.roll) + magneticSense.Mz1 * math.cos(accelerometer.roll) * math.cos(accelerometer.pitch);
        
        
        
        heading =  TO_DEG * math.atan2(My2,Mx2);
        
        // Compensate for fact that the way the PCB is installed shifts the axis through 90 degrees.
        // Easier than soldering....
        heading = heading - 90.0;
        
        if ( ( My2 < 0 ) || ( heading < 0 ) )
            heading = 360.0 + heading;
        
        
        
        heading = (heading + headingCalibrate) % 360.0;
        this.accelerometer.pitch = this.accelerometer.pitch * TO_DEG;
        this.accelerometer.roll  =  this.accelerometer.roll * TO_DEG;
        
        if( logEnabled == 1 )
            server.log(format("Heading %.2f Pitch %.2f Roll %.2f", this.heading, this.accelerometer.pitch, this.accelerometer.roll ));
                            
    }
    
}

//--------------------------------------------------------------------------------------------------------
// IO Expander Class for SX1509
// This class modified version of electric imp sample. Specifically, the I2C general functionality
// was refactored to another class called I2C which helps remove I2C code duplication when multiple
// I2C peripherals are connected. 
// http://devwiki.electricimp.com/doku.php?id=codewebcolor&s[]=sx1509
//--------------------------------------------------------------------------------------------------------
class IoExpander extends I2C
{
    constructor(port, address)
    {
        base.constructor( port, address )
    }
    
    // Set a GPIO level
    function setPin(gpio, level)
    {
        writeBit(gpio>=8?0x10:0x11, gpio&7, level?1:0);
    }
 
    // Set a GPIO direction
    function setDir(gpio, output)
    {
        writeBit(gpio>=8?0x0e:0x0f, gpio&7, output?0:1);
    }
 
    // Set a GPIO internal pull up
    function setPullUp(gpio, enable)
    {
        writeBit(gpio>=8?0x06:0x07, gpio&7, enable);
    }
 
    // Set GPIO interrupt mask
    function setIrqMask(gpio, enable)
    {
        writeBit(gpio>=8?0x12:0x13, gpio&7, enable);
    }
 
    // Set GPIO interrupt edges
    function setIrqEdges(gpio, rising, falling)
    {
        local addr = 0x17 - (gpio>>2);
        local mask = 0x03 << ((gpio&3)<<1);
        local data = (2*falling + rising) << ((gpio&3)<<1);    
        writedMasked(addr, data, mask);
    }
 
    // Clear an interrupt
    function clearIrq(gpio)
    {
        writeBit(gpio>=8?0x18:0x19, gpio&7, 1);
    }
 
    // Get a GPIO input pin level
    function getPin(gpio)
    {
        return (read(gpio>=8?0x10:0x11)&(1<<(gpio&7)))?1:0;
    }
}

//--------------------------------------------------------------------------------------------------------
// Stepper Motor Class for steppers driven off the IoExpander
// Motors wired up to the SX1509 IO Extender as follows
    // 0 dir    Stepper Azimuth
    // 1 step
    // 2 slp
    // 3 rst
    // 4 dir    Stepper Elevation
    // 5 step
    // 6 slp
    // 7 rst
//--------------------------------------------------------------------------------------------------------
class StepperMotor extends IoExpander
{
    // IO Pin assignments
    pinDIR = null;
    pinSTP = null;
    pinSLP = null;
    pinRST = null;
    
    stepsToGo = -1.0;
    
    constructor(port, address, dir, step, slp, rst)
    {
        base.constructor(port, address);
 
        // Save pin assignments
        pinDIR = dir;
        pinSTP = step;
        pinSLP = slp;
        pinRST = rst;
 
        // Set pins as outputs
        writeBit(pinDIR>7?0x0E:0x0F, pinDIR>7?(pinDIR-7):pinDIR, 0);
        writeBit(pinSTP>7?0x0E:0x0F, pinSTP>7?(pinSTP-7):pinSTP, 0);
        writeBit(pinSLP>7?0x0E:0x0F, pinSLP>7?(pinSLP-7):pinSLP, 0);
        writeBit(pinRST>7?0x0E:0x0F, pinRST>7?(pinRST-7):pinRST, 0);
        
        
        // Set pins active pullup
        writeBit(pinDIR>7?0x06:0x07, pinDIR>7?(pinDIR-7):pinDIR, 1);
        writeBit(pinSTP>7?0x06:0x07, pinSTP>7?(pinSTP-7):pinSTP, 1);
        writeBit(pinSLP>7?0x06:0x07, pinSLP>7?(pinSLP-7):pinSLP, 1);
        writeBit(pinRST>7?0x06:0x07, pinRST>7?(pinRST-7):pinRST, 1);
        
 
        // Reset stepper controller chip
        setPin(pinRST, 0);
        setPin(pinRST, 1);
        setPin(pinSLP, 1);
        
    }
    
    function setDirection( dir )
    {
        setPin(pinDIR, dir );
    }
 
    // Step motor in desired direction
    function step( )
    {
        this.stepsToGo = this.stepsToGo - 1.0;
        setPin(pinSTP, 1);
        setPin(pinSTP, 0);
    }
}

//--------------------------------------------------------------------------------------------------------
// AxisTrackerMotor - A motor that drives the axis error to zero based on the resolution of the
// axis and a user defined deadband. This is predictive tracking - not PID
//--------------------------------------------------------------------------------------------------------
class AxisTrackerMotor extends StepperMotor
{
    resolution  = null;
    name        = null;
    
    constructor(port, address, dir, step, slp, rst, axisResolution, axisName)
    {
        base.constructor(port, address, dir, step, slp, rst);
        resolution  = axisResolution;
        name        = axisName;
    }
    
    function track( axisError, deadBand )
    {
        if( math.abs( axisError ) > deadBand )
        {
            stepsToGo = math.abs(axisError) * resolution;
            server.log(format("Tracker %s to move = %f", name, stepsToGo ));
            // Determine whether to move forward or backward ( or up /down depending on your point of view)
            setDirection ( axisError < 0.0 ? 0 : 1 );
            moveStepsToCompletion( );   
        }
    }
    
    function moveStepsToCompletion( )
    {
        if( stepsToGo >= 0 )    
        {
            step();
            imp.wakeup( 0.020, this.moveStepsToCompletion.bindenv(this) );
        }
        else
        {
            server.log(format("Movement complete: %s", name ));
        }
    }
    
}

//--------------------------------------------------------------------------------------------------------
local tiltCompass               = TiltCompensatedCompass();
local azimuthTrackerMotor       = AxisTrackerMotor( I2C_89, 0x3E, 4, 5, 6, 7, AZIMUTH_AXIS_RESOLUTION, "AZIMUTH" );
local elevationTrackerMotor     = AxisTrackerMotor( I2C_89, 0x3E, 0, 1, 2, 3, ELEVATION_AXIS_RESOLUTION, "ELEVATION" );

//--------------------------------------------------------------------------------------------------------
function logSQLData()
{
    // Set the ouputs to transfer to the SQL database
    local portString = format("%s,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f", hardware.getimpeeid(), tiltCompass.heading, 
        tiltCompass.accelerometer.roll + INSTALLATION_ROLL_ANGLE, tiltCompass.accelerometer.roll, 
        hardware.voltage(),udtSunCoordinatesdAzimuth, 90.0 - udtSunCoordinatesdZenithAngle );
    compassPort.set( portString );
   
    if( logEnabled == 1 )
        server.log("DataLogged:" + portString );
}

//--------------------------------------------------------------------------------------------------------
function sendCosm()
{
    headingPort.set( tiltCompass.heading );
    rollPort.set( tiltCompass.accelerometer.roll );
    pitchPort.set( tiltCompass.accelerometer.pitch ); 
    sunAzimuthPort.set( udtSunCoordinatesdAzimuth );
    sunZenithPort.set( udtSunCoordinatesdZenithAngle );
}

//--------------------------------------------------------------------------------------------------------
function StepAzToCompletion()
{
    if( azimuthTrackerMotor.stepsToGo >= 0 )    
    {
        azimuthTrackerMotor.step()
        imp.wakeup( 0.025, StepAzToCompletion )
    }
    else
    {
        server.log(format("AZ Step Count %d",azimuthTrackerMotor.stepsToGo) );
    }
}

//--------------------------------------------------------------------------------------------------------
function StepElToCompletion()
{
    if( elevationTrackerMotor.stepsToGo >= 0 )    
    {
        elevationTrackerMotor.step()
        imp.wakeup( 0.005, StepElToCompletion )
    }
    else
    {
        server.log(format("EL Step Count %d",elevationTrackerMotor.stepsToGo ) );
    }
}

//--------------------------------------------------------------------------------------------------------
function getReadingsAndMove()
{
    
    tiltCompass.calcHeading();
    cycleCount++;
    logSQLData();
    
    //if( (cycleCount % 2) == 0)
    {
        sunpos();
        
        //sendCosm();
        //logSQLData();            
                
        
        if( autoTrack == 1 )
        {
            if( ( azimuthTrackerMotor.stepsToGo < 0.0 ) && ( elevationTrackerMotor.stepsToGo < 0.0 ) )
            {
                
                local offsetAzimuth = (tiltCompass.heading ) - udtSunCoordinatesdAzimuth;
                //if( tiltCompass.accelerometer.roll <=0.01 )
                // tiltCompass.accelerometer.roll = 90.0*RAD;
                local offsetElevation = ( 90.0 - udtSunCoordinatesdZenithAngle ) - (tiltCompass.accelerometer.roll + INSTALLATION_ROLL_ANGLE ); 
                server.log( format("Offset Azimuth %f    Offset Zenith %f", offsetAzimuth, offsetElevation ));
                azimuthTrackerMotor.track( offsetAzimuth, 1.0 );
                elevationTrackerMotor.track( offsetElevation, 1.0 );
            }
        }
    }
        
    imp.wakeup(720, getReadingsAndMove);    
}

//--------------------------------------------------------------------------------------------------------
// Input class for the HTML post - refer to IMP website
//--------------------------------------------------------------------------------------------------------
class HtmlCommands extends InputPort
{
    name = "HtmlCommands"
    type = "string"
    
    function set(v) 
    { 
        server.log(v.command);
        server.show("HTTP " + v.command + " " + v.headingslider + " " + v.elevationslider );
        local startIdx = v.command.find("MOVE");
        if( startIdx != null )
        {
            server.log(v.command);
            autoTrack = 0;
            server.log(format("%s", v.headingslider ));
            local offsetAzimuth = (tiltCompass.heading ) - v.headingslider.tofloat();
            azimuthTrackerMotor.track( offsetAzimuth, 1.0 );
            
            local offsetElevation = v.elevationslider.tofloat() - (tiltCompass.accelerometer.roll + INSTALLATION_ROLL_ANGLE ); 
            elevationTrackerMotor.track( offsetElevation, 1.0 );
            server.log( format("Offset Azimuth %f    Offset Elevation %f", offsetAzimuth, offsetElevation ));
        }    
        else
            autoTrack = 1;
    }
}


//--------------------------------------------------------------------------------------------------------
// Electronic Compass, Solar Angle Calculator and Stepper Driver thingymabobber
//--------------------------------------------------------------------------------------------------------
imp.configure("CompassAccelSolar", [HtmlCommands], [compassPort,headingPort,rollPort,pitchPort,sunAzimuthPort,sunZenithPort]);

getReadingsAndMove();