/*
 * Unicoder
 *
 * Copyright 2009 by Noetic Design, Inc.
 * www.nubotics.com
 *
 * Noetic Design grants you the right to use, modify, make derivative works and
 * redistribute this source file provided you do not remove this copyright notice.
 */

package com.nubotics.ndi;
import com.nubotics.ndi.CommandInterface;
import com.nubotics.ndi.BinaryCommandInterface;
import com.nubotics.ndi.NdiCommander;
import com.nubotics.ndi.Log;

/**
 * This class provides an example of the Nubotics ME-210 
 * Unicoder intelligent encoder attached to the IntelliBrain. 
 */
public class Unicoder extends NdiCommander {

    public class AS5040 {
        public boolean parity;
        public boolean magnitude_decrease;
        public boolean magnitude_increase;
        public boolean linearity_alarm;
        public boolean cordic_overflow;
        public boolean offset_compensation_finished;
        public short angle;
        public short factory;
        public AS5040()
        {
            parity = false;
            magnitude_decrease = false;
            magnitude_increase = false;
            linearity_alarm = false;
            cordic_overflow = false;
            offset_compensation_finished = false;
            angle = 0;
            factory = 0;
        }
    }
    public AS5040 a;
    public class AS5040Prog {
        public byte pmode;
        public byte div;
        public boolean index;
        public short z;
        public boolean ccw;
        public AS5040Prog()
        {
            pmode = 0;
            div = 0;
            index = false;
            z = 0;
            ccw = false;
        }
    }
    public AS5040Prog p;

/*
 * Unicoder constructor
 */

    public Unicoder(CommandInterface com_int)
    {
        super(com_int);
        a = new AS5040();
        p = new AS5040Prog();
    }

    public Unicoder(BinaryCommandInterface com_int)
    {
        super(com_int);
        a = new AS5040();
        p = new AS5040Prog();
    }

    public boolean setShortMode(boolean sh_mode) throws Exception
    {
        byte mode = doGetConstant((byte)2);
        mode &= ~0x08;
        if (sh_mode)
            mode |= 0x08;
        return doSetConstant((byte)2, mode);
    }

    public boolean setI2CAddress(byte address)
    {
        return doSetConstant((byte)4, address);
    }

    public boolean doProg(AS5040Prog p) // byte pmode, byte div, boolean index, short z, boolean ccw)
    {
        int i, c;
        i = p.index ? 1 : 0;
        c = p.ccw ? 1 : 0;
        short prog = (short)((p.pmode & 0x03) | ((p.div & 0x03) << 2) | (i << 4) | ((p.z & 0x3ff) << 5) | (c << 15));
        if (doSetConstant((byte)5, (byte)(prog & 0x0ff)))
            return doSetConstant((byte)6, (byte)((prog >> 8) & 0x0ff));
        else
            return false;
    }

    public void getProg(AS5040Prog p) throws Exception
    {
        byte lo, hi;
        lo = doGetConstant((byte)5);
        hi = doGetConstant((byte)6);
        p.pmode = (byte)(lo & 0x03);
        p.div = (byte)((lo >> 2) & 0x03);
        p.index = (lo & 0x10) != 0 ? true : false;
        p.z = (short)(((lo >> 5) & 0x07) | ((hi & 0x7f) << 5));
        p.ccw = (hi & 0x80) != 0 ? true : false;
    }

    public void getAS5040(AS5040 a)
    {
        int ret = ci.doGetDWordCmd('Z');
        a.parity = (ret & 0x01) != 0 ? true : false;
        a.magnitude_decrease = (ret & 0x02) != 0 ? true : false;
        a.magnitude_increase = (ret & 0x04) != 0 ? true : false;
        a.linearity_alarm = (ret & 0x08) != 0 ? true : false;
        a.cordic_overflow = (ret & 0x10) != 0 ? true : false;
        a.offset_compensation_finished = (ret & 0x20) != 0 ? true : false;
        a.angle = (short)((ret >> 6) & 0x3ff); 
        a.factory = (short) ((ret >> 16) & 0xffff);
    }

    public int getStatus()
    {
        return ci.doGetWordCmd('S');
    }

    public int getTime()
    {
        return ci.doGetDWordCmd('T');
    }

    boolean doTime(int time)
    {
        return ci.doSetDWordCmd('T', time);
    }

    public boolean doSetConstant(byte address, byte value)
    {
        //Log.log("doSetConstant: both = " + both + ": 0x" + Integer.toHexString(both) + ", address = " + address + ", value = " + value);
        return ci.doSetByteByteCmd('F', address, value);
    }

    public byte doGetConstant(byte address) throws Exception
    {
        return ci.doGetByteByteCmd('F', address);
    }

    public boolean doStoreConstants()
    {
        return ci.doSetWordCmd('F', (short)0xffff);
    }

    public short getVelocity()
    {
        return ci.doGetWordCmd('V');
    }

    public int getPeriod()
    {
        return ci.doGetDWordCmd('P');
    }

    public short getDirChangeCount()
    {
        return ci.doGetWordCmd('Q');
    }

    public boolean doDirChangeCount(short count)
    {
        return ci.doSetWordCmd('Q', count);
    }

    public short getAngle()
    {
        ci.empty();
        return ci.doGetWordCmd('W');
    }

    public boolean doDistance(int distance)
    {
        return ci.doSetDWordCmd('X', distance);
    }

    public int getDistance()
    {
        ci.empty();
        return ci.doGetDWordCmd('D');
    }

    public boolean doCmd(char cmdChar, char expected)
    {
        for (int i = 1; i < 10; i++) {
            try {
                if (ci.doCmd(cmdChar, expected))
                    return true;
                Thread.sleep(1);
                doSync();
            } catch (Exception e) {
                //error = "doCmd Exception: " + e.getMessage();
                //Log.log(error);
            }
        }
        Log.log("bad doCmd return; cmd = " + cmdChar + "; expected " + expected);
        return false;
    }

    public boolean doCoast()
    {
        return doCmd('C', 'a');
    }

    public boolean doReset()
    {
        return doCmd('R', '\0');
    }

    public boolean doAligmentMode() // must power cycle to get out of this mode in the AS5040
    {
        return doCmd('A', 'a');
    }

}