using System;
using System.IO;
/*
 BASED ON python script here
 http://binarymillenium.googlecode.com/svn/trunk/processing/psynth/bin_to_csv.py
 
 via blog posts
  
 http://binarymillenium.com/2008/08/exporting-point-clouds-from-photosynth.html
 http://binarymillenium.com/2008/08/photosynth-export-process-tutorial.html
 
 and
  
 http://getsatisfaction.com/livelabs/topics/pointcloud_exporter
 
 any errors are mine (http://conceptdev.blogspot.com)
 */
namespace PointExtractor
{
    //TODO: struct minmax { float x; float y; float z; float x1; float y1; float z1;}
    class Program
    {
        static void Main(string[] args)
        {
            ReadPoints(args[0]);
        }
        
        /// <summary>
        /// def checkfloats(fbin):
        ///     for i in range(3):
        ///         if not((abs(fbin[i]) &gt; 1e-10) and (abs(fbin[i]) &lt; 1e6)): return False
        ///     return True;
        /// </summary>
        static bool checkFloats(byte[] fbin)
        {
            for (int i = 0; i < 3; i++)
            {
                float t = System.BitConverter.ToSingle(fbin, i*4);
                if (
                    ! ( (Math.Abs(t) > 1e-10)
                     && (Math.Abs(t) < 1e6) )
                ) 
                    return false;
            }
            return true;
        }
        /// <summary>
        /// fbin = array.array('f')
        /// fbin.fromfile(fraw, 3)
        /// </summary>
        static float f(byte[] b, int i)
        { 
            var f1=new byte[4]{b[i],b[i+1],b[i+2],b[i+3]};
            return BitConverter.ToSingle(f1, 0);
        }
        /// <summary>
        ///    bin = array.array('H')
        /// bin.fromfile(fraw, 1)
        /// </summary>
        static short b(byte[] b)
        {
            var f1=new byte[2] {b[0],b[1]};
            return BitConverter.ToInt16(f1, 0);
        }
 
        /// <summary>
        /// 14 bytes long. The first 3 sets of 4 bytes are the xyz position in floating point values. 
        /// in python I had to do a byteswap on those bytes (presumably from network order) to get them to be read in right with the readfile command.
        /// The last 2 bytes is the color of the point. It's only 4-bits per color channel,
        /// </summary>
        static void ReadPoints(string filename)
        {
            using (BinaryReader fraw = new BinaryReader(File.Open(filename, FileMode.Open)))
            {
                int filesize = (int)fraw.BaseStream.Length;
 
                var bsize = 14;
                //HACK: i don't seem to be able to get this working, but if i cycle 
                //HACK: through even numbers i eventually hit one that works for each bin file...
                var offset = filesize - (Convert.ToInt32(filesize / bsize) - 1) * bsize;
                offset = 10; // try
                offset = 20; // try, or any other value... :-s
 
                var bin = new byte[offset];
                bin = fraw.ReadBytes(offset);   // use up the initial bytes
                int pos = sizeof(byte) * offset;
 
                // get first chunk to check
                bin = fraw.ReadBytes(14);
                pos += sizeof(byte) * 14;
                while (pos < filesize)
                {
                    bin = byteswap(bin);
 
                    if (checkFloats(bin))
                    {
                        float t1 = f(bin, 0);
                        float t2 = f(bin, 4);
                        float t3 = f(bin, 8);
                        Console.Write("{0}, {1} ,{2}",t1,t2,t3);
                        short color = b(bin); // already did byteswap above
                        var red =     (color >> 11) & 0x1f;
                        var green = (color >> 5)  & 0x3f;
                        var blue = (color >> 0) & 0x1f;
                        Console.WriteLine(", {0}, {1}, {2}", red,green,blue);
 
                        //TODO: collect min/max
                        for (int j = 0; j < 3; j++)
                        {
                            //float m1 = System.BitConverter.ToSingle(fbin, j);
                        }
                       
                    }
                    // get another 14 bytes
                    bin = fraw.ReadBytes(14);
                    pos += sizeof(byte) * 14;
                }
                // TODO: output min/max
            }
            //Console.ReadKey();
        }
 
        /// <summary>
        /// fbin.byteswap()
        /// </summary>
        private static byte[] byteswap(byte[] bin)
        {
            // HACK: !
            var b = new byte[14];
            b[0] = bin[1];
            b[1] = bin[0];
            b[2] = bin[3];
            b[3] = bin[2];
            b[4] = bin[5];
            b[5] = bin[4];
            b[6] = bin[7];
            b[7] = bin[6];
            b[8] = bin[9];
            b[9] = bin[8];
            b[10] = bin[11];
            b[11] = bin[10];
            b[12] = bin[13];
            b[13] = bin[12];
            return b;
        }
    }
}