//
// Prefix header for all source files of the 'Example_MarkerBasedAR'
//

#import <Availability.h>

#ifndef __IPHONE_5_0
#warning "This project uses features only available in iOS SDK 5.0 and later."
#endif

#ifdef __cplusplus
#include <opencv2/opencv.hpp>
#endif

#ifdef __OBJC__
  #import <UIKit/UIKit.h>
  #import <Foundation/Foundation.h>
#endif

@protocol VideoSourceDelegate<NSObject>

-(void)frameReady:(BGRAVideoFrame) frame;

@end

@interface VideoSource : NSObject<AVCaptureVideoDataOutputSampleBufferDelegate>
{

}

@property (nonatomic, retain) AVCaptureSession *captureSession;
@property (nonatomic, retain) AVCaptureDeviceInput *deviceInput;
@property (nonatomic, retain) id<VideoSourceDelegate> delegate;

- (bool) startWithDevicePosition:(AVCaptureDevicePosition)devicePosition;
- (CameraCalibration) getCalibration;
- (CGSize) getFrameSize;

@end

@protocol VideoSourceDelegate<NSObject>
-(void)frameReady:(BGRAVideoFrame) frame;

@end

- (id)init
{
  if ((self = [super init]))
  {
    AVCaptureSession * capSession = [[AVCaptureSession alloc] init];

    if ([capSession canSetSessionPreset:AVCaptureSessionPreset640x480])
    {
      [capSession setSessionPreset:AVCaptureSessionPreset640x480];
      NSLog(@"Set capture session preset AVCaptureSessionPreset640x480");
    }
    else if ([capSession canSetSessionPreset:AVCaptureSessionPresetLow])
    {
      [capSession setSessionPreset:AVCaptureSessionPresetLow];
      NSLog(@"Set capture session preset AVCaptureSessionPresetLow");
    }

    self.captureSession = capSession;
  }
  return self;
}

- (bool) startWithDevicePosition:(AVCaptureDevicePosition)devicePosition
{
  AVCaptureDevice *videoDevice = [self cameraWithPosition:devicePosition];

  if (!videoDevice)
    return FALSE;

  {
    NSError *error;

    AVCaptureDeviceInput *videoIn = [AVCaptureDeviceInput 
    deviceInputWithDevice:videoDevice error:&error];
    self.deviceInput = videoIn;

    if (!error)
    {
      if ([[self captureSession] canAddInput:videoIn])
      {
        [[self captureSession] addInput:videoIn];
      }
      else
      {
        NSLog(@"Couldn't add video input");
        return FALSE;
      }
    }
    else
    {
      NSLog(@"Couldn't create video input");
      return FALSE;
    }
  }

  [self addRawViewOutput];
  [captureSession startRunning];
  return TRUE;
}

- (void) addRawViewOutput
{
  /*We setupt the output*/
  AVCaptureVideoDataOutput *captureOutput = [[AVCaptureVideoDataOutput alloc] init];

  /*While a frame is processes in -captureOutput:didOutputSampleBuffer:fromConnection: delegate methods no other frames are added in the queue.
    If you don't want this behaviour set the property to NO */
  captureOutput.alwaysDiscardsLateVideoFrames = YES; 

  /*We create a serial queue to handle the processing of our frames*/
  dispatch_queue_t queue;
  queue = dispatch_queue_create("com.Example_MarkerBasedAR.cameraQueue", 
  NULL);
  [captureOutput setSampleBufferDelegate:self queue:queue];
  dispatch_release(queue);

  // Set the video output to store frame in BGRA (It is supposed to be faster)
  NSString* key = (NSString*)kCVPixelBufferPixelFormatTypeKey;
  NSNumber* value = [NSNumber 
  numberWithUnsignedInt:kCVPixelFormatType_32BGRA];

  NSDictionary* videoSettings = [NSDictionary dictionaryWithObject:value 
  forKey:key];
  [captureOutput setVideoSettings:videoSettings];

  // Register an output
  [self.captureSession addOutput:captureOutput];
}

