/*
 * Unicoder Test
 *
 * 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.
 */

import java.lang.System;
import java.lang.Integer;
import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.io.Display;
import com.ridgesoft.io.I2CMaster;
import com.nubotics.ndi.*;
import com.ridgesoft.robotics.*;
import com.ridgesoft.intellibrain.*;

/**
 * This class provides an example of the Nubotics Unicoder
 * encoder attached to the IntelliBrain.
 */
public class UnicoderTest {
    Unicoder iuc;
    BinaryCommandInterface bci;
    I2CSlaveDevice id;
    int slave_address;
    Display display;
    boolean short_mode;

/*
 * UnicoderTest constructor
 */

    UnicoderTest()
    {
        Log.log("---------------------------");
        Log.log("Unicoder Test");
        Log.log("Connecting to Unicoder at address 0x30");
        slave_address = (byte)0x30;
        try {
            id = new I2CSlaveDevice(slave_address, 400); // create I2C connection
            bci = new BinaryCommandInterface(id);
            iuc = new Unicoder(bci); // create I2C slave Unicoder
        } catch (Exception e) {
            e.fillInStackTrace();
            e.printStackTrace();
        }
    }

//********************************************
// Main Entry Point
//
//********************************************
    public static void main(String args[]) {

        try {
            long t;
            boolean st;
            UnicoderTest uct = new UnicoderTest();
            Unicoder uc = uct.iuc;

// connect to the Unicoder
            Log.log("Trying sync:");
            if (uc.doSync()) {
                Log.log("Got sync; trying echo:");

// confirm communications
                if (uc.doEcho((byte)0x5b)) {
                    Log.log("Echo worked.  Connected!");

// check device type
                    String name = uc.doName();
                    Log.log("Device name and version: " + name);
                    if (!name.equals(new String("NUc0E"))) {
                        Log.log("Wrong firmware version FAIL!");
                        System.exit(1);
                    }

// display useful info
                    byte mode = uc.doGetConstant((byte)2);
                    Log.log("Current mode = " + Integer.toHexString(mode & 0x0ff));

                    if ((mode & 0xfe) != 0x30) {
                        st = uc.doSetConstant((byte)2, (byte)0x31);
                        if (!st) {
                            Log.log("Failed to write mode!");
                            System.exit(1);
                        }
                        st = uc.doStoreConstants();
                        if (!st) {
                            Log.log("Failed to store constants!");
                            System.exit(1);
                        }
                        Log.log("Reset...");
                        uc.doReset();
                        t = System.currentTimeMillis();
                        while (System.currentTimeMillis() < (t + 1000)) {
                        }
                        uc.doSync();
                        mode = uc.doGetConstant((byte)2);
                        if (mode != 0x30) {
                            Log.log("Wrong mode FAIL!");
                            System.exit(1);
                        }
                        Log.log("Changed mode to 0x30 and stored.");
                    }
                    uc.doSync();

                    int status;
                    status = uc.getStatus() & 0x1f;
                    Log.log("Status: 0x" + Integer.toHexString(status));

                    Log.log("Getting AS5040 query register");
                    uc.getAS5040(uc.a);
                    Log.log("AS5040 P=" + uc.a.parity + " D=" + uc.a.magnitude_decrease + " I=" + uc.a.magnitude_increase + " L=" + 
                            uc.a.linearity_alarm + " C=" + uc.a.cordic_overflow + " O=" + uc.a.offset_compensation_finished + " A=" + uc.a.angle + " F=0x" + Integer.toHexString(uc.a.factory));
                    if ((uc.a.factory < 0x2001) || (uc.a.factory > 0x4001) || ((uc.a.factory & 0x8ffe) != 0)) {
                        Log.log("Invalid factory data!  FAIL!");
                        System.exit(1);
                    }

                    Log.log("Getting AS5040 programming register");
                    uc.getProg(uc.p);
                    Log.log("AS5040Prog M=" + uc.p.pmode + " D=" + uc.p.div + " I=" + uc.p.index + " Z=" + uc.p.z + " CCW=" + uc.p.ccw);

                    Log.log("Constants:");
                    for (int i = 0; i < 11; i++)
                        Log.log("Addr " + i + " = " + uc.doGetConstant((byte)i));

                    Log.log("---------------------------");

                    boolean aligment_mode = false;
                    int dist;
                    int old_dist = -100000;
                    int start_dist = uc.getDistance();

                    int min_alignment_filter = 0;
                    int ave_alignment_filter = 0;
                    int max_alignment_filter = 0;
                    int ave_alignment_sum = 0;
                    int alignment_sample = 0;
                    int total_samples = 0;
                    boolean looking_for_min = false;
                    boolean looking_for_max = false;
                    boolean found_min = false;
                    boolean found_max = false;
                    boolean started = false;
                    int cycle_length_sum = 0;
                    int prev_min_alignment_sample = 0;
                    int min_alignment_sample = 0;
                    int prev_max_alignment_sample = 0;
                    int max_alignment_sample = 0;
                    int cycle_length_count = 0;
                    int cycle_length;
                    int min_alignment = 1024;
                    int max_alignment = 0;
                    int ave_alignment = 0;
                    int recent_slope = 0;
                    int cycle = 0;
                    int recent_alignments[] = new int[4];
                    int recent_alignment_index = 0;
                    recent_alignments[0] = recent_alignments[1] = recent_alignments[2] = recent_alignments[3] = 0;

                    short old_angle = -1;
                    short angle;
                    short velocity;
                    short old_velocity = -10000;
                    short dir_change_count;
                    short old_dir_change_count = 0;
                    boolean stat;
                    int speed;
                    Servo s = IntelliBrain.getServo(1);
                    AnalogInput a = IntelliBrain.getThumbWheel();
                    ContinuousRotationServo crs = new ContinuousRotationServo(s, false);
                    //speed = ((a.sample() - a.getMaximum()/2) * 30) / (a.getMaximum()/2);
                    speed = 30;
                    crs.setPower(speed);
                    stat = uc.doDirChangeCount((short)0);

                    for (;;) {
                        status = uc.getStatus();
                        t = System.currentTimeMillis();
                        while (System.currentTimeMillis() < (t + 1)) {
                        }
                        velocity = uc.getVelocity();
                        t = System.currentTimeMillis();
                        while (System.currentTimeMillis() < (t + 1)) {
                        }
                        dir_change_count = uc.getDirChangeCount();
                        t = System.currentTimeMillis();
                        while (System.currentTimeMillis() < (t + 1)) {
                        }
                        dist = uc.getDistance();
                        dist -= start_dist;

                        t = System.currentTimeMillis();
                        while (System.currentTimeMillis() < (t + 1)) {
                        }
                        angle = uc.getAngle();
                        if (aligment_mode) {
                            total_samples++;
                            alignment_sample++;
                            ave_alignment_sum += angle;
                            if ((alignment_sample > 20) && !started) {
                                ave_alignment = ave_alignment_sum / alignment_sample;
                                started = true;
                                Log.log("Starting cycle analysis: average = " + ave_alignment);
                            }
                            if (angle < min_alignment) {
                                min_alignment = angle;
                                min_alignment_sample = total_samples;
                            }
                            if (angle > max_alignment) {
                                max_alignment = angle;
                                max_alignment_sample = total_samples;
                            }
                            recent_alignments[recent_alignment_index] = angle;
                            recent_alignment_index++;
                            if (recent_alignment_index > 3)
                                recent_alignment_index = 0;
                            recent_slope = (recent_alignments[3] - recent_alignments[0]) / 3;
                            if (started) { // we have a rough average value now
                                if (min_alignment < max_alignment) {
                                    if (angle < (ave_alignment - (ave_alignment - min_alignment)/2)) {
                                        looking_for_min = true;
                                    } else if (angle > (ave_alignment + (max_alignment - ave_alignment)/2)) {
                                        looking_for_max = true;
                                    }
                                }
                                if (looking_for_min && (recent_slope > 0)) {
                                    found_min = true;
                                    looking_for_min = false;
                                }
                                if (looking_for_max && (recent_slope < 0)) {
                                    found_max = true;
                                    looking_for_max = false;
                                }
                                if (found_min && found_max) {
                                    found_min = found_max = false;
                                    cycle++;
                                    cycle_length_count++;
                                    cycle_length_sum += max_alignment_sample - prev_max_alignment_sample;
                                    prev_max_alignment_sample = max_alignment_sample;
                                    cycle_length = cycle_length_sum / cycle_length_count;
                                    if (min_alignment_filter == 0)
                                        min_alignment_filter = min_alignment;
                                    else
                                        min_alignment_filter = (min_alignment + min_alignment_filter * 9) / 10;
                                    min_alignment = 1024;
                                    if (max_alignment_filter == 0)
                                        max_alignment_filter = max_alignment;
                                    else
                                        max_alignment_filter = (max_alignment + max_alignment_filter * 9) / 10;
                                    max_alignment = 0;
                                    ave_alignment = ave_alignment_sum / alignment_sample;
                                    if (ave_alignment_filter == 0)
                                        ave_alignment_filter = ave_alignment;
                                    else
                                        ave_alignment_filter = (ave_alignment + ave_alignment_filter * 9) / 10;
                                    ave_alignment_sum = ave_alignment_filter;
                                    alignment_sample = 0;
                                    Log.log("Cycle " + cycle + ", min " + min_alignment_filter + ", ave " + ave_alignment_filter + ", max " + max_alignment_filter + ", cycle len " + cycle_length + ", delta " + (max_alignment_filter - min_alignment_filter));
                                }
                            }
                        } else {
                            if ((velocity != old_velocity) || (dist != old_dist) || (angle != old_angle)) {
                                old_velocity = velocity;
                                old_dist = dist;
                                old_angle = angle;
                                Log.log("A" + angle + " V" + velocity + " D" + dist);
                                //Log.log("S" + speed + " V" + velocity + " D" + dist);
                            }

                            if (dir_change_count != old_dir_change_count) {
                                Log.log("Dir Change Count: " + dir_change_count);
                                //stat = uc.doDirChangeCount((short)0);
                                //dir_change_count = 0;
                                old_dir_change_count = dir_change_count;
                            }

                            if (dist > 10000) {
                                start_dist = uc.getDistance();
                                speed = -30;
                                crs.setPower(speed);
                            }
                            if (dist < -10000) {
                                Log.log("Dir Change Count: " + dir_change_count);
                                Log.log("PASS!");
                                uc.doCoast();
                                break;
                            }

                            if ((status & 0x0c) != 0) { // COF or LIN failure
                                Log.log("Status FAIL! 0x" + Integer.toHexString(status));
                                uc.doCoast();
                                break;
                            }

                            if (dir_change_count > 10) {
                                Log.log("Dir change FAIL!");
                                uc.doCoast();
                                break;
                            }

                            if ((dist > 200) && (velocity < 0)) {
                                Log.log("Forward FAIL!  Status: 0x" + Integer.toHexString(status));
                                uc.doCoast();
                                break;
                            } else if ((dist < -200) && (velocity > 0)) {
                                Log.log("Backward FAIL!  Status: 0x" + Integer.toHexString(status));
                                uc.doCoast();
                                break;
                            }
                        }
                        if (IntelliBrain.getStartButton().isPressed()) {
                            IntelliBrain.getStartButton().waitReleased();
                            angle = uc.getAngle();
                            uc.p.z += angle;
                            if (uc.p.z >= 1024)
                                uc.p.z -= 1024;
                            uc.doProg(uc.p); // when start pressed, reset index to this position
                            Log.log("- index set to " + uc.p.z + " -");
                            angle = uc.getAngle();
                            Log.log("-> A " + angle);
                            Log.log("Going to alignment mode; power cycle to leave this mode.");
                            if (uc.doAligmentMode())
                                aligment_mode = true;
                        }
                    }
                } else
                    Log.log("Echo failed.");
            } else
                Log.log("Sync failed.");
        } catch (Throwable t) {
            t.fillInStackTrace();
            t.printStackTrace();                                                        // Display a stack trace for any error
        }
    }
}