- (void)captureOutput:(AVCaptureOutput *)captureOutput 
didOutputSampleBuffer:(CMSampleBufferRef)sampleBuffer 
       fromConnection:(AVCaptureConnection *)connection 
{ 
  // Get a image buffer holding video frame
  CVImageBufferRef imageBuffer = CMSampleBufferGetImageBuffer(sampleBuffer);

  // Lock the image buffer
  CVPixelBufferLockBaseAddress(imageBuffer,0); 

  // Get information about the image
  uint8_t *baseAddress = (uint8_t *)CVPixelBufferGetBaseAddress(imageBuffer); 
  size_t width = CVPixelBufferGetWidth(imageBuffer); 
  size_t height = CVPixelBufferGetHeight(imageBuffer);  
  size_t stride = CVPixelBufferGetBytesPerRow(imageBuffer);

  BGRAVideoFrame frame = {width, height, stride, baseAddress};
  [delegate frameReady:frame];

  /*We unlock the  image buffer*/
  CVPixelBufferUnlockBaseAddress(imageBuffer,0);
}

/**
 * A top-level class that encapsulate marker detector algorithm
 */
class MarkerDetector
{
public:

  /**
   * Initialize a new instance of marker detector object
   * @calibration[in] - Camera calibration necessary for pose estimation.
   */
  MarkerDetector(CameraCalibration calibration);

  void processFrame(const BGRAVideoFrame& frame);

  const std::vector<Transformation>& getTransformations() const;

  protected:
  bool findMarkers(const BGRAVideoFrame& frame, std::vector<Marker>& 
  detectedMarkers);

  void prepareImage(const cv::Mat& bgraMat,
                    cv::Mat& grayscale);

  void performThreshold(const cv::Mat& grayscale,
                        cv::Mat& thresholdImg);

  void findContours(const cv::Mat& thresholdImg, 
                    std::vector<std::vector<cv::Point> >& contours,
                    int minContourPointsAllowed);

  void findMarkerCandidates(const std::vector<std::vector<cv::Point> >& 
  contours, std::vector<Marker>& detectedMarkers);

  void detectMarkers(const cv::Mat& grayscale,
                     std::vector<Marker>& detectedMarkers);

  void estimatePosition(std::vector<Marker>& detectedMarkers);

private:
};

void MarkerDetector::prepareImage(const cv::Mat& bgraMat, cv::Mat& grayscale)
{
  // Convert to grayscale
  cv::cvtColor(bgraMat, grayscale, CV_BGRA2GRAY);
}

void MarkerDetector::performThreshold(const cv::Mat& grayscale, cv::Mat& thresholdImg)
{
  cv::adaptiveThreshold(grayscale,   // Input image
                        thresholdImg,// Result binary image
                        255,         //
                        cv::ADAPTIVE_THRESH_GAUSSIAN_C, //
                        cv::THRESH_BINARY_INV, //
                        7, //
                        7  //
                        );
}

void MarkerDetector::findContours(const cv::Mat& thresholdImg,
                                  std::vector<std::vector<cv::Point> >& contours,
int minContourPointsAllowed)
{
    std::vector< std::vector<cv::Point> > allContours;
    cv::findContours(thresholdImg, allContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

    contours.clear();
    for (size_t i=0; i<allContours.size(); i++)
    {
        int contourSize = allContours[i].size();
        if (contourSize > minContourPointsAllowed)
        {
            contours.push_back(allContours[i]);
        }
    }
}

void MarkerDetector::findCandidates
(
    const ContoursVector& contours,
    std::vector<Marker>& detectedMarkers
)
{
    std::vector<cv::Point>  approxCurve;
    std::vector<Marker>     possibleMarkers;

    // For each contour, analyze if it is a parallelepiped likely to be the 
    marker
    for (size_t i=0; i<contours.size(); i++)
    {
        // Approximate to a polygon
        double eps = contours[i].size() * 0.05;
        cv::approxPolyDP(contours[i], approxCurve, eps, true);

        // We interested only in polygons that contains only four points
        if (approxCurve.size() != 4)
            continue;

        // And they have to be convex
        if (!cv::isContourConvex(approxCurve))
            continue;

        // Ensure that the distance between consecutive points is large enough
        float minDist = std::numeric_limits<float>::max();

        for (int i = 0; i < 4; i++)
        {
            cv::Point side = approxCurve[i] - approxCurve[(i+1)%4];
            float squaredSideLength = side.dot(side);
            minDist = std::min(minDist, squaredSideLength);
        }

        // Check that distance is not very small
        if (minDist < m_minContourLengthAllowed)
            continue;

        // All tests are passed. Save marker candidate:
        Marker m;

        for (int i = 0; i<4; i++)
            m.points.push_back( cv::Point2f(approxCurve[i].x,approxCurve[i].y) );

        // Sort the points in anti-clockwise order
        // Trace a line between the first and second point.
        // If the third point is at the right side, then the points are anti-
        clockwise
        cv::Point v1 = m.points[1] - m.points[0];
        cv::Point v2 = m.points[2] - m.points[0];

        double o = (v1.x * v2.y) - (v1.y * v2.x);

        if (o < 0.0)         //if the third point is in the left side, then 
        sort in anti-clockwise order
            std::swap(m.points[1], m.points[3]);

        possibleMarkers.push_back(m);
    }

    // Remove these elements which corners are too close to each other.
    // First detect candidates for removal:
    std::vector< std::pair<int,int> > tooNearCandidates;
    for (size_t i=0;i<possibleMarkers.size();i++)
    { 
        const Marker& m1 = possibleMarkers[i];

        //calculate the average distance of each corner to the nearest corner 
        of the other marker candidate
        for (size_t j=i+1;j<possibleMarkers.size();j++)
        {
            const Marker& m2 = possibleMarkers[j];

            float distSquared = 0;

            for (int c = 0; c < 4; c++)
            {
                cv::Point v = m1.points[c] - m2.points[c];
                distSquared += v.dot(v);
            }

            distSquared /= 4;

            if (distSquared < 100)
            {
                tooNearCandidates.push_back(std::pair<int,int>(i,j));
            }
        }                
    }

    // Mark for removal the element of the pair with smaller perimeter
    std::vector<bool> removalMask (possibleMarkers.size(), false);

    for (size_t i=0; i<tooNearCandidates.size(); i++)
    {
        float p1 = perimeter(possibleMarkers[tooNearCandidates[i].first 
        ].points);
        float p2 = 
        perimeter(possibleMarkers[tooNearCandidates[i].second].points);

        size_t removalIndex;
        if (p1 > p2)
            removalIndex = tooNearCandidates[i].second;
        else
            removalIndex = tooNearCandidates[i].first;

        removalMask[removalIndex] = true;
    }

    // Return candidates
    detectedMarkers.clear();
    for (size_t i=0;i<possibleMarkers.size();i++)
    {
        if (!removalMask[i])
            detectedMarkers.push_back(possibleMarkers[i]);
    }
}

cv::Mat canonicalMarker;
Marker& marker = detectedMarkers[i];

// Find the perspective transfomation that brings current marker to rectangular form
cv::Mat M = cv::getPerspectiveTransform(marker.points, m_markerCorners2d);

// Transform image to get a canonical marker image
cv::warpPerspective(grayscale, canonicalMarker,  M, markerSize);

//threshold image
cv::threshold(markerImage, markerImage, 125, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);

cv::Mat src(400,400,CV_8UC1);
cv::Rect r(10,10,50,50);
cv::Mat subView = src(r);

 cv::Mat bitMatrix = cv::Mat::zeros(5,5,CV_8UC1);

  //get information(for each inner square, determine if it is  black or white)
  for (int y=0;y<5;y++)
  {
    for (int x=0;x<5;x++)
    {
      int cellX = (x+1)*cellSize;
      int cellY = (y+1)*cellSize;
      cv::Mat cell = grey(cv::Rect(cellX,cellY,cellSize,cellSize));

      int nZ = cv::countNonZero(cell);
      if (nZ> (cellSize*cellSize) /2)
        bitMatrix.at<uchar>(y,x) = 1;
    }
  }

  //check all possible rotations
  cv::Mat rotations[4];
  int distances[4];

  rotations[0] = bitMatrix;  
  distances[0] = hammDistMarker(rotations[0]);

  std::pair<int,int> minDist(distances[0],0);

  for (int i=1; i<4; i++)
  {
    //get the hamming distance to the nearest possible word
    rotations[i] = rotate(rotations[i-1]);
    distances[i] = hammDistMarker(rotations[i]);

    if (distances[i] < minDist.first)
    {
      minDist.first  = distances[i];
      minDist.second = i;
    }
  }

//sort the points so that they are always in the same order
// no matter the camera orientation
std::rotate(marker.points.begin(), marker.points.begin() + 4 - nRotations,
marker.points.end());

std::vector<cv::Point2f> preciseCorners(4 * goodMarkers.size());

for (size_t i=0; i<goodMarkers.size(); i++)
{
  Marker& marker = goodMarkers[i];

  for (int c=0;c<4;c++)
  {
    preciseCorners[i*4+c] = marker.points[c];
  }
}

cv::cornerSubPix(grayscale, preciseCorners, cvSize(5,5), cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_ITER,30,0.1));

//copy back
for (size_t i=0;i<goodMarkers.size();i++)
{
  Marker&marker = goodMarkers[i];

  for (int c=0;c<4;c++)
  {
    marker.points[c] = preciseCorners[i*4+c];
  }
}

P = A * [R | T] * M

/**
 * A camera calibration class that stores intrinsic matrix and distorsion coefficients.
 */
class CameraCalibration
{
  public:
  CameraCalibration();
  CameraCalibration(float fx, float fy, float cx, float cy);
  CameraCalibration(float fx, float fy, float cx, float cy, float 
  distorsionCoeff[4]);

  void getMatrix34(float cparam[3][4]) const;

  const Matrix33& getIntrinsic() const;
  const Vector4&  getDistorsion() const;

  private:
  Matrix33 m_intrinsic;
  Vector4  m_distorsion;
};

void solvePnP(const Mat& objectPoints, const Mat& imagePoints, const Mat& 
cameraMatrix, const Mat& distCoeffs, Mat& rvec, Mat& tvec, bool 
useExtrinsicGuess=false);

T(v) = Rv + t

It(R) = 1

void MarkerDetector::estimatePosition(std::vector<Marker>& detectedMarkers)
{
  for (size_t i=0; i<detectedMarkers.size(); i++)
  { 
    Marker& m = detectedMarkers[i];

    cv::Mat Rvec;
    cv::Mat_<float> Tvec;
    cv::Mat raux,taux;
    cv::solvePnP(m_markerCorners3d, m.points, camMatrix, distCoeff,raux,taux);
    raux.convertTo(Rvec,CV_32F);
    taux.convertTo(Tvec ,CV_32F);

    cv::Mat_<float> rotMat(3,3); 
    cv::Rodrigues(Rvec, rotMat);

    // Copy to transformation matrix
    m.transformation = Transformation();

    for (int col=0; col<3; col++)
    {
      for (int row=0; row<3; row++)
      {
        m.transformation.r().mat[row][col] = rotMat(row,col); // Copy rotation 
        component
      }
      m.transformation.t().data[col] = Tvec(col); // Copy translation 
      component
    }

    // Since solvePnP finds camera location, w.r.t to marker pose, to get 
    marker pose w.r.t to the camera we invert it.
    m.transformation = m.transformation.getInverted();
  }

@class EAGLContext;

// This class wraps the CAEAGLLayer from CoreAnimation into a convenient UIView subclass.
// The view content is basically an EAGL surface you render your OpenGL scene into.
// Note that setting the view non-opaque will only work if the EAGL surface has an alpha channel.
@interface EAGLView : UIView
{
@private
    // The OpenGL ES names for the framebuffer and renderbuffer used to render 
    to this view.
    GLuint defaultFramebuffer, colorRenderbuffer;
}

@property (nonatomic, retain) EAGLContext *context;
// The pixel dimensions of the CAEAGLLayer.
@property (readonly) GLint framebufferWidth;
@property (readonly) GLint framebufferHeight;

- (void)setFramebuffer;
- (BOOL)presentFramebuffer;
- (void)initContext;
@end

//The EAGL view is stored in the nib file. When it's unarchived it's sent -initWithCoder:.
- (id)initWithCoder:(NSCoder*)coder
{
  self = [super initWithCoder:coder];
  if (self) {
    CAEAGLLayer *eaglLayer = (CAEAGLLayer *)self.layer;

    eaglLayer.opaque = TRUE;
    eaglLayer.drawableProperties = [NSDictionary dictionaryWithObjectsAndKeys:
                                   [NSNumber numberWithBool:FALSE], 
                                   kEAGLDrawablePropertyRetainedBacking,
                                   kEAGLColorFormatRGBA8, 
                                   kEAGLDrawablePropertyColorFormat,
                                   nil];

    [self initContext];
  }

  return self;
}

- (void)createFramebuffer
{
  if (context && !defaultFramebuffer) {
    [EAGLContext setCurrentContext:context];

    // Create default framebuffer object.
    glGenFramebuffers(1, &defaultFramebuffer);
    glBindFramebuffer(GL_FRAMEBUFFER, defaultFramebuffer);

    // Create color render buffer and allocate backing store.
    glGenRenderbuffers(1, &colorRenderbuffer);
    glBindRenderbuffer(GL_RENDERBUFFER, colorRenderbuffer);
    [context renderbufferStorage:GL_RENDERBUFFER fromDrawable:(CAEAGLLayer *)self.layer];
    glGetRenderbufferParameteriv(GL_RENDERBUFFER, GL_RENDERBUFFER_WIDTH, &framebufferWidth);
    glGetRenderbufferParameteriv(GL_RENDERBUFFER, GL_RENDERBUFFER_HEIGHT, &framebufferHeight);

    glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, 
    GL_RENDERBUFFER, colorRenderbuffer);

    if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE)
    NSLog(@"Failed to make complete framebuffer object %x", 
    glCheckFramebufferStatus(GL_FRAMEBUFFER));

    //glClearColor(0, 0, 0, 0);
    NSLog(@"Framebuffer created");
  }
}

@interface SimpleVisualizationController : NSObject<VisualizationController>
{
  EAGLView * m_glview;
  GLuint m_backgroundTextureId;
  std::vector<Transformation> m_transformations;
  CameraCalibration m_calibration;
  CGSize m_frameSize;
}

-(id) initWithGLView:(EAGLView*)view calibration:(CameraCalibration) calibration frameSize:(CGSize) size;

-(void) drawFrame;
-(void) updateBackground:(BGRAVideoFrame) frame;
-(void) setTransformationList:(const std::vector<Transformation>&) transformations;

- (void)drawFrame
{
  // Set the active framebuffer
  [m_glview setFramebuffer];  

  // Draw a video on the background
  [self drawBackground];

  // Draw 3D objects on the position of the detected markers
  [self drawAR];

  // Present framebuffer
  bool ok = [m_glview presentFramebuffer];

  int glErCode = glGetError();
  if (!ok || glErCode != GL_NO_ERROR)
  {
    std::cerr << "GL error detected. Error code:" << glErCode << std::endl;
  }
}

- (void) drawBackground
{
  GLfloat w = m_glview.bounds.size.width;
  GLfloat h = m_glview.bounds.size.height;
  const GLfloat squareVertices[] =
  {
    0, 0,
    w, 0,
    0, h,
    w, h
  };

  static const GLfloat textureVertices[] =
  {
    1, 0,
    1, 1,
    0, 0,
    0, 1
  };

  static const GLfloat proj[] =
  {
    0, -2.f/w, 0, 0,
    -2.f/h, 0, 0, 0,
    0, 0, 1, 0,
    1, 1, 0, 1
  };

  glMatrixMode(GL_PROJECTION);
  glLoadMatrixf(proj);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  glDisable(GL_COLOR_MATERIAL);

  glEnable(GL_TEXTURE_2D);
  glBindTexture(GL_TEXTURE_2D, m_backgroundTextureId);

  // Update attribute values.
  glVertexPointer(2, GL_FLOAT, 0, squareVertices);
  glEnableClientState(GL_VERTEX_ARRAY);
  glTexCoordPointer(2, GL_FLOAT, 0, textureVertices);
  glEnableClientState(GL_TEXTURE_COORD_ARRAY);

  glColor4f(1,1,1,1);
  glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);

  glDisableClientState(GL_VERTEX_ARRAY);
  glDisableClientState(GL_TEXTURE_COORD_ARRAY);
  glDisable(GL_TEXTURE_2D);
}

- (void)buildProjectionMatrix:(Matrix33)cameraMatrix: (int)screen_width: (int)screen_height: (Matrix44&) projectionMatrix
{    
  float near = 0.01;  // Near clipping distance
  float far = 100;  // Far clipping distance

  // Camera parameters
  float f_x = cameraMatrix.data[0]; // Focal length in x axis
  float f_y = cameraMatrix.data[4]; // Focal length in y axis (usually the 
  same?)
  float c_x = cameraMatrix.data[2]; // Camera primary point x
  float c_y = cameraMatrix.data[5]; // Camera primary point y

  projectionMatrix.data[0] =  - 2.0 * f_x / screen_width;
  projectionMatrix.data[1] = 0.0;
  projectionMatrix.data[2] = 0.0;
  projectionMatrix.data[3] = 0.0;

  projectionMatrix.data[4] = 0.0;
  projectionMatrix.data[5] = 2.0 * f_y / screen_height;
  projectionMatrix.data[6] = 0.0;
  projectionMatrix.data[7] = 0.0;

  projectionMatrix.data[8] = 2.0 * c_x / screen_width - 1.0;
  projectionMatrix.data[9] = 2.0 * c_y / screen_height - 1.0;   
  projectionMatrix.data[10] = -( far+near ) / ( far - near );
  projectionMatrix.data[11] = -1.0;

  projectionMatrix.data[12] = 0.0;
  projectionMatrix.data[13] = 0.0;
  projectionMatrix.data[14] = -2.0 * far * near / ( far - near );
  projectionMatrix.data[15] = 0.0;
}

- (void) drawAR
{
  Matrix44 projectionMatrix;
  [self buildProjectionMatrix:m_calibration.getIntrinsic():m_frameSize.width
  :m_frameSize.height :projectionMatrix];

  glMatrixMode(GL_PROJECTION);
  glLoadMatrixf(projectionMatrix.data);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  glEnableClientState(GL_VERTEX_ARRAY);
  glEnableClientState(GL_NORMAL_ARRAY);

  glPushMatrix();
  glLineWidth(3.0f);

  float lineX[] = {0,0,0,1,0,0};
  float lineY[] = {0,0,0,0,1,0};
  float lineZ[] = {0,0,0,0,0,1};

  const GLfloat squareVertices[] = {
    -0.5f, -0.5f,
    0.5f,  -0.5f,
    -0.5f,  0.5f,
    0.5f,   0.5f,
  };
  const GLubyte squareColors[] = {
    255, 255,   0, 255,
    0,   255, 255, 255,
    0,     0,   0,   0,
    255,   0, 255, 255,
  };

  for (size_t transformationIndex=0; 
  transformationIndex<m_transformations.size(); transformationIndex++)
  {
    const Transformation& transformation = 
    m_transformations[transformationIndex];

    Matrix44 glMatrix = transformation.getInverted().getMat44();

    glLoadMatrixf(reinterpret_cast<const GLfloat*>(&glMatrix.data[0]));

    // draw data
    glVertexPointer(2, GL_FLOAT, 0, squareVertices);
    glEnableClientState(GL_VERTEX_ARRAY);
    glColorPointer(4, GL_UNSIGNED_BYTE, 0, squareColors);
    glEnableClientState(GL_COLOR_ARRAY);

    glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
    glDisableClientState(GL_COLOR_ARRAY);

    float scale = 0.5;
    glScalef(scale, scale, scale);

    glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
    glVertexPointer(3, GL_FLOAT, 0, lineX);
    glDrawArrays(GL_LINES, 0, 2);

    glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
    glVertexPointer(3, GL_FLOAT, 0, lineY);
    glDrawArrays(GL_LINES, 0, 2);

    glColor4f(0.0f, 0.0f, 1.0f, 1.0f);
    glVertexPointer(3, GL_FLOAT, 0, lineZ);
    glDrawArrays(GL_LINES, 0, 2);
  }
  glPopMatrix();
  glDisableClientState(GL_VERTEX_ARRAY);
